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