diff --git a/kazmathxx/boundvec2.h b/kazmathxx/boundvec2.h index 9c01aa9..7f4cb0d 100644 --- a/kazmathxx/boundvec2.h +++ b/kazmathxx/boundvec2.h @@ -117,6 +117,25 @@ namespace km kmScalar to; return segmentIntersectsSegment( other, tt, to, outPoint ); } + + inline kmScalar minimumDistance2ToPoint(const vec2 &pt){ + boundvec2 perpendicular; + perpendicular.pos = pt; + perpendicular.vec = vec2(vec.y, -vec.x); + + kmScalar t, otherT; + vec2 thisPt; + intersects(perpendicular, t , otherT, thisPt); + if(t<0.0){ + thisPt = pt - pos; + }else if(t > 1.0){ + thisPt = pt - (pos + vec); + }else{ + thisPt -= pt; + } + return thisPt.lengthSq(); + } + }; } //end of namespace km diff --git a/kazmathxx/tcubic.h b/kazmathxx/tcubic.h new file mode 100644 index 0000000..f75d1e5 --- /dev/null +++ b/kazmathxx/tcubic.h @@ -0,0 +1,60 @@ +#ifndef _KAZMATH_TCUBIC_H +#define _KAZMATH_TCUBIC_H + +#include "../kazmath/utility.h" +#include "tquadratic.h" + +namespace km +{ + +/// f(t) = a*t^3 + b*t^2 + c*t + d +class tcubic +{ +public: + kmScalar a; // term for t^0 + kmScalar b; // term for t^1 + kmScalar c; // term for t^2 + kmScalar d; // term for t^3 + + tcubic(kmScalar aval = 0.0, + kmScalar bval = 0.0, + kmScalar cval = 0.0, + kmScalar dval = 0.0 ) : a(aval), b(bval), c(cval){}; + + void set(kmScalar aval = 0.0, + kmScalar bval = 0.0, + kmScalar cval = 0.0, + kmScalar dval = 0.0 ) + { + a = aval; + b = bval; + c = cval; + d = dval; + }; + + kmScalar value(const kmScalar t){ + kmScalar t2 = t * t; + kmScalar t3 = t * t * t; + return (a * t3 + b*t2 + c*t + d); + } + + void derive(tquadratic &outQuadratic){ + outQuadratic.set(3*a, 2*b, c); + } + + void sample(int nSamples, + const kmScalar begin, + const kmScalar step, + kmScalar *output) + { + kmScalar t = begin; + for(int i=0; i < nSamples; ++i){ + output[i] = value(t); + t += step; + } + } +}; + +} + +#endif // _KAZMATH_TCUBIC_H diff --git a/kazmathxx/tcurve2.h b/kazmathxx/tcurve2.h new file mode 100644 index 0000000..163ec6c --- /dev/null +++ b/kazmathxx/tcurve2.h @@ -0,0 +1,41 @@ +#ifndef _KAZMATH_TCURVE2_H +#define _KAZMATH_TCURVE2_H + +#include "tfunction.h" + +namespace km +{ + +class tcurve2 +{ + +public: + virtual ~tcurve2(){}; + virtual vec2 value(const kmScalar t) = 0; + virtual void sample(unsigned int nSamples, + const kmScalar tbegin, + const kmScalar tstep, + vec2 *output) + { + for(unsigned int i=0; i < nSamples; ++i){ + output[i] = value(tbegin + tstep*i); + } + } +}; + +class tcurve2functions : public tcurve2 +{ +public: + tfunction *xfunc; + tfunction *yfunc; + + tcurve2functions(tfunction *xf, + tfunction *yf) : xfunc(xf), yfunc(yf){}; + + virtual vec2 value(const kmScalar t){ + return vec2(xfunc->value(t), yfunc->value(t)); + } +}; + +} +#endif // _KAZMATH_TCURVE2_H \ No newline at end of file diff --git a/kazmathxx/tfunction.h b/kazmathxx/tfunction.h new file mode 100644 index 0000000..7ed0521 --- /dev/null +++ b/kazmathxx/tfunction.h @@ -0,0 +1,19 @@ +#ifndef _KAZMATHXX_TFUNCTION_H +#define _KAZMATHXX_TFUNCTION_H + +#include "../kazmath/utility.h" +#include "vec2.h" + +namespace km +{ + +class tfunction +{ +public: + virtual ~tfunction(); + virtual kmScalar value(const kmScalar t) = 0; +}; + +} + +#endif // _KAZMATHXX_TFUNCTION_H \ No newline at end of file diff --git a/kazmathxx/tline.h b/kazmathxx/tline.h new file mode 100644 index 0000000..9fce551 --- /dev/null +++ b/kazmathxx/tline.h @@ -0,0 +1,64 @@ +#ifndef _KAZMATH_TRECT_H +#define _KAZMATH_TRECT_H + +#include "../kazmath/utility.h" +#include "boundvec2.h" + +namespace km +{ + +/// f(t) = m * t + c +class tline +{ +public: + kmScalar m; + kmScalar c; + + tline(kmScalar mVal = 0.0, + kmScalar cVal = 0.0) : m(mVal), c(cVal) {}; + + void set(const kmScalar mval, + const kmScalar cval) + { + m = mval; + c = cval; + } + + void fromBoundVec(const boundvec2 &bvec) + { + m = bvec.vec.y / bvec.vec.x; + c = bvec.pos.y / (bvec.pos.x * m); + } + + kmScalar value(const kmScalar t){ + return (t*m + c); + } + + bool solve(const kmScalar desiredValue, + kmScalar &outT){ + if(m != 0){ + outT = (desiredValue - c)/m; + return true; + }else if( desiredValue + kmEpsilon > c && + desiredValue - kmEpsilon < c ){ + outT = 0; // but there are infinite + // values that give the desiredValue + return true; + }else{ + return false; + } + } + + void sample(unsigned int nSamples, + const kmScalar tbegin, + const kmScalar tstep, + kmScalar *output){ + for(unsigned int i = 0; i < nSamples; ++i){ + output[i] = value(tbegin + tstep * i); + } + } +}; + +} + +#endif // _KAZMATH_TRECT_H \ No newline at end of file diff --git a/kazmathxx/tparabola.h b/kazmathxx/tparabola.h new file mode 100644 index 0000000..663b525 --- /dev/null +++ b/kazmathxx/tparabola.h @@ -0,0 +1,119 @@ +#ifndef _KAZMATH_TPARABOLA_H +#define _KAZMATH_TPARABOLA_H + +#include "tquadratic.h" +#include "tline.h" +#include "vec2.h" +#include "tcurve2.h" + +namespace km +{ + +class tparabola +{ +public: + tline xfunction; + tquadratic yfunction; + + tparabola(kmScalar xM, + kmScalar xC, + kmScalar yA, + kmScalar yB, + kmScalar yC) : xfunction(xM, xC), + yfunction(yA, yB, yC){}; + + // return the number of points of intersection + int intersectsSegment(const boundvec2 &segment, + kmScalar &segmentT0, kmScalar ¶bolaT0, vec2 &outPt0, + kmScalar &segmentT1, kmScalar ¶bolaT1, vec2 &outPt1) + { + kmScalar pt0, pt1, rt0, rt1; + int res = intersectline(segment, pt0, pt1, rt0, rt1); + if(res > 0){ + int notinsegment = 0; + if(rt0 < 0 || rt0 > 1.0){ + notinsegment++; + }else{ + segmentT0 = rt0; + parabolaT0 = pt0; + outPt0 = segment.pos + rt0 * segment.vec; + } + + if(res > 1){ + if(rt1 < 0.0 || rt1 > 1.0){ + notinsegment++; + }else{ + if(notinsegment>0){ + segmentT0 = rt1; + parabolaT0 = pt1; + outPt0 = segment.pos + rt1 * segment.vec; + }else{ + segmentT1 = rt1; + parabolaT1 = pt1; + outPt1 = segment.pos + rt1 * segment.vec; + } + } + } + res -= notinsegment; + } + return res; + } + + int intersectline(const boundvec2 &bvec, + kmScalar &outTParabola0, + kmScalar &outTParabola1, + kmScalar &outtline0, + kmScalar &outtline1) + { + kmScalar tmpA = yfunction.a * bvec.vec.x; + kmScalar tmpB = yfunction.b * bvec.vec.x - xfunction.m * bvec.vec.y; + kmScalar tmpC = bvec.vec.x * (yfunction.c - bvec.pos.y) - + bvec.vec.y * (xfunction.c - bvec.pos.x); + + tquadratic tmp(tmpA, tmpB, tmpC); + int res = tmp.solve(0.0f, outTParabola0, outTParabola1); + if(res > 0){ + outtline0 = (xfunction.m * outTParabola0 + xfunction.c - bvec.pos.x) / bvec.vec.x; + if(res > 1){ + outtline1 = (xfunction.m * outTParabola1 + xfunction.c - bvec.pos.x) / bvec.vec.x; + } + } + return res; + } + + vec2 value(const kmScalar t ) + { + return vec2(xfunction.value(t), yfunction.value(t)); + } + + void sample(unsigned int nSamples, + const kmScalar tbegin, + const kmScalar tstep, + vec2 *output) + { + for(unsigned int i = 0; i < nSamples; ++i){ + output[i] = value(tbegin + tstep * i); + } + } +}; + +// wrapper for the tparabola that implements the +// tcurve2 interface +class tparabolacurve2 : tcurve2 +{ +public: + tparabola innerParabola; + + tparabolacurve2(kmScalar xM, + kmScalar xC, + kmScalar yA, + kmScalar yB, + kmScalar yC) : innerParabola(xM, xC, yA, yB, yC){}; + virtual vec2 value(const kmScalar t){ + return innerParabola.value(t); + } +}; + +} + +#endif // _KAZMATH_TPARABOLA_H diff --git a/kazmathxx/tquadratic.h b/kazmathxx/tquadratic.h new file mode 100644 index 0000000..f3f1819 --- /dev/null +++ b/kazmathxx/tquadratic.h @@ -0,0 +1,87 @@ +#ifndef _KAZMATH_TQUADRATIC_H +#define _KAZMATH_TQUADRATIC_H + +#include "../kazmath/utility.h" +#include "tline.h" + +namespace km +{ + +/// f(t) = a*t^2 + b*t + c +class tquadratic +{ +public: + kmScalar a; // term for t^2 + kmScalar b; // term for t^1 + kmScalar c; // term for t^0 + + tquadratic(kmScalar aval = 0.0, + kmScalar bval = 0.0, + kmScalar cval = 0.0 ) : a(aval), b(bval), c(cval) {}; + + void set(const kmScalar aval, + const kmScalar bval, + const kmScalar cval) + { + a = aval; + b = bval; + c = cval; + } + + kmScalar value(const kmScalar t){ + kmScalar t2 = t * t; + return (a*t2 + b*t + c); + } + + // return the number of solutions : 0, 1 or 2 + int solve(const kmScalar desiredValue, + kmScalar &solutionA, + kmScalar &solutionB){ + kmScalar dc = c - desiredValue; + if(a != 0){ + kmScalar delta = b*b - 4 * a * dc; + if(delta < .0f){ + return 0; + }else if(delta == .0f){ + solutionA = (-b * 0.5f) / a; + return 1; + }else{ + kmScalar sd = sqrt(delta); + solutionA = 0.5f * (-b - sd) / a; + solutionB = 0.5f * (-b + sd) / a; + return 2; + } + }else{ + if(b == 0){ + if(desiredValue == c){ + solutionA = desiredValue; + return 1; + }else{ + return 0; + } + }else{ + solutionA = -dc / b; + return 1; + } + } + } + + void derive(tline &outLine){ + outLine.set(2*a, b); + } + + void sample(int nSamples, + const kmScalar begin, + const kmScalar step, + kmScalar *output){ + kmScalar t = begin; + for(int i=0; i < nSamples; ++i){ + output[i] = value(t); + t += step; + } + } +}; + +} + +#endif // _KAZMATH_TQUADRATIC_H \ No newline at end of file