public bool InPlanePolygon(MyVector3 p)//the sum of inner angle is 360
{
//this.ProjectVerticesTo2d();
if (this.planeVertices.Count == 0)
{
return(false);
}
double theta = 0;
MyVector3 v1 = p - planeVertices[0];
MyVector3 v2 = p - planeVertices[planeVertices.Count - 1];
theta = Math.Acos(v1.Dot(v2) / (v1.Length() * v2.Length()));
for (int i = 0; i < planeVertices.Count - 1; i++)
{
v1 = p - planeVertices[i];
v2 = p - planeVertices[i + 1];
theta += Math.Acos(v1.Dot(v2) / (v1.Length() * v2.Length()));
}
if (Math.Abs(2 * Math.PI - theta) < 1e-10)
{
return(true);
}
else
{
return(false);
}
}
public static bool IsParallel(MyVector3 _v1, MyVector3 _v2, double angle = 1)
{
if ((_v1.Dot(_v2) / (_v1.Length() * _v2.Length())) >= Math.Cos(angle * Math.PI / 180))
{
return(true);
}
else
{
return(false);
}
}
public static bool IsParallel(MyPlane p1, MyPlane p2, double angle = 5)
{
MyVector3 _v1 = p1.Normal();
MyVector3 _v2 = p2.Normal();
if (Math.Abs(_v1.Dot(_v2) / (_v1.Length() * _v2.Length())) >= Math.Cos(angle * Math.PI / 180))
{
return(true);
}
else
{
return(false);
}
}
public MyMatrix3d LogMatrix(out MyVector3 axis, out double angle)
{
MyMatrix3d m;
double cos = (this.Trace() - 1) / 2.0;
if (cos < -1)
{
cos = -1;
}
else if (cos > 1)
{
cos = 1;
}
double theta = Math.Acos(cos);
if (Math.Abs(theta) < 0.0001)
{
if (theta >= 0.0)
{
theta = 0.0001;
}
else
{
theta = -0.0001;
}
}
m = (new MyMatrix3d(this) - this.Transpose()) * (0.5 / Math.Sin(theta) * theta);
MyVector3 r = new MyVector3(m[2, 1], m[0, 2], m[1, 0]);
angle = r.Length();
axis = r / angle;
return(m);
}
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 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();
}
