diff --git a/panda/src/parametrics/curve.cxx b/panda/src/parametrics/curve.cxx index 2b7cec7e67..b30afc6bc3 100644 --- a/panda/src/parametrics/curve.cxx +++ b/panda/src/parametrics/curve.cxx @@ -18,6 +18,7 @@ #include "curve.h" #include "config_parametrics.h" +#include "hermiteCurve.h" #include "nurbsCurve.h" #include "curveDrawer.h" @@ -204,7 +205,7 @@ calc_length(double from, double to) const { //////////////////////////////////////////////////////////////////// double ParametricCurve:: compute_t(double start_t, double length_offset, double guess, - double threshold) const { + double threshold) const { if (length_offset > 0.0) { // If the length_offset is positive, we are looking forward. // Enforce that the guess is greater than the start. @@ -245,7 +246,7 @@ compute_t(double start_t, double length_offset, double guess, // Clamp it to the end of the curve. if (guess > max_t) { if (clamped) { - return max_t; + return max_t; } clamped = true; guess = max_t; @@ -270,7 +271,6 @@ compute_t(double start_t, double length_offset, double guess, //////////////////////////////////////////////////////////////////// bool ParametricCurve:: convert_to_hermite(HermiteCurve &hc) const { -#if 0 //[////todo:skyler BezierSegs bz_segs; if (!GetBezierSegs(bz_segs)) { return false; @@ -293,9 +293,9 @@ convert_to_hermite(HermiteCurve &hc) const { scale_in = scale_out; scale_out = bz_segs[i+1]._t - bz_segs[i]._t; - if (!bz_segs[i]._v[3].almostEqual(bz_segs[i+1]._v[0], 0.0001)) { - // Oops, we have a cut. - hc.set_cv_type(n, HC_CUT); + if (!bz_segs[i]._v[3].almost_equal(bz_segs[i+1]._v[0], 0.0001)) { + // Oops, we have a cut. + hc.set_cv_type(n, HC_CUT); } n = hc.append_cv(HC_FREE, bz_segs[i+1]._v[0]); @@ -317,21 +317,20 @@ convert_to_hermite(HermiteCurve &hc) const { int num_cvs = hc.get_num_cvs(); for (n = 1; n < num_cvs-1; n++) { if (hc.get_cv_type(n)!=HC_CUT) { - pfVec3 in = hc.get_cv_in(n); - pfVec3 out = hc.get_cv_out(n); + LVector3f in = hc.get_cv_in(n); + LVector3f out = hc.get_cv_out(n); - if (in.almostEqual(out, 0.0001)) { - hc.set_cv_type(n, HC_SMOOTH); + if (in.almost_equal(out, 0.0001)) { + hc.set_cv_type(n, HC_SMOOTH); } else { - in.normalize(); - out.normalize(); - if (in.almostEqual(out, 0.0001)) { - hc.set_cv_type(n, HC_G1); - } + in.normalize(); + out.normalize(); + if (in.almost_equal(out, 0.0001)) { + hc.set_cv_type(n, HC_G1); + } } } } -#endif //] return true; } @@ -359,8 +358,8 @@ convert_to_nurbs(NurbsCurve &nc) const { nc.append_cv(bz_segs[i]._v[1]); nc.append_cv(bz_segs[i]._v[2]); if (i == bz_segs.size()-1 || - !bz_segs[i]._v[3].almost_equal(bz_segs[i+1]._v[0], 0.0001)) { - nc.append_cv(bz_segs[i]._v[3]); + !bz_segs[i]._v[3].almost_equal(bz_segs[i+1]._v[0], 0.0001)) { + nc.append_cv(bz_segs[i]._v[3]); } } @@ -379,9 +378,9 @@ convert_to_nurbs(NurbsCurve &nc) const { nc.set_knot(ki+2, t); ki += 3; if (i == bz_segs.size()-1 || - !bz_segs[i]._v[3].almost_equal(bz_segs[i+1]._v[0], 0.0001)) { - nc.set_knot(ki, t); - ki++; + !bz_segs[i]._v[3].almost_equal(bz_segs[i+1]._v[0], 0.0001)) { + nc.set_knot(ki, t); + ki++; } } } @@ -443,8 +442,8 @@ ascii_draw() const { r = (p[2] - minz) * zscale; if (r>=0 && r=0 && crecompute(max(t1, 0.0), min(t2, get_max_t()), this); } } @@ -560,7 +559,7 @@ invalidate_all() { //////////////////////////////////////////////////////////////////// float ParametricCurve:: r_calc_length(double t1, double t2, const LVector3f &p1, const LVector3f &p2, - float seglength) const { + float seglength) const { static const float length_tolerance = 0.0000001; static const double t_tolerance = 0.000001; @@ -701,7 +700,7 @@ get_2ndtangent(double t, LVector3f &tangent2) const { //////////////////////////////////////////////////////////////////// bool PiecewiseCurve:: adjust_point(double t, - float px, float py, float pz) { + float px, float py, float pz) { const ParametricCurve *curve; bool result = find_curve(curve, t); @@ -711,9 +710,9 @@ adjust_point(double t, } rebuild_curveseg(RT_CV | RT_KEEP_ORIG, 0.0, LVector4f(), - RT_POINT, t, LVector4f(px, py, pz, 1.0), - RT_TANGENT | RT_KEEP_ORIG, t, LVector4f(), - RT_CV | RT_KEEP_ORIG, 0.0, LVector4f()); + RT_POINT, t, LVector4f(px, py, pz, 1.0), + RT_TANGENT | RT_KEEP_ORIG, t, LVector4f(), + RT_CV | RT_KEEP_ORIG, 0.0, LVector4f()); return true; } @@ -726,7 +725,7 @@ adjust_point(double t, //////////////////////////////////////////////////////////////////// bool PiecewiseCurve:: adjust_tangent(double t, - float tx, float ty, float tz) { + float tx, float ty, float tz) { const ParametricCurve *curve; bool result = find_curve(curve, t); @@ -736,9 +735,9 @@ adjust_tangent(double t, } rebuild_curveseg(RT_CV | RT_KEEP_ORIG, 0.0, LVector4f(), - RT_POINT | RT_KEEP_ORIG, t, LVector4f(), - RT_TANGENT, t, LVector4f(tx, ty, tz, 0.0), - RT_CV | RT_KEEP_ORIG, 0.0, LVector4f()); + RT_POINT | RT_KEEP_ORIG, t, LVector4f(), + RT_TANGENT, t, LVector4f(tx, ty, tz, 0.0), + RT_CV | RT_KEEP_ORIG, 0.0, LVector4f()); return true; } @@ -750,8 +749,8 @@ adjust_tangent(double t, //////////////////////////////////////////////////////////////////// bool PiecewiseCurve:: adjust_pt(double t, - float px, float py, float pz, - float tx, float ty, float tz) { + float px, float py, float pz, + float tx, float ty, float tz) { const ParametricCurve *curve; bool result = find_curve(curve, t); @@ -761,9 +760,9 @@ adjust_pt(double t, } rebuild_curveseg(RT_CV | RT_KEEP_ORIG, 0.0, LVector4f(), - RT_POINT, t, LVector4f(px, py, pz, 1.0), - RT_TANGENT, t, LVector4f(tx, ty, tz, 0.0), - RT_CV | RT_KEEP_ORIG, 0.0, LVector4f()); + RT_POINT, t, LVector4f(px, py, pz, 1.0), + RT_TANGENT, t, LVector4f(tx, ty, tz, 0.0), + RT_CV | RT_KEEP_ORIG, 0.0, LVector4f()); return true; } @@ -832,11 +831,11 @@ insert_curveseg(int ti, ParametricCurve *seg, double tlength) { } else if (ti==0) { _segs.insert(_segs.begin(), - Curveseg(seg, tlength)); + Curveseg(seg, tlength)); } else { _segs.insert(_segs.begin() + ti, - Curveseg(seg, _segs[ti-1]._tend + tlength)); + Curveseg(seg, _segs[ti-1]._tend + tlength)); } return true; @@ -954,15 +953,15 @@ set_tlength(int ti, double tlength) { //////////////////////////////////////////////////////////////////// void PiecewiseCurve:: make_nurbs(int order, int num_cvs, - const double knots[], const LVector4f cvs[]) { + const double knots[], const LVector4f cvs[]) { remove_all_curvesegs(); for (int i=0; i knots[i+order-1]) { int ti = get_num_segs(); bool result = - insert_curveseg(ti, new CubicCurveseg(order, knots+i, cvs+i), - knots[i+order] - knots[i+order-1]); + insert_curveseg(ti, new CubicCurveseg(order, knots+i, cvs+i), + knots[i+order] - knots[i+order-1]); assert(result); } } @@ -1005,9 +1004,9 @@ GetBezierSegs(BezierSegs &bz_segs) const { //////////////////////////////////////////////////////////////////// bool PiecewiseCurve:: rebuild_curveseg(int, double, const LVector4f &, - int, double, const LVector4f &, - int, double, const LVector4f &, - int, double, const LVector4f &) { + int, double, const LVector4f &, + int, double, const LVector4f &, + int, double, const LVector4f &) { cerr << "rebuild_curveseg not implemented for this curve type.\n"; return false; } @@ -1090,13 +1089,13 @@ find_curve(const ParametricCurve *&curve, double &t) const { if (ti >= _segs.size()) { if (_segs.empty()) { - curve = NULL; - t = 0.0; - return false; + curve = NULL; + t = 0.0; + return false; } else { - curve = _segs.back()._curve; - t = 1.0; - return false; + curve = _segs.back()._curve; + t = 1.0; + return false; } } else if (!_segs[ti]._curve->is_valid()) { curve = _segs[ti]._curve; @@ -1244,6 +1243,36 @@ get_2ndtangent(double t, LVector3f &tangent2) const { } +//////////////////////////////////////////////////////////////////// +// Function: CubicCurveseg::hermite_basis +// Access: Public +// Description: Defines the curve segment as a Hermite. This only +// sets up the basis vectors, so the curve will be +// computed correctly; it does not retain the CV's. +//////////////////////////////////////////////////////////////////// +void CubicCurveseg:: +hermite_basis(const HermiteCurveCV &cv0, + const HermiteCurveCV &cv1, + double tlength) { + static LMatrix4f + Mh(2, -3, 0, 1, + -2, 3, 0, 0, + 1, -2, 1, 0, + 1, -1, 0, 0); + + LVector4f Gx(cv0._p[0], cv1._p[0], + cv0._out[0]*tlength, cv1._in[0]*tlength); + LVector4f Gy(cv0._p[1], cv1._p[1], + cv0._out[1]*tlength, cv1._in[1]*tlength); + LVector4f Gz(cv0._p[2], cv1._p[2], + cv0._out[2]*tlength, cv1._in[2]*tlength); + + Bx = Gx * Mh; + By = Gy * Mh; + Bz = Gz * Mh; + rational = false; +} + //////////////////////////////////////////////////////////////////// // Function: CubicCurveseg::bezier_basis // Access: Public @@ -1271,7 +1300,7 @@ bezier_basis(const BezierSeg &seg) { static LVector4f nurbs_blending_function(int order, int i, int j, - const double knots[]) { + const double knots[]) { // This is doubly recursive. Ick. LVector4f r; @@ -1292,9 +1321,9 @@ nurbs_blending_function(int order, int i, int j, // First term. Division by zero is defined to equal zero. if (d0 != 0.0) { if (d1 != 0.0) { - r = bi0 / d0 - bi1 / d1; + r = bi0 / d0 - bi1 / d1; } else { - r = bi0 / d0; + r = bi0 / d0; } } else if (d1 != 0.0) { @@ -1313,9 +1342,9 @@ nurbs_blending_function(int order, int i, int j, // Second term. if (d0 != 0.0) { if (d1 != 0.0) { - r += bi0 * (- knots[i] / d0) + bi1 * (knots[i+j] / d1); + r += bi0 * (- knots[i] / d0) + bi1 * (knots[i+j] / d1); } else { - r += bi0 * (- knots[i] / d0); + r += bi0 * (- knots[i] / d0); } } else if (d1 != 0.0) { @@ -1328,8 +1357,8 @@ nurbs_blending_function(int order, int i, int j, void compute_nurbs_basis(int order, - const double knots_in[], - LMatrix4f &basis) { + const double knots_in[], + LMatrix4f &basis) { int i; // Scale the supplied knots to the range 0..1. @@ -1408,9 +1437,9 @@ bool CubicCurveseg:: GetBezierSeg(BezierSeg &seg) const { static LMatrix4f Mbi(0.0, 0.0, 0.0, 1.0, - 0.0, 0.0, 1.0/3.0, 1.0, - 0.0, 1.0/3.0, 2.0/3.0, 1.0, - 1.0, 1.0, 1.0, 1.0); + 0.0, 0.0, 1.0/3.0, 1.0, + 0.0, 1.0/3.0, 2.0/3.0, 1.0, + 1.0, 1.0, 1.0, 1.0); LVector4f Gx = Bx * Mbi; LVector4f Gy = By * Mbi; @@ -1437,9 +1466,9 @@ GetBezierSeg(BezierSeg &seg) const { inline LVector4f operator * (const LMatrix4f &M, const LVector4f &v) { return LVector4f(M(0,0)*v[0] + M(0,1)*v[1] + M(0,2)*v[2] + M(0,3)*v[3], - M(1,0)*v[0] + M(1,1)*v[1] + M(1,2)*v[2] + M(1,3)*v[3], - M(2,0)*v[0] + M(2,1)*v[1] + M(2,2)*v[2] + M(2,3)*v[3], - M(3,0)*v[0] + M(3,1)*v[1] + M(3,2)*v[2] + M(3,3)*v[3]); + M(1,0)*v[0] + M(1,1)*v[1] + M(1,2)*v[2] + M(1,3)*v[3], + M(2,0)*v[0] + M(2,1)*v[1] + M(2,2)*v[2] + M(2,3)*v[3], + M(3,0)*v[0] + M(3,1)*v[1] + M(3,2)*v[2] + M(3,3)*v[3]); } //////////////////////////////////////////////////////////////////// @@ -1450,12 +1479,12 @@ operator * (const LMatrix4f &M, const LVector4f &v) { //////////////////////////////////////////////////////////////////// static bool compute_seg_col(int c, - int rtype, double t, const LVector4f &v, - const LMatrix4f &B, - const LMatrix4f &Bi, - const LMatrix4f &G, - const LMatrix4f &GB, - LMatrix4f &T, LMatrix4f &P) { + int rtype, double t, const LVector4f &v, + const LMatrix4f &B, + const LMatrix4f &Bi, + const LMatrix4f &G, + const LMatrix4f &GB, + LMatrix4f &T, LMatrix4f &P) { int keep_orig = (rtype & RT_KEEP_ORIG); switch (rtype & RT_BASE_TYPE) { @@ -1556,12 +1585,12 @@ compute_seg_col(int c, //////////////////////////////////////////////////////////////////// bool CubicCurveseg:: compute_seg(int rtype0, double t0, const LVector4f &v0, - int rtype1, double t1, const LVector4f &v1, - int rtype2, double t2, const LVector4f &v2, - int rtype3, double t3, const LVector4f &v3, - const LMatrix4f &B, - const LMatrix4f &Bi, - LMatrix4f &G) { + int rtype1, double t1, const LVector4f &v1, + int rtype2, double t2, const LVector4f &v2, + int rtype3, double t3, const LVector4f &v3, + const LMatrix4f &B, + const LMatrix4f &Bi, + LMatrix4f &G) { // We can define a cubic curve segment given four arbitrary // properties of the segment: any point along the curve, any tangent @@ -1590,9 +1619,9 @@ compute_seg(int rtype0, double t0, const LVector4f &v0, } if (! (compute_seg_col(0, rtype0, t0, v0, B, Bi, G, GB, T, P) && - compute_seg_col(1, rtype1, t1, v1, B, Bi, G, GB, T, P) && - compute_seg_col(2, rtype2, t2, v2, B, Bi, G, GB, T, P) && - compute_seg_col(3, rtype3, t3, v3, B, Bi, G, GB, T, P))) { + compute_seg_col(1, rtype1, t1, v1, B, Bi, G, GB, T, P) && + compute_seg_col(2, rtype2, t2, v2, B, Bi, G, GB, T, P) && + compute_seg_col(3, rtype3, t3, v3, B, Bi, G, GB, T, P))) { return false; }