// 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); }
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 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 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); } } }
private void SampleCirclePoints3d(MyVector3 center, MyVector3 normal, double radius, int sample) { MyPlane circleplane = new MyPlane(center, normal); //MyVector3 pointonplane = circleplane.ProjectPoint(center - new MyVector3(0.01, 0.01, 0.01)); //MyVector3 u = pointonplane - center; MyVector3 pointonplane = circleplane.ProjectPoint(center + new MyVector3(-0.1, 0, 0)); MyVector3 u = pointonplane - center; MyVector3 v = u.Cross(normal); u = u.Normalize(); v = v.Normalize(); int num = sample; double theta = 2.0 * Math.PI / (num - 1); for (int j = 0; j < num; j++) { double x = center.x + radius * (u.x * Math.Cos(j * theta) + v.x * Math.Sin(j * theta)); double y = center.y + radius * (u.y * Math.Cos(j * theta) + v.y * Math.Sin(j * theta)); double z = center.z + radius * (u.z * Math.Cos(j * theta) + v.z * Math.Sin(j * theta)); this.circlePoints3d.Add(new MyVector3(x, y, z)); } }
public MyPlane(MyVector3 point, MyVector3 normal) { double d = -point.Dot(normal); planeEquation = new Plane((float)normal.x, (float)normal.y, (float)normal.z, (float)d); planeCenter = point; planeColor = Color.FromArgb(150, 255, 255, 0); // add three plane points MyVector3 x = new MyVector3(normal.y, -normal.x, 0); if (x.x == 0 && x.y == 0) { x = new MyVector3(1, 0, 0); } MyVector3 y = normal.Cross(x).Normalize(); CoordinateFrame frame = new CoordinateFrame(point, x, y, normal); double s = 0.8; MyVector3 v1 = frame.GetPointLocalCoord(new MyVector3(s, s, 0)); MyVector3 v2 = frame.GetPointLocalCoord(new MyVector3(-s, s, 0)); MyVector3 v3 = frame.GetPointLocalCoord(new MyVector3(-s, -s, 0)); MyVector3 v4 = frame.GetPointLocalCoord(new MyVector3(s, -s, 0)); planeVertices.Add(v1); planeVertices.Add(v2); planeVertices.Add(v3); planeVertices.Add(v4); planePoints.Add(v1); planePoints.Add(v2); planePoints.Add(v3); planePoints.Add(v4); this.ComputeVertices(); this.ComputeBoundQuad(); this.ComputeCenter(); }
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 void Drag(MyVector2 pt) { edPt = pt; edVec = MapToSphere(pt); //angle = Math.Acos(stVec.Dot(edVec)); double epsilon = 1.0e-5; MyVector3 prep = stVec.Cross(edVec); if (prep.Length() > epsilon) { quat = new MyVector4(); quat.x = prep.x; quat.y = prep.y; quat.z = prep.z; quat.w = stVec.x + edVec.x + stVec.y + edVec.y + stVec.z + edVec.z; } else { quat = new MyVector4(); } //if (prep.Length() > epsilon) //{ // quat = new MyVector3(); // quat.x = prep.x; // quat.y = prep.y; // quat.z = prep.z; //} //else // quat = new MyVector3(); //angle = Math.PI / 2.0 * prep.Length(); }
public MyVector3 Rotate(MyVector3 axis, double cos, double sin) { return(this * cos + (axis.Cross(this)) * sin + axis * ((1.0 - cos) * (axis.Dot(this)))); }