public Vector3d GetPointAt(int i, double t)
{
Vector3d u = (points[i] - points[i + 1]) * 2 +
B[i] + B[i + 1];
Vector3d v = (points[i + 1] - points[i]) * 3 -
(2 * B[i] + B[i + 1]);
Vector3d w = B[i];
Vector3d r = points[i];
double t0 = 1, t1 = t, t2 = t * t, t3 = t * t * t;
Vector4d g = new Vector4d(t3, t2, t1, t0);
Vector4d p = new Vector4d(u.x, v.x, w.x, r.x);
Vector4d q = new Vector4d(u.y, v.y, w.y, r.y);
Vector4d o = new Vector4d(u.z, v.z, w.z, r.z);
double x = g.Dot(p);
double y = g.Dot(q);
double z = g.Dot(o);
return(new Vector3d(x, y, z));
}
public Vector3d[] GetPoints2(int numPerInterval)
{
List <Vector3d> curvePoints = new List <Vector3d>();
int num = numOfPoints + numPerInterval * (numOfPoints - 1);
double lamb = 1.0 / (numPerInterval + 1);
for (int i = 0; i < numOfPoints - 1; ++i)
{
Vector3d u = (points[i] - points[i + 1]) * 2 +
B[i] + B[i + 1];
Vector3d v = (points[i + 1] - points[i]) * 3 -
(2 * B[i] + B[i + 1]);
Vector3d w = B[i];
Vector3d r = points[i];
Vector4d p = new Vector4d(u.x, v.x, w.x, r.x);
Vector4d q = new Vector4d(u.y, v.y, w.y, r.y);
Vector4d o = new Vector4d(u.z, v.z, w.z, r.z);
for (int j = 0; j <= numPerInterval; ++j) // start point + inner points
{
double t = j * lamb;
double t0 = 1, t1 = t, t2 = t * t, t3 = t * t * t;
Vector4d g = new Vector4d(t3, t2, t1, t0);
double x = g.Dot(p);
double y = g.Dot(q);
double z = g.Dot(o);
curvePoints.Add(
new Vector3d(x, y, z)
);
}
}
Vector3d pt = points[numOfPoints - 1];
curvePoints.Add( // last point
new Vector3d(pt.x, pt.y, pt.z)
);
return(curvePoints.ToArray());
}
public static Matrix3d QuatToMatrix3d(Vector4d q)
{
double n = q.Dot(q);
double s = (n > 0.0) ? (2.0 / n) : 0.0f;
double xs, ys, zs;
double wx, wy, wz;
double xx, xy, xz;
double yy, yz, zz;
xs = q.x * s; ys = q.y * s; zs = q.z * s;
wx = q.w * xs; wy = q.w * ys; wz = q.w * zs;
xx = q.x * xs; xy = q.x * ys; xz = q.x * zs;
yy = q.y * ys; yz = q.y * zs; zz = q.z * zs;
Matrix3d m = new Matrix3d();
m[0,0] = 1.0 - (yy + zz); m[1,0] = xy - wz; m[2,0] = xz + wy;
m[0,1] = xy + wz; m[1,1] = 1.0 - (xx + zz); m[2,1] = yz - wx;
m[0,2] = xz - wy; m[1,2] = yz + wx; m[2,2] = 1.0 - (xx + yy);
return m;
}
private Matrix3d QuatToMatrix3d(Vector4d q)
{
double n = q.Dot(q);
double s = (n > 0.0) ? (2.0 / n) : 0.0f;
double xs, ys, zs;
double wx, wy, wz;
double xx, xy, xz;
double yy, yz, zz;
xs = q.x * s; ys = q.y * s; zs = q.z * s;
wx = q.w * xs; wy = q.w * ys; wz = q.w * zs;
xx = q.x * xs; xy = q.x * ys; xz = q.x * zs;
yy = q.y * ys; yz = q.y * zs; zz = q.z * zs;
Matrix3d m = new Matrix3d();
m[0, 0] = 1.0 - (yy + zz); m[1, 0] = xy - wz; m[2, 0] = xz + wy;
m[0, 1] = xy + wz; m[1, 1] = 1.0 - (xx + zz); m[2, 1] = yz - wx;
m[0, 2] = xz - wy; m[1, 2] = yz + wx; m[2, 2] = 1.0 - (xx + yy);
return(m);
}
