public AwQuaternion setAxisAngle(AwVector axis, double theta)
{
double sumOfSquares =
(double)axis.x * axis.x +
(double)axis.y * axis.y +
(double)axis.z * axis.z;
if (sumOfSquares <= AwMath.kDoubleEpsilon)
{
w = 1.0;
x = 0.0;
y = 0.0;
z = 0.0;
}
else
{
theta *= 0.5;
w = Math.Cos(theta);
double commonFactor = Math.Sin(theta);
if (!AwMath.equivalent(sumOfSquares, 1.0))
{
commonFactor /= Math.Sqrt(sumOfSquares);
}
x = commonFactor * (double)axis.x;
y = commonFactor * (double)axis.y;
z = commonFactor * (double)axis.z;
}
return(this);
}
public bool getAxisAngle(AwVector axis, ref double theta)
{
bool result;
double inverseOfSinThetaByTwo, thetaExtended;
if (AwMath.equivalent(w, (double)1.0))
{
theta = 0.0;
if (axis.length() < AwMath.kDoubleEpsilon)
{
axis.set(0.0, 0.0, 1.0);
}
result = false;
}
else
{
thetaExtended = Math.Acos(AwMath.clamp(w, -1.0, 1.0));
theta = thetaExtended * 2.0;
inverseOfSinThetaByTwo = 1.0 / Math.Sin(thetaExtended);
axis.x = x * inverseOfSinThetaByTwo;
axis.y = y * inverseOfSinThetaByTwo;
axis.z = z * inverseOfSinThetaByTwo;
result = true;
}
return(result);
}
public bool isParallel(AwVector otherVector, double tolerance)
{
AwVector v1, v2;
v1 = normal();
v2 = otherVector.normal();
double dotPrd = v1.dotProduct(v2);
return(AwMath.equivalent(Math.Abs(dotPrd), (double)1.0, tolerance));
}
public void normalize()
{
double n = norm();
if (n > AwMath.kDoubleEpsilon && !AwMath.equivalent(n, 1.0, 2.0 * AwMath.kDoubleEpsilon))
{
double factor = 1.0 / Math.Sqrt(n);
x *= factor;
y *= factor;
z *= factor;
}
}
public AwQuaternion(AwVector a, AwVector b)
{
w = 1.0;
x = 0.0;
y = 0.0;
z = 0.0;
double factor = a.length() * b.length();
if (Math.Abs(factor) > AwMath.kFloatEpsilon)
{
// Vectors have length > 0
AwVector pivotVector = new AwVector();
double dot = a.dotProduct(b) / factor;
double theta = Math.Acos(AwMath.clamp(dot, -1.0, 1.0));
pivotVector = a.crossProduct(b);
if (dot < 0.0 && pivotVector.length() < AwMath.kFloatEpsilon)
{
// Vectors parallel and opposite direction, therefore a rotation
// of 180 degrees about any vector perpendicular to this vector
// will rotate vector a onto vector b.
//
// The following guarantees the dot-product will be 0.0.
//
uint dominantIndex = (uint)a.dominantAxis();
uint index = (dominantIndex + 1) % 3;
double value = -a.getIndex(index);
pivotVector.setIndex(dominantIndex, value);
pivotVector.setIndex((dominantIndex + 1) % 3, a.getIndex(dominantIndex));
pivotVector.setIndex((dominantIndex + 2) % 3, 0);
}
setAxisAngle(pivotVector, theta);
}
}
