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