}// createPointClound
public void create(Vector3d u, Vector3d v)
{
// body bone
Vector3d center = (u + v) / 2;
Vector3d dir = (v - u).normalize();
double radius = (u - v).Length() / 2;
Matrix4d scaleMat = Matrix4d.ScalingMatrix(radius / _x, 1, 1);
Vector3d axis = new Vector3d(1, 0, 0);
Vector3d rotAxis = axis.Cross(dir).normalize();
Matrix4d transMat = Matrix4d.TranslationMatrix(center);
Matrix4d rotMat = Matrix4d.IdentityMatrix();
if (!double.IsNaN(rotAxis.x) && !double.IsNaN(rotAxis.y) && !double.IsNaN(rotAxis.z))
{
double acos = axis.Dot(dir);
if (acos < -1)
{
acos = -1;
}
else if (acos > 1)
{
acos = 1;
}
double rot_angle = Math.Acos(acos);
rotMat = Matrix4d.RotationMatrix(rotAxis, rot_angle);
}
Matrix4d T = transMat * rotMat * scaleMat;
this.TransformFromUnit(T);
}// create
public Matrix4d getTransformMatrix(int perspective)
{
Matrix4d m = Matrix4d.IdentityMatrix();
switch (this.motion)
{
case MotionType.NONE:
break;
case MotionType.Pan:
{
Vector3d d = this.currPos - this.startPos;
m = Matrix4d.TranslationMatrix(d);
break;
}
case MotionType.Scale:
{
m[0, 0] = m[1, 1] = m[2, 2] = 1.0 + (this.currPos.x - this.startPos.x);
break;
}
case MotionType.Rotate:
default:
{
m = this.getRotationMatrix(perspective);
break;
}
}
return(m);
}
public Matrix4d getRotationMatrixAlongAxis(int axis)
{
Matrix4d m = Matrix4d.IdentityMatrix();
double alpha = Math.Acos(currPos.Dot(startPos));
if (axis == 0)
{
m[1, 1] = Math.Cos(alpha);
m[1, 2] = Math.Sin(alpha);
m[2, 1] = -Math.Sin(alpha);
m[2, 2] = Math.Cos(alpha);
}
if (axis == 1)
{
m[0, 0] = Math.Cos(alpha);
m[0, 2] = -Math.Sin(alpha);
m[2, 0] = Math.Sin(alpha);
m[2, 2] = Math.Cos(alpha);
}
if (axis == 2)
{
m[0, 0] = Math.Cos(alpha);
m[0, 1] = Math.Sin(alpha);
m[1, 0] = -Math.Sin(alpha);
m[1, 1] = Math.Cos(alpha);
}
return(m);
}// getRotationMatrixAlongAxis
public static Matrix4d ReflectionalMatrix(Vector3d plane_normal)
{
// create an coordinates sys, with plane_normal as x-axis
Vector3d x = plane_normal;
Vector3d y;
if (x.x == 0 && x.y == 0)
{
y = new Vector3d(1, 0, 0);
}
else
{
y = (new Vector3d(-x.y, x.x, 0)).normalize();
}
Vector3d z = x.Cross(y).normalize();
Matrix3d R = new Matrix3d(x, y, z).Transpose();
Matrix3d InvR = R.Inverse();
Matrix4d U = new Matrix4d(R);
Matrix4d V = new Matrix4d(InvR);
Matrix4d I = Matrix4d.IdentityMatrix();
I[0, 0] = -1; // reflect matrix along yz plane
return(V * I * U);
}