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;
}
