Add basic curves

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

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

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

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

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

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 &parabolaT0, vec2 &outPt0,
+                          kmScalar &segmentT1, kmScalar &parabolaT1, 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