//package Applet3D;
|
|
public class cQuat implements java.io.Serializable
|
{
|
static final long serialVersionUID = -4375060109944420639L;
|
|
public cQuat(double d, double d1, double d2, double d3)
|
{
|
double d4 = 1.0D / Math.sqrt(d * d + d1 * d1 + d2 * d2 + d3 * d3);
|
x = d * d4;
|
y = d1 * d4;
|
z = d2 * d4;
|
w = d3 * d4;
|
}
|
|
public cQuat(double ad[])
|
{
|
double d = 1.0D / Math.sqrt(ad[0] * ad[0] + ad[1] * ad[1] + ad[2] * ad[2] + ad[3] * ad[3]);
|
x = ad[0] * d;
|
y = ad[1] * d;
|
z = ad[2] * d;
|
w = ad[3] * d;
|
}
|
|
public cQuat(cQuat quat)
|
{
|
x = quat.x;
|
y = quat.y;
|
z = quat.z;
|
w = quat.w;
|
}
|
|
public cQuat()
|
{
|
}
|
|
public cQuat(cVector axe, double angle)
|
{
|
set(axe, angle);
|
}
|
|
public cQuat(cVector pA, cVector pB)
|
{
|
setRotation(pA, pB);
|
}
|
|
public void setRotation(cVector pA, cVector pB)
|
{
|
if (pA == null || pB == null)
|
{
|
x = y = z = 0.0D;
|
w = 1.0D;
|
return;
|
}
|
cVector lA = new cVector(pA);
|
cVector lB = new cVector(pB);
|
if (lA.length() > 0)
|
lA.normalize();
|
if (lB.length() > 0)
|
lB.normalize();
|
double lDot = lA.dot(lB);
|
cVector lCP = new cVector();
|
lCP.cross(lA, lB);
|
if (lCP.length() > 0)
|
lCP.normalize();
|
else
|
if (lDot < 0.0D)
|
{
|
lCP.cross(lA, cVector.Y);
|
if (lCP.length() > 0)
|
{
|
lCP.normalize();
|
} else
|
{
|
lCP.cross(lA, cVector.X);
|
lCP.normalize();
|
}
|
}
|
double lTmp = (lDot + 1.0D) / 2D;
|
double lCos = Math.sqrt(lTmp);
|
double lSin = Math.sqrt(1.0D - lTmp);
|
x = (double)lCP.x * lSin;
|
y = (double)lCP.y * lSin;
|
z = (double)lCP.z * lSin;
|
w = lCos;
|
}
|
|
public void set(cVector axe, double angle)
|
{
|
double s = Math.sin(angle/2);
|
double c = Math.cos(angle/2);
|
|
axe.normalize();
|
|
x = axe.x * s;
|
y = axe.y * s;
|
z = axe.z * s;
|
w = c;
|
}
|
|
public void conjugate(cQuat quat)
|
{
|
x = -quat.x;
|
y = -quat.y;
|
z = -quat.z;
|
w = quat.w;
|
}
|
|
public void conjugate()
|
{
|
x = -x;
|
y = -y;
|
z = -z;
|
}
|
|
public void mul(cQuat quat, cQuat quat1)
|
{
|
if (this != quat && this != quat1)
|
{
|
w = quat.w * quat1.w - quat.x * quat1.x - quat.y * quat1.y - quat.z * quat1.z;
|
x = (quat.w * quat1.x + quat1.w * quat.x + quat.y * quat1.z) - quat.z * quat1.y;
|
y = ((quat.w * quat1.y + quat1.w * quat.y) - quat.x * quat1.z) + quat.z * quat1.x;
|
z = (quat.w * quat1.z + quat1.w * quat.z + quat.x * quat1.y) - quat.y * quat1.x;
|
} else
|
{
|
double d2 = quat.w * quat1.w - quat.x * quat1.x - quat.y * quat1.y - quat.z * quat1.z;
|
double d = (quat.w * quat1.x + quat1.w * quat.x + quat.y * quat1.z) - quat.z * quat1.y;
|
double d1 = ((quat.w * quat1.y + quat1.w * quat.y) - quat.x * quat1.z) + quat.z * quat1.x;
|
z = (quat.w * quat1.z + quat1.w * quat.z + quat.x * quat1.y) - quat.y * quat1.x;
|
w = d2;
|
x = d;
|
y = d1;
|
}
|
}
|
|
public void mul(cQuat quat)
|
{
|
double d2 = w * quat.w - x * quat.x - y * quat.y - z * quat.z;
|
double d = (w * quat.x + quat.w * x + y * quat.z) - z * quat.y;
|
double d1 = ((w * quat.y + quat.w * y) - x * quat.z) + z * quat.x;
|
z = (w * quat.z + quat.w * z + x * quat.y) - y * quat.x;
|
w = d2;
|
x = d;
|
y = d1;
|
}
|
|
public void rotate(cVector p)
|
{
|
double m00 = w*w + x*x - y*y - z*z;
|
double m01 = 2*(x*y - w*z);
|
double m02 = 2*(w*y + x*z);
|
double m10 = 2*(w*z + x*y);
|
double m11 = w*w - x*x + y*y - z*z;
|
double m12 = 2*(y*z - w*x);
|
double m20 = 2*(x*z - w*y);
|
double m21 = 2*(w*x + y*z);
|
double m22 = w*w - x*x - y*y + z*z;
|
|
double fx = p.x*m00 + p.y*m01 + p.z*m02;
|
double fy = p.x*m10 + p.y*m11 + p.z*m12;
|
double fz = p.x*m20 + p.y*m21 + p.z*m22;
|
|
/*
|
double tx = p.x*x;
|
double ty = p.y*x;
|
double tz = p.z*x;
|
|
double fx = x*tx + y*ty + z*tz;
|
|
tx = p.x*y;
|
ty = p.y*y;
|
tz = p.z*y;
|
double fy = x*tx + y*ty + z*tz;
|
|
tx = p.x*z;
|
ty = p.y*z;
|
tz = p.z*z;
|
double fz = x*tx + y*ty + z*tz;
|
*/
|
|
p.x = (double)fx;
|
p.y = (double)fy;
|
p.z = (double)fz;
|
}
|
|
public void mulInverse(cQuat quat, cQuat quat1)
|
{
|
cQuat quat2 = new cQuat(quat1);
|
quat2.inverse();
|
mul(quat, quat2);
|
}
|
|
public void mulInverse(cQuat quat)
|
{
|
cQuat quat1 = new cQuat(quat);
|
quat1.inverse();
|
mul(quat1);
|
}
|
|
public void inverse(cQuat quat)
|
{
|
double d = 1.0D / (quat.w * quat.w + quat.x * quat.x + quat.y * quat.y + quat.z * quat.z);
|
w = d * quat.w;
|
x = -d * quat.x;
|
y = -d * quat.y;
|
z = -d * quat.z;
|
}
|
|
public void inverse()
|
{
|
double d = 1.0D / (w * w + x * x + y * y + z * z);
|
w *= d;
|
x *= -d;
|
y *= -d;
|
z *= -d;
|
}
|
|
public void normalize(cQuat quat)
|
{
|
double d = quat.x * quat.x + quat.y * quat.y + quat.z * quat.z + quat.w * quat.w;
|
if (d > 0.0D)
|
{
|
d = 1.0D / Math.sqrt(d);
|
x = d * quat.x;
|
y = d * quat.y;
|
z = d * quat.z;
|
w = d * quat.w;
|
} else
|
{
|
x = 0.0D;
|
y = 0.0D;
|
z = 0.0D;
|
w = 0.0D;
|
}
|
}
|
|
public void normalize()
|
{
|
double d = x * x + y * y + z * z + w * w;
|
if (d > 0.0D)
|
{
|
d = 1.0D / Math.sqrt(d);
|
x *= d;
|
y *= d;
|
z *= d;
|
w *= d;
|
} else
|
{
|
x = 0.0D;
|
y = 0.0D;
|
z = 0.0D;
|
w = 0.0D;
|
}
|
}
|
|
/*
|
public final void set(Matrix4f matrix4f)
|
{
|
double d = 0.25D * (double)(matrix4f.m00 + matrix4f.m11 + matrix4f.m22 + matrix4f.m33);
|
if (d >= 0.0D)
|
{
|
if (d >= 1.0000000000000001E-30D)
|
{
|
w = Math.sqrt(d);
|
d = 0.25D / w;
|
x = (double)(matrix4f.m21 - matrix4f.m12) * d;
|
y = (double)(matrix4f.m02 - matrix4f.m20) * d;
|
z = (double)(matrix4f.m10 - matrix4f.m01) * d;
|
return;
|
}
|
} else
|
{
|
w = 0.0D;
|
x = 0.0D;
|
y = 0.0D;
|
z = 1.0D;
|
return;
|
}
|
w = 0.0D;
|
d = -0.5D * (double)(matrix4f.m11 + matrix4f.m22);
|
if (d >= 0.0D)
|
{
|
if (d >= 1.0000000000000001E-30D)
|
{
|
x = Math.sqrt(d);
|
d = 1.0D / (2D * x);
|
y = (double)matrix4f.m10 * d;
|
z = (double)matrix4f.m20 * d;
|
return;
|
}
|
} else
|
{
|
x = 0.0D;
|
y = 0.0D;
|
z = 1.0D;
|
return;
|
}
|
x = 0.0D;
|
d = 0.5D * (1.0D - (double)matrix4f.m22);
|
if (d >= 1.0000000000000001E-30D)
|
{
|
y = Math.sqrt(d);
|
z = (double)matrix4f.m21 / (2D * y);
|
return;
|
} else
|
{
|
y = 0.0D;
|
z = 1.0D;
|
return;
|
}
|
}
|
|
public final void set(Matrix4d matrix4d)
|
{
|
double d = 0.25D * (matrix4d.m00 + matrix4d.m11 + matrix4d.m22 + matrix4d.m33);
|
if (d >= 0.0D)
|
{
|
if (d >= 1.0000000000000001E-30D)
|
{
|
w = Math.sqrt(d);
|
d = 0.25D / w;
|
x = (matrix4d.m21 - matrix4d.m12) * d;
|
y = (matrix4d.m02 - matrix4d.m20) * d;
|
z = (matrix4d.m10 - matrix4d.m01) * d;
|
return;
|
}
|
} else
|
{
|
w = 0.0D;
|
x = 0.0D;
|
y = 0.0D;
|
z = 1.0D;
|
return;
|
}
|
w = 0.0D;
|
d = -0.5D * (matrix4d.m11 + matrix4d.m22);
|
if (d >= 0.0D)
|
{
|
if (d >= 1.0000000000000001E-30D)
|
{
|
x = Math.sqrt(d);
|
d = 0.5D / x;
|
y = matrix4d.m10 * d;
|
z = matrix4d.m20 * d;
|
return;
|
}
|
} else
|
{
|
x = 0.0D;
|
y = 0.0D;
|
z = 1.0D;
|
return;
|
}
|
x = 0.0D;
|
d = 0.5D * (1.0D - matrix4d.m22);
|
if (d >= 1.0000000000000001E-30D)
|
{
|
y = Math.sqrt(d);
|
z = matrix4d.m21 / (2D * y);
|
return;
|
} else
|
{
|
y = 0.0D;
|
z = 1.0D;
|
return;
|
}
|
}
|
|
public final void set(Matrix3f matrix3f)
|
{
|
double d = 0.25D * ((double)(matrix3f.m00 + matrix3f.m11 + matrix3f.m22) + 1.0D);
|
if (d >= 0.0D)
|
{
|
if (d >= 1.0000000000000001E-30D)
|
{
|
w = Math.sqrt(d);
|
d = 0.25D / w;
|
x = (double)(matrix3f.m21 - matrix3f.m12) * d;
|
y = (double)(matrix3f.m02 - matrix3f.m20) * d;
|
z = (double)(matrix3f.m10 - matrix3f.m01) * d;
|
return;
|
}
|
} else
|
{
|
w = 0.0D;
|
x = 0.0D;
|
y = 0.0D;
|
z = 1.0D;
|
return;
|
}
|
w = 0.0D;
|
d = -0.5D * (double)(matrix3f.m11 + matrix3f.m22);
|
if (d >= 0.0D)
|
{
|
if (d >= 1.0000000000000001E-30D)
|
{
|
x = Math.sqrt(d);
|
d = 0.5D / x;
|
y = (double)matrix3f.m10 * d;
|
z = (double)matrix3f.m20 * d;
|
return;
|
}
|
} else
|
{
|
x = 0.0D;
|
y = 0.0D;
|
z = 1.0D;
|
return;
|
}
|
x = 0.0D;
|
d = 0.5D * (1.0D - (double)matrix3f.m22);
|
if (d >= 1.0000000000000001E-30D)
|
{
|
y = Math.sqrt(d);
|
z = (double)matrix3f.m21 / (2D * y);
|
}
|
y = 0.0D;
|
z = 1.0D;
|
}
|
|
public final void set(Matrix3d matrix3d)
|
{
|
double d = 0.25D * (matrix3d.m00 + matrix3d.m11 + matrix3d.m22 + 1.0D);
|
if (d >= 0.0D)
|
{
|
if (d >= 1.0000000000000001E-30D)
|
{
|
w = Math.sqrt(d);
|
d = 0.25D / w;
|
x = (matrix3d.m21 - matrix3d.m12) * d;
|
y = (matrix3d.m02 - matrix3d.m20) * d;
|
z = (matrix3d.m10 - matrix3d.m01) * d;
|
return;
|
}
|
} else
|
{
|
w = 0.0D;
|
x = 0.0D;
|
y = 0.0D;
|
z = 1.0D;
|
return;
|
}
|
w = 0.0D;
|
d = -0.5D * (matrix3d.m11 + matrix3d.m22);
|
if (d >= 0.0D)
|
{
|
if (d >= 1.0000000000000001E-30D)
|
{
|
x = Math.sqrt(d);
|
d = 0.5D / x;
|
y = matrix3d.m10 * d;
|
z = matrix3d.m20 * d;
|
return;
|
}
|
} else
|
{
|
x = 0.0D;
|
y = 0.0D;
|
z = 1.0D;
|
return;
|
}
|
x = 0.0D;
|
d = 0.5D * (1.0D - matrix3d.m22);
|
if (d >= 1.0000000000000001E-30D)
|
{
|
y = Math.sqrt(d);
|
z = matrix3d.m21 / (2D * y);
|
return;
|
} else
|
{
|
y = 0.0D;
|
z = 1.0D;
|
return;
|
}
|
}
|
|
public final void set(AxisAngle4f axisangle4f)
|
{
|
double d1 = Math.sqrt(axisangle4f.x * axisangle4f.x + axisangle4f.y * axisangle4f.y + axisangle4f.z * axisangle4f.z);
|
if (d1 < 9.9999999999999995E-07D)
|
{
|
w = 0.0D;
|
x = 0.0D;
|
y = 0.0D;
|
z = 0.0D;
|
} else
|
{
|
double d = Math.sin((double)axisangle4f.angle / 2D);
|
d1 = 1.0D / d1;
|
w = Math.cos((double)axisangle4f.angle / 2D);
|
x = (double)axisangle4f.x * d1 * d;
|
y = (double)axisangle4f.y * d1 * d;
|
z = (double)axisangle4f.z * d1 * d;
|
}
|
}
|
|
public final void set(AxisAngle4d axisangle4d)
|
{
|
double d1 = Math.sqrt(axisangle4d.x * axisangle4d.x + axisangle4d.y * axisangle4d.y + axisangle4d.z * axisangle4d.z);
|
if (d1 < 9.9999999999999995E-07D)
|
{
|
w = 0.0D;
|
x = 0.0D;
|
y = 0.0D;
|
z = 0.0D;
|
} else
|
{
|
d1 = 1.0D / d1;
|
double d = Math.sin(axisangle4d.angle / 2D);
|
w = Math.cos(axisangle4d.angle / 2D);
|
x = axisangle4d.x * d1 * d;
|
y = axisangle4d.y * d1 * d;
|
z = axisangle4d.z * d1 * d;
|
}
|
}
|
*/
|
|
public void interpolate(cQuat quat, double d)
|
{
|
double d1 = x * quat.x + y * quat.y + z * quat.z + w * quat.w;
|
if (d1 < 0.0D)
|
{
|
quat.x = -quat.x;
|
quat.y = -quat.y;
|
quat.z = -quat.z;
|
quat.w = -quat.w;
|
}
|
double d2;
|
double d3;
|
if (1.0D - Math.abs(d1) > 9.9999999999999995E-07D)
|
{
|
double d4 = Math.acos(d1);
|
double d5 = Math.sin(d4);
|
d2 = Math.sin((1.0D - d) * d4) / d5;
|
d3 = Math.sin(d * d4) / d5;
|
} else
|
{
|
d2 = 1.0D - d;
|
d3 = d;
|
}
|
w = d2 * w + d3 * quat.w;
|
x = d2 * x + d3 * quat.x;
|
y = d2 * y + d3 * quat.y;
|
z = d2 * z + d3 * quat.z;
|
}
|
|
public void interpolate(cQuat quat, cQuat quat1, double d)
|
{
|
double d1 = quat1.x * quat.x + quat1.y * quat.y + quat1.z * quat.z + quat1.w * quat.w;
|
if (d1 < 0.0D)
|
{
|
quat.x = -quat.x;
|
quat.y = -quat.y;
|
quat.z = -quat.z;
|
quat.w = -quat.w;
|
}
|
double d2;
|
double d3;
|
if (1.0D - Math.abs(d1) > 9.9999999999999995E-07D)
|
{
|
double d4 = Math.acos(d1);
|
double d5 = Math.sin(d4);
|
d2 = Math.sin((1.0D - d) * d4) / d5;
|
d3 = Math.sin(d * d4) / d5;
|
} else
|
{
|
d2 = 1.0D - d;
|
d3 = d;
|
}
|
w = d2 * quat.w + d3 * quat1.w;
|
x = d2 * quat.x + d3 * quat1.x;
|
y = d2 * quat.y + d3 * quat1.y;
|
z = d2 * quat.z + d3 * quat1.z;
|
}
|
|
//static final long serialVersionUID = 0x69289dcfL;
|
//static final double EPS = 9.9999999999999995E-07D;
|
//static final double EPS2 = 1.0000000000000001E-30D;
|
//static final double PIO2 = 1.57079632679D;
|
|
double x,y,z,w;
|
}
|
