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