public void ComputeVertexNormal() { Array.Clear(vertexNormal, 0, vertexNormal.Length); for (int i = 0, j = 0; i < faceCount; i++, j += 3) { int c1 = faceIndex[j] * 3; int c2 = faceIndex[j + 1] * 3; int c3 = faceIndex[j + 2] * 3; vertexNormal[c1] += faceNormal[j]; vertexNormal[c2] += faceNormal[j]; vertexNormal[c3] += faceNormal[j]; vertexNormal[c1 + 1] += faceNormal[j + 1]; vertexNormal[c2 + 1] += faceNormal[j + 1]; vertexNormal[c3 + 1] += faceNormal[j + 1]; vertexNormal[c1 + 2] += faceNormal[j + 2]; vertexNormal[c2 + 2] += faceNormal[j + 2]; vertexNormal[c3 + 2] += faceNormal[j + 2]; } for (int i = 0, j = 0; i < vertexCount; i++, j += 3) { MyVector3 n = new MyVector3(vertexNormal, j); n = n.Normalize(); vertexNormal[j] = n.x; vertexNormal[j + 1] = n.y; vertexNormal[j + 2] = n.z; } }
public MyPlane(MyVector3 v1, MyVector3 v2, MyVector3 v3) { planeVertices.Add(v1); planeVertices.Add(v2); planeVertices.Add(v3); planePoints.Add(v1); planePoints.Add(v2); planePoints.Add(v3); MyVector3 planenormal = (v1 - v2).Cross(v1 - v3); planenormal.Normalize(); double offset = -v1.Dot(planenormal); planeEquation = new Plane((float)planenormal.x, (float)planenormal.y, (float)planenormal.z, (float)offset); planeCenter = (v1 + v2 + v3) / 3; planeColor = Color.FromArgb(150, 255, 255, 0); // Accord.Math.Point3 _v1 = new Point3((float)v1.x, (float)v1.y, (float)v1.z); // Accord.Math.Point3 _v2 = new Point3((float)v2.x, (float)v2.y, (float)v2.z); // Accord.Math.Point3 _v3 = new Point3((float)v3.x, (float)v3.y, (float)v3.z); // planeEquation = Plane.FromPoints(_v1, _v2, _v3); //ComputePlaneArea(); ComputeBoundQuad(); }
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 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); }
//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); }
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(List <MyVector3> points, bool SP = true)//sweep points { planePoints = new List <MyVector3>(points); double[,] A = new double[3, 3]; double[,] B = new double[3, 1]; foreach (MyVector3 point in points) { A[0, 0] += point.x * point.x; A[0, 1] += point.x * point.y; A[0, 2] += point.x; A[1, 0] += point.x * point.y; A[1, 1] += point.y * point.y; A[1, 2] += point.y; A[2, 0] += point.x; A[2, 1] += point.y; A[2, 2] = points.Count(); B[0, 0] += point.x * point.z; B[1, 0] += point.y * point.z; B[2, 0] += point.z; } double[,] x = Matrix.Solve(A, B, leastSquares: true); MyVector3 ori_normal = new MyVector3(x[0, 0] / x[2, 0], x[1, 0] / x[2, 0], -1 / x[2, 0]); MyVector3 normal = ori_normal.Normalize(); double offset = 1.0 / ori_normal.Length(); planeEquation = new Plane((float)normal.x, (float)normal.y, (float)normal.z, (float)offset); Random rnd = new Random(); planeColor = Color.FromArgb(150, rnd.Next(100, 255), rnd.Next(100, 255), rnd.Next(100, 255)); //this.planenormal = new MyVector3(this.planeEquation.A, this.planeEquation.B, this.planeEquation.C); if (SP) { for (int i = 0; i < planePoints.Count; i++) { if (this.DistanceToPoint(planePoints[i]) > 0.001) { tag = -1;//curved plane return; } } planeColor = Color.FromArgb(150, 255, 255, 0); tag = 1;//sweep plane } for (int i = 0; i < planePoints.Count; i++) { planePoints[i] = this.ProjectPoint(planePoints[i]); } ComputeCenter(); choosen = true; if (planePoints.Count > 3) { ComputeVertices(); } else { planeVertices.AddRange(planePoints); } //this.Scale(1.2); this.ProjectVerticesTo2d(); ComputeBoundQuad(); }