public static Matrix4d RotationMatrixU2V(Vector3 u, Vector3 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
Matrix4d R = Matrix4d.IdentityMatrix();
float cos = Vector3.Dot(u.normalized, v.normalized);
if (Mathf.Abs(Mathf.Abs(cos) - 1.0f) < 1e-5) // coincident, do nothing
{
return(R);
}
Vector3 axis = Vector3.Normalize(Vector3.Cross(u, v));
if (!float.IsNaN(axis.x))
{
if (cos < -1)
{
cos = -1;
}
if (cos > 1)
{
cos = 1;
}
float angle = Mathf.Acos(cos);
R = Matrix4d.RotationMatrix(axis, angle);
}
return(R);
}
// compute the reflect point of p according to the Canvas3 (Canvas3_normal, Canvas3_center)
public static Vector3 Reflect(Vector3 p, Vector3 Canvas3_normal, Vector3 Canvas3_center)
{
Vector3 u = p - Canvas3_center;
// create a coordinate system (x,y,z), project to that sys, reflect and project back
Vector3 x = Canvas3_normal;
Vector3 y;
if (x.x == 0 && x.y == 0)
{
y = new Vector3(0, -x.z, x.y);
}
else
{
y = new Vector3(x.y, -x.x, 0);
}
Vector3 z = Vector3.Cross(x, y);
Matrix3d R = new Matrix3d(x, y, z);
Matrix3d InvR = R.InverseSVD();
Matrix4d U = new Matrix4d(R);
Matrix4d V = new Matrix4d(InvR);
Matrix4d I = Matrix4d.IdentityMatrix();
I[0, 0] = -1; // reflect matrix along yz Canvas3
Matrix4d T = V * I * U; // the reflection matrix
// reflect
Vector4 r = new Vector4(u.x, u.y, u.z, 0);
Vector3 temp = T * r;
Vector3 q = Canvas3_center + temp;
return(q);
}
public Matrix4d GetMatrix()
{
if (type == MotionType.Rotation)
{
return(QuatToMatrix4d(quat));
}
if (type == MotionType.Scale)
{
Matrix4d m = Matrix4d.IdentityMatrix();
m[0, 0] = m[1, 1] = m[2, 2] = 1.0 + (edPt.x - stPt.x) * adjustWidth;
return(m);
}
if (type == MotionType.Pan)
{
Matrix4d m = Matrix4d.IdentityMatrix();
m[0, 3] = edPt.x - stPt.x;
m[1, 3] = edPt.y - stPt.y;
return(m);
}
return(Matrix4d.IdentityMatrix());
}