Eigen::Matrix<T,4,1> blend(T t, Eigen::Matrix<T,4,1> p00, Eigen::Matrix<T,4,1> pNorm, Eigen::Matrix<T,4,1> p10){ T r0 = p00.mag(); T r1 = p10.mag(); T rs = linear(t,r0,r1); T dotp = dot(p00,p10); dotp = abs(dotp); T theta = acos(dotp/r0/r1); Eigen::Quaternion<T> dq0; p00.normalize(); p10.normalize(); Eigen::Quaternion<T> q00(p00); Eigen::Quaternion<T> q10(p10); if (theta < 0.01) { //just give it a little nudge in the right direction T o = 0.10; Eigen::Quaternion<T> dq(cos(o/2.), sin(o/2.)*pNorm[0], sin(o/2.)*pNorm[1], sin(o/2.)*pNorm[2]); Eigen::Matrix<T,4,1> p01 = dq.rotate(p00); Eigen::Quaternion<T> q01(p01); dq0 = Eigen::Quaternion<T>::slerp(q01,q10,t); } else { dq0 = Eigen::Quaternion<T>::slerp(q00,q10,t); } T dott = p00.dot(p10); T dotq0 = q00.dot(q10); T dotq1 = dq0.dot(q10); return Eigen::Matrix<T,4,1>(rs*dq0[1],rs*dq0[2],rs*dq0[3]); //return Eigen::Matrix<T,4,1>(p01); }
dFloat64 dBezierSpline::CalculateLength (dFloat64 tol) const { dBigVector stackPool[32][3]; int stack = 0; dFloat64 length = 0.0f; dFloat64 tol2 = tol * tol; dFloat64 u0 = m_knotVector[m_degree]; dBigVector p0 (CurvePoint (u0)); for (int i = m_degree; i < (m_knotsCount - m_degree - 1); i ++) { dFloat64 u1 = m_knotVector[i + 1]; dBigVector p1 (CurvePoint (u1)); stackPool[stack][0] = p0; stackPool[stack][1] = p1; stackPool[stack][2] = dBigVector (u0, u1, dFloat64(0.0f), dFloat64(0.0f)); stack ++; while (stack) { stack --; dBigVector q0 (stackPool[stack][0]); dBigVector q1 (stackPool[stack][1]); dFloat64 t0 = stackPool[stack][2][0]; dFloat64 t1 = stackPool[stack][2][1]; dFloat64 t01 = (t1 + t0) * 0.5f; dBigVector p01 ((q1 + q0).Scale (0.5f)); dBigVector q01 (CurvePoint (t01)); dBigVector err (q01 - p01); dFloat64 err2 = err.DotProduct3(err); if (err2 < tol2) { dBigVector step (q1 - q0); length += dSqrt (step.DotProduct3(step)); } else { stackPool[stack][0] = q01; stackPool[stack][1] = q1; stackPool[stack][2] = dBigVector (t01, t1, dFloat64(0.0f), dFloat64(0.0f)); stack ++; stackPool[stack][0] = q0; stackPool[stack][1] = q01; stackPool[stack][2] = dBigVector (t0, t01, dFloat64(0.0f), dFloat64(0.0f)); stack ++; } } u0 = u1; p0 = p1; } return length; }
vecN<vecN<vecN<OutputType, 2>, 4>, 2> split_cubicT(c_array<const vecN<InputType, 2> > pts) { FASTUIDRAWassert(pts.size() == 4); vecN<vecN<vecN<OutputType, 2>, 4>, 2> return_value; vecN<IntermediateType, 2> p0(pts[0]), p1(pts[1]), p2(pts[2]), p3(pts[3]); vecN<IntermediateType, 2> p01, p23, pA, pB, pC; const IntermediateType two(2), three(3), four(4), eight(8); p01 = (p0 + p1) / two; p23 = (p2 + p3) / two; pA = (p0 + two * p1 + p2) / four; pB = (p1 + two * p2 + p3) / four; pC = (p0 + three * p1 + three * p2 + p3) / eight; vecN<OutputType, 2> q0(pts[0]), q01(p01), qA(pA), qC(pC); vecN<OutputType, 2> qB(pB), q23(p23), q3(pts[3]); return_value[0] = vecN<vecN<OutputType, 2>, 4>(q0, q01, qA, qC); return_value[1] = vecN<vecN<OutputType, 2>, 4>(qC, qB, q23, q3); return return_value; }