// compute the releft point of p according to the Canvas3 (Canvas3_normal, Canvas3_center) public static MyVector3 Reflect(MyVector3 p, MyVector3 Canvas3_normal, MyVector3 Canvas3_center) { MyVector3 u = p - Canvas3_center; // create a coord system (x,y,z), project to that sys, reflect and project back MyVector3 x = Canvas3_normal; MyVector3 y; if (x.x == 0 && x.y == 0) { y = new MyVector3(0, -x.z, x.y); } else { y = new MyVector3(x.y, -x.x, 0); } MyVector3 z = x.Cross(y); MyMatrix3d R = new MyMatrix3d(x, y, z); MyMatrix3d InvR = R.InverseSVD(); MyMatrix4d U = new MyMatrix4d(R); MyMatrix4d V = new MyMatrix4d(InvR); MyMatrix4d I = MyMatrix4d.IdentityMatrix(); I[0, 0] = -1; // reflect matrix along yz Canvas3 MyMatrix4d T = V * I * U; // the reflection matrix // reflect MyVector4 r = new MyVector4(u, 0); MyVector3 q = Canvas3_center + (T * r).XYZ(); return(q); }
//just. 1116 private MyMatrix4d QuatToMyMatrix4d(MyVector3 q, double a) { q.Normalize(); double cos_theta = Math.Cos(a); double sin_theta = Math.Sin(a); double xx = q.x * q.x; double yy = q.y * q.y; double zz = q.z * q.z; double xy = q.x * q.y; double xz = q.x * q.z; double yz = q.y * q.z; MyMatrix4d m = new MyMatrix4d(); m[0, 0] = cos_theta + xx * (1 - cos_theta); m[0, 1] = xy * (1 - cos_theta) - q.z * sin_theta; m[0, 2] = xz * (1 - cos_theta) + q.y * sin_theta; m[1, 0] = xy * (1 - cos_theta) + q.z * sin_theta; m[1, 1] = cos_theta + yy * (1 - cos_theta); m[1, 2] = yz * (1 - cos_theta) - q.x * sin_theta; m[2, 0] = xz * (1 - cos_theta) - q.y * sin_theta; m[2, 1] = yz * (1 - cos_theta) + q.x * sin_theta; m[2, 2] = cos_theta + zz * (1 - cos_theta); m[3, 3] = 1.0; //m.Inverse(); return(m); }
public static MyMatrix4d RotationMatrix(MyVector3 axis, double cos, double sin) { MyMatrix4d m = IdentityMatrix(); double c = cos; double s = sin; axis.Normalize(); double nx = axis[0]; double ny = axis[1]; double nz = axis[2]; m[0, 0] = c + (1 - c) * nx * nx; m[0, 1] = -s * nz + (1 - c) * nx * ny; m[0, 2] = s * ny + (1 - c) * nx * nz; m[0, 3] = 0.0; m[1, 0] = s * nz + (1 - c) * nx * ny; m[1, 1] = c + (1 - c) * ny * ny; m[1, 2] = -s * nx + (1 - c) * ny * nz; m[1, 3] = 0.0; m[2, 0] = -s * ny + (1 - c) * nz * nx; m[2, 1] = s * nx + (1 - c) * nz * ny; m[2, 2] = c + (1 - c) * nz * nz; m[2, 3] = 0.0; m[3, 0] = 0.0; m[3, 1] = 0.0; m[3, 2] = 0.0; m[3, 3] = 1.0; return(m); }
public static MyMatrix4d RotationMatrixU2V(MyVector3 u, MyVector3 v) { // find the rotational matrix which rotate u to v // one should be extremely careful here, very small viboration // will make a lot of difference // e.g., u = (0.5*e-10, 0.1*e-10, 1), v = (0,0,-1), will make // an fliping around axie (1, 5, 0) with angele Pi MyMatrix4d R = MyMatrix4d.IdentityMatrix(); double cos = u.Normalize().Dot(v.Normalize()); if (Math.Abs(Math.Abs(cos) - 1.0f) < 1e-5) // coincident, do nothing { return(R); } MyVector3 axis = u.Cross(v).Normalize(); if (!double.IsNaN(axis.x)) { if (cos < -1) { cos = -1; } if (cos > 1) { cos = 1; } double angle = Math.Acos(cos); R = MyMatrix4d.RotationMatrix(axis, angle); } return(R); }
public static MyMatrix4d IdentityMatrix() { MyMatrix4d m = new MyMatrix4d(); m[0] = m[5] = m[10] = m[15] = 1.0; return(m); }
public static MyMatrix4d CompressMatrix(double alpha) { MyMatrix4d m = IdentityMatrix(); m[3, 2] = alpha; return(m); }
public MyMatrix4d GetMatrix() { if (type == MotionType.Rotation) { return(QuatToMyMatrix4d(quat)); } if (type == MotionType.Scale) { MyMatrix4d m = MyMatrix4d.IdentityMatrix(); m[0, 0] = m[1, 1] = m[2, 2] = 1.0 + (edPt.x - stPt.x) * adjustWidth; //m[0,0] = m[1,1] = m[2,2] = 1.0 + adjustWidth; return(m); } if (type == MotionType.Pan) { MyMatrix4d m = MyMatrix4d.IdentityMatrix(); if (edPt == stPt) { return(m); } m[0, 3] = 0.07 * (edPt.x - stPt.x); m[1, 3] = 0.07 * (edPt.y - stPt.y); return(m); } return(MyMatrix4d.IdentityMatrix()); }
public static MyMatrix4d TranslationMatrix(MyVector3 t) { MyMatrix4d m = IdentityMatrix(); m[0, 3] = t[0]; m[1, 3] = t[1]; m[2, 3] = t[2]; return(m); }
public static MyMatrix4d operator *(MyMatrix4d m, double d) { MyMatrix4d ret = new MyMatrix4d(); for (int i = 0; i < len; i++) { ret[i] = m[i] * d; } return(ret); }
public static MyMatrix4d operator -(MyMatrix4d m1, MyMatrix4d m2) { MyMatrix4d ret = new MyMatrix4d(); for (int i = 0; i < len; i++) { ret[i] = m1[i] - m2[i]; } return(ret); }
public static MyMatrix4d ScalingMatrix(double sx, double sy, double sz) { MyMatrix4d m = IdentityMatrix(); m[0, 0] = sx; m[1, 1] = sy; m[2, 2] = sz; m[3, 3] = 1.0; return(m); }
public void Transform(MyMatrix4d tran) { for (int i = 0, j = 0; i < vertexCount; i++, j += 3) { MyVector4 v = new MyVector4(vertexPos[j], vertexPos[j + 1], vertexPos[j + 2], 1.0); v = tran * v; vertexPos[j] = v.x; vertexPos[j + 1] = v.y; vertexPos[j + 2] = v.z; } }
public MyMatrix4d Transpose() { MyMatrix4d m = new MyMatrix4d(); for (int i = 0; i < row_size; i++) { for (int j = 0; j < row_size; j++) { m[j, i] = this[i, j]; } } return(m); }
public void ComputeBoundQuad() { MyVector3 normal = Normal(); MyVector3 xaxis = new MyVector3(1, 0, 0); MyVector3 yaxis = new MyVector3(0, 1, 0); MyVector3 zaxis = new MyVector3(0, 0, 1); MyVector3 rotAxis = zaxis.Cross(normal).Normalize(); double cos = zaxis.Dot(normal); if (cos > 1) { cos = 1; } if (cos < -1) { cos = -1; } double rotAngle = Math.Acos(cos); MyMatrix4d R = MyMatrix4d.RotationMatrix(rotAxis, rotAngle); MyVector3 new_xaxis = (R * new MyVector4(xaxis)).XYZ().Normalize(); MyVector3 new_yaxis = (R * new MyVector4(yaxis)).XYZ().Normalize(); CoordinateFrame frame = new CoordinateFrame(this.planeCenter, new_xaxis, new_yaxis, normal); double xmin = double.MaxValue; double ymin = double.MaxValue; double xmax = double.MinValue; double ymax = double.MinValue; foreach (MyVector3 v in this.planeVertices) { MyVector3 u = frame.GetPointLocalCoord(v); xmin = Math.Min(u.x, xmin); xmax = Math.Max(u.x, xmax); ymin = Math.Min(u.y, ymin); ymax = Math.Max(u.y, ymax); } MyVector3 v1 = new MyVector3(xmin, ymin, 0); MyVector3 v2 = new MyVector3(xmax, ymin, 0); MyVector3 v3 = new MyVector3(xmax, ymax, 0); MyVector3 v4 = new MyVector3(xmin, ymax, 0); SmartCanvas.Point3[] pts3 = new SmartCanvas.Point3[4] { new SmartCanvas.Point3(frame.GetPointSpaceCoord(v1)), new SmartCanvas.Point3(frame.GetPointSpaceCoord(v2)), new SmartCanvas.Point3(frame.GetPointSpaceCoord(v3)), new SmartCanvas.Point3(frame.GetPointSpaceCoord(v4)) }; this.boundQuad = new Quad3D(pts3); }
public static MyMatrix4d operator *(MyMatrix4d m1, MyMatrix4d m2) { MyMatrix4d ret = new MyMatrix4d(); for (int i = 0; i < row_size; i++) { for (int j = 0; j < row_size; j++) { ret[i, j] = 0.0; for (int k = 0; k < row_size; k++) { ret[i, j] += m1[i, k] * m2[k, j]; } } } return(ret); }
public static void FindRotAxisAngle(MyMatrix4d rotMat, out MyVector3 rotAxis, out double angle) { double trace = rotMat[0, 0] + rotMat[1, 1] + rotMat[2, 2]; angle = Math.Acos((trace - 1) / 2); if (rotMat[0, 1] > 0) { angle = -angle; /// this may be violated... } double sin2 = 2 * Math.Sin(angle); double x = (rotMat[2, 1] - rotMat[1, 2]) / sin2; double y = (rotMat[0, 2] - rotMat[2, 0]) / sin2; double z = (rotMat[1, 0] - rotMat[0, 1]) / sin2; rotAxis = new MyVector3(x, y, z).Normalize(); }
public void Project3dToBelongPlane() { // prepare 2d coordinate MyVector3 circle_norm = this.belongPlane.Normal(); MyVector3 X = new MyVector3(1, 0, 0); MyVector3 Y = new MyVector3(0, 1, 0); MyVector3 Z = new MyVector3(0, 0, 1); MyVector3 rotAxis = Z.Cross(circle_norm).Normalize(); double angle_cos = Z.Dot(circle_norm); if (angle_cos > 1) { angle_cos = 1; } if (angle_cos < -1) { angle_cos = -1; } double rotAngle = Math.Acos(angle_cos); MyMatrix4d Mat = MyMatrix4d.RotationMatrix(rotAxis, rotAngle); MyVector3 X_new = (Mat * new MyVector4(X)).XYZ().Normalize(); MyVector3 Y_new = (Mat * new MyVector4(Y)).XYZ().Normalize(); CoordinateFrame frame = new CoordinateFrame(this.belongPlane.planeCenter, X_new, Y_new, circle_norm); // projection and denoise(leave out far away points) MyVector3 tmpc = frame.GetPointLocalCoord(this.center); this.center2d = new MyVector2(tmpc.x, tmpc.y); for (int i = 0; i < this.circleSlice3d.Count; i++) { MyVector3 vert = this.circleSlice3d[i]; MyVector3 tmp = frame.GetPointLocalCoord(vert); MyVector2 tmp2 = new MyVector2(tmp.x, tmp.y); double dist = Math.Abs((tmp2 - this.center2d).Length() - this.radius); if (dist > 0.5 * this.radius && dist < 0.75 * this.radius) { this.circleSlice3d.RemoveAt(i); i--; } else { this.circleSlice2d.Add(tmp2); } } }
public MyMatrix4d OuterCross(MyVector4 v) { MyMatrix4d m = new MyMatrix4d(); m[0, 0] = x * v.x; m[0, 1] = x * v.y; m[0, 2] = x * v.z; m[0, 3] = x * v.w; m[1, 0] = y * v.x; m[1, 1] = y * v.y; m[1, 2] = y * v.z; m[1, 3] = y * v.w; m[2, 0] = z * v.x; m[2, 1] = z * v.y; m[2, 2] = z * v.z; m[2, 3] = z * v.w; m[3, 0] = w * v.x; m[3, 1] = w * v.y; m[3, 2] = w * v.z; m[3, 3] = w * v.w; return(m); }
private MyMatrix4d QuatToMyMatrix4d(MyVector4 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; MyMatrix4d m = new MyMatrix4d(); 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); m[3, 3] = 1.0; return(m); }
public void BuildLocalFrame() { MyVector3 X = new MyVector3(1, 0, 0); MyVector3 Y = new MyVector3(0, 1, 0); MyVector3 Z = new MyVector3(0, 0, 1); MyVector3 rotAxis = Z.Cross(normal).Normalize(); double angle_cos = Z.Dot(normal); if (angle_cos < -1) { angle_cos = -1; } if (angle_cos > 1) { angle_cos = 1; } double rotAngle = Math.Acos(angle_cos); MyMatrix4d Mat = MyMatrix4d.RotationMatrix(rotAxis, rotAngle); MyVector3 X_new = (Mat * new MyVector4(X)).XYZ().Normalize(); MyVector3 Y_new = (Mat * new MyVector4(Y)).XYZ().Normalize(); frame = new CoordinateFrame(center, X_new, Y_new, normal); }
public static MyVector3 GetTranslation(MyMatrix4d T) { return(new MyVector3(T[0, 3], T[1, 3], T[2, 3])); }
public MyMatrix4d(MyMatrix4d m) : this(m.e) { }