// 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 static MyVector4 Min(MyVector4 v1, MyVector4 v2)
{
return(new MyVector4(
(v1.x < v2.x) ? v1.x : v2.x,
(v1.y < v2.y) ? v1.y : v2.y,
(v1.z < v2.z) ? v1.z : v2.z,
(v1.w < v2.w) ? v1.w : v2.w
));
}
public static MyVector4 Max(MyVector4 v1, MyVector4 v2)
{
return(new MyVector4(
(v1.x > v2.x) ? v1.x : v2.x,
(v1.y > v2.y) ? v1.y : v2.y,
(v1.z > v2.z) ? v1.z : v2.z,
(v1.w > v2.w) ? v1.w : v2.w
));
}
public static MyVector4 operator *(MyMatrix4d m, MyVector4 v)
{
MyVector4 ret = new MyVector4();
ret.x = m[0] * v.x + m[1] * v.y + m[2] * v.z + m[3] * v.w;
ret.y = m[4] * v.x + m[5] * v.y + m[6] * v.z + m[7] * v.w;
ret.z = m[8] * v.x + m[9] * v.y + m[10] * v.z + m[11] * v.w;
ret.w = m[12] * v.x + m[13] * v.y + m[14] * v.z + m[15] * v.w;
return(ret);
}
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 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);
}
public static MyMatrix3d QuatToMyMatrix3d(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;
MyMatrix3d m = new MyMatrix3d();
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);
return(m);
}
public void Drag(MyVector2 pt)
{
edPt = pt;
edVec = MapToSphere(pt);
//angle = Math.Acos(stVec.Dot(edVec));
double epsilon = 1.0e-5;
MyVector3 prep = stVec.Cross(edVec);
if (prep.Length() > epsilon)
{
quat = new MyVector4();
quat.x = prep.x;
quat.y = prep.y;
quat.z = prep.z;
quat.w = stVec.x + edVec.x + stVec.y + edVec.y + stVec.z + edVec.z;
}
else
{
quat = new MyVector4();
}
//if (prep.Length() > epsilon)
//{
// quat = new MyVector3();
// quat.x = prep.x;
// quat.y = prep.y;
// quat.z = prep.z;
//}
//else
// quat = new MyVector3();
//angle = Math.PI / 2.0 * prep.Length();
}
public double Dot(MyVector4 v)
{
return(x * v.x + y * v.y + z * v.z + w * v.w);
}
public void End()
{
quat = new MyVector4();
type = MotionType.None;
}