// compute the releft point of p according to the Canvas3 (Canvas3_normal, Canvas3_center)
public static MyVector3 Reflect(MyVector3 p, MyVector3 Canvas3_normal, MyVector3 Canvas3_center)
{
MyVector3 u = p - Canvas3_center;
// create a coord system (x,y,z), project to that sys, reflect and project back
MyVector3 x = Canvas3_normal;
MyVector3 y;
if (x.x == 0 && x.y == 0)
{
y = new MyVector3(0, -x.z, x.y);
}
else
{
y = new MyVector3(x.y, -x.x, 0);
}
MyVector3 z = x.Cross(y);
MyMatrix3d R = new MyMatrix3d(x, y, z);
MyMatrix3d InvR = R.InverseSVD();
MyMatrix4d U = new MyMatrix4d(R);
MyMatrix4d V = new MyMatrix4d(InvR);
MyMatrix4d I = MyMatrix4d.IdentityMatrix();
I[0, 0] = -1; // reflect matrix along yz Canvas3
MyMatrix4d T = V * I * U; // the reflection matrix
// reflect
MyVector4 r = new MyVector4(u, 0);
MyVector3 q = Canvas3_center + (T * r).XYZ();
return(q);
}
//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);
}
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);
}
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 IdentityMatrix()
{
MyMatrix4d m = new MyMatrix4d();
m[0] = m[5] = m[10] = m[15] = 1.0;
return(m);
}
public static MyMatrix4d CompressMatrix(double alpha)
{
MyMatrix4d m = IdentityMatrix();
m[3, 2] = alpha;
return(m);
}
public MyMatrix4d GetMatrix()
{
if (type == MotionType.Rotation)
{
return(QuatToMyMatrix4d(quat));
}
if (type == MotionType.Scale)
{
MyMatrix4d m = MyMatrix4d.IdentityMatrix();
m[0, 0] = m[1, 1] = m[2, 2] = 1.0 + (edPt.x - stPt.x) * adjustWidth;
//m[0,0] = m[1,1] = m[2,2] = 1.0 + adjustWidth;
return(m);
}
if (type == MotionType.Pan)
{
MyMatrix4d m = MyMatrix4d.IdentityMatrix();
if (edPt == stPt)
{
return(m);
}
m[0, 3] = 0.07 * (edPt.x - stPt.x);
m[1, 3] = 0.07 * (edPt.y - stPt.y);
return(m);
}
return(MyMatrix4d.IdentityMatrix());
}
public static MyMatrix4d TranslationMatrix(MyVector3 t)
{
MyMatrix4d m = IdentityMatrix();
m[0, 3] = t[0];
m[1, 3] = t[1];
m[2, 3] = t[2];
return(m);
}
public static MyMatrix4d operator *(MyMatrix4d m, double d)
{
MyMatrix4d ret = new MyMatrix4d();
for (int i = 0; i < len; i++)
{
ret[i] = m[i] * d;
}
return(ret);
}
public static MyMatrix4d operator -(MyMatrix4d m1, MyMatrix4d m2)
{
MyMatrix4d ret = new MyMatrix4d();
for (int i = 0; i < len; i++)
{
ret[i] = m1[i] - m2[i];
}
return(ret);
}
public static MyMatrix4d ScalingMatrix(double sx, double sy, double sz)
{
MyMatrix4d m = IdentityMatrix();
m[0, 0] = sx;
m[1, 1] = sy;
m[2, 2] = sz;
m[3, 3] = 1.0;
return(m);
}
public void Transform(MyMatrix4d tran)
{
for (int i = 0, j = 0; i < vertexCount; i++, j += 3)
{
MyVector4 v = new MyVector4(vertexPos[j], vertexPos[j + 1], vertexPos[j + 2], 1.0);
v = tran * v;
vertexPos[j] = v.x;
vertexPos[j + 1] = v.y;
vertexPos[j + 2] = v.z;
}
}
public MyMatrix4d Transpose()
{
MyMatrix4d m = new MyMatrix4d();
for (int i = 0; i < row_size; i++)
{
for (int j = 0; j < row_size; j++)
{
m[j, i] = this[i, j];
}
}
return(m);
}
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 static MyMatrix4d operator *(MyMatrix4d m1, MyMatrix4d m2)
{
MyMatrix4d ret = new MyMatrix4d();
for (int i = 0; i < row_size; i++)
{
for (int j = 0; j < row_size; j++)
{
ret[i, j] = 0.0;
for (int k = 0; k < row_size; k++)
{
ret[i, j] += m1[i, k] * m2[k, j];
}
}
}
return(ret);
}
public static void FindRotAxisAngle(MyMatrix4d rotMat, out MyVector3 rotAxis, out double angle)
{
double trace = rotMat[0, 0] + rotMat[1, 1] + rotMat[2, 2];
angle = Math.Acos((trace - 1) / 2);
if (rotMat[0, 1] > 0)
{
angle = -angle; /// this may be violated...
}
double sin2 = 2 * Math.Sin(angle);
double x = (rotMat[2, 1] - rotMat[1, 2]) / sin2;
double y = (rotMat[0, 2] - rotMat[2, 0]) / sin2;
double z = (rotMat[1, 0] - rotMat[0, 1]) / sin2;
rotAxis = new MyVector3(x, y, z).Normalize();
}
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 MyMatrix4d OuterCross(MyVector4 v)
{
MyMatrix4d m = new MyMatrix4d();
m[0, 0] = x * v.x;
m[0, 1] = x * v.y;
m[0, 2] = x * v.z;
m[0, 3] = x * v.w;
m[1, 0] = y * v.x;
m[1, 1] = y * v.y;
m[1, 2] = y * v.z;
m[1, 3] = y * v.w;
m[2, 0] = z * v.x;
m[2, 1] = z * v.y;
m[2, 2] = z * v.z;
m[2, 3] = z * v.w;
m[3, 0] = w * v.x;
m[3, 1] = w * v.y;
m[3, 2] = w * v.z;
m[3, 3] = w * v.w;
return(m);
}
private MyMatrix4d QuatToMyMatrix4d(MyVector4 q)
{
double n = q.Dot(q);
double s = (n > 0.0) ? (2.0 / n) : 0.0f;
double xs, ys, zs;
double wx, wy, wz;
double xx, xy, xz;
double yy, yz, zz;
xs = q.x * s; ys = q.y * s; zs = q.z * s;
wx = q.w * xs; wy = q.w * ys; wz = q.w * zs;
xx = q.x * xs; xy = q.x * ys; xz = q.x * zs;
yy = q.y * ys; yz = q.y * zs; zz = q.z * zs;
MyMatrix4d m = new MyMatrix4d();
m[0, 0] = 1.0 - (yy + zz); m[1, 0] = xy + wz; m[2, 0] = xz - wy;
m[0, 1] = xy - wz; m[1, 1] = 1.0 - (xx + zz); m[2, 1] = yz + wx;
m[0, 2] = xz + wy; m[1, 2] = yz - wx; m[2, 2] = 1.0 - (xx + yy);
m[3, 3] = 1.0;
return(m);
}
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 static MyVector3 GetTranslation(MyMatrix4d T)
{
return(new MyVector3(T[0, 3], T[1, 3], T[2, 3]));
}
public MyMatrix4d(MyMatrix4d m) : this(m.e)
{
}