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 Quaternion Inverse()
{
double b = s * s + v.Dot(v);
if (b != 0)
{
return(new Quaternion(s / b, -v.x / b, -v.y / b, -v.z / b));
}
throw new DivideByZeroException();
}
public MyVector3 RayIntersection(MyVector3 v1, MyVector3 dir)
{
if (Math.Abs(dir.Dot(this.Normal())) < 1e-7)
{
return(new MyVector3());
}
double a = dir.x;
double b = dir.y;
double c = dir.z;
double A = this.planeEquation.A;
double B = this.planeEquation.B;
double C = this.planeEquation.C;
double D = this.planeEquation.Offset;
double t = (-D - A * v1.x - B * v1.y - C * v1.z) / (A * a + B * b + C * c);
MyVector3 result = new MyVector3();
result.x = a * t + v1.x;
result.y = b * t + v1.y;
result.z = c * t + v1.z;
return(result);
}
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 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 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);
}
}
}
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 MyVector3 Rotate(MyVector3 axis, double cos, double sin)
{
return(this * cos + (axis.Cross(this)) * sin +
axis * ((1.0 - cos) * (axis.Dot(this))));
}
