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