public void inverseTransform(
final Number3D position, final Number3D scale, final Quaternion orientation) {
Number3D invTranslate = position.inverse();
Number3D invScale = new Number3D(1 / scale.x, 1 / scale.y, 1 / scale.z);
Quaternion invRot = orientation.inverse();
invTranslate = invRot.multiply(invTranslate);
invTranslate.multiply(invScale);
invRot.toRotationMatrix(mTmp);
m[0] = invScale.x * mTmp[0];
m[1] = invScale.x * mTmp[1];
m[2] = invScale.x * mTmp[2];
m[3] = invTranslate.x;
m[4] = invScale.y * mTmp[4];
m[5] = invScale.y * mTmp[5];
m[6] = invScale.y * mTmp[6];
m[7] = invTranslate.y;
m[8] = invScale.z * mTmp[8];
m[9] = invScale.z * mTmp[9];
m[10] = invScale.z * mTmp[10];
m[11] = invTranslate.z;
m[12] = 0;
m[13] = 0;
m[14] = 0;
m[15] = 1;
}
public static Quaternion times(Quaternion result, Quaternion a, Quaternion b) {
result.w = (a.w * b.w) - (a.x * b.x) - (a.y * b.y) - (a.z * b.z);
result.x = (a.w * b.x) + (a.x * b.w) + (a.y * b.z) - (a.z * b.y);
result.y = (a.w * b.y) - (a.x * b.z) + (a.y * b.w) + (a.z * b.x);
result.z = (a.w * b.z) + (a.x * b.y) - (a.y * b.x) + (a.z * b.w);
return result;
}
public static Quaternion makeRotation(Quaternion result, float theta, Vector axis) {
float halfTheta = theta / 2;
float sinHalfTheta = GMath.sin(halfTheta);
result.w = GMath.cos(halfTheta);
result.x = axis.getX() * sinHalfTheta;
result.y = axis.getY() * sinHalfTheta;
result.z = axis.getZ() * sinHalfTheta;
return result;
}
/**
* Post multiplies this {@link Matrix4} with the rotation specified by the provided {@link
* Quaternion}.
*
* @param quat {@link Quaternion} describing the rotation to apply.
* @return A reference to this {@link Matrix4} to facilitate chaining.
*/
@NonNull
public Matrix4 rotate(@NonNull Quaternion quat) {
if (mMatrix == null) {
mMatrix = quat.toRotationMatrix();
} else {
quat.toRotationMatrix(mMatrix);
}
return multiply(mMatrix);
}
/**
* Creates a string to describe this Rotation as a quaternion.
*
* @return A string that describes this Rotation as a quaternion.
*/
public String toQuaternionString() {
return "("
+ quaternion.getX()
+ ", "
+ quaternion.getY()
+ ", "
+ quaternion.getZ()
+ ", "
+ quaternion.getW()
+ ")";
}
/**
* http://ogre.sourcearchive.com/documentation/1.4.5/classOgre_1_1Vector3_eeef4472ad0c4d5f34a038a9f2faa819.html#eeef4472ad0c4d5f34a038a9f2faa819
*
* @param direction
* @return
*/
public Quaternion getRotationTo(Number3D direction) {
// Based on Stan Melax's article in Game Programming Gems
Quaternion q = new Quaternion();
// Copy, since cannot modify local
Number3D v0 = this;
Number3D v1 = direction;
v0.normalize();
v1.normalize();
float d = Number3D.dot(v0, v1);
// If dot == 1, vectors are the same
if (d >= 1.0f) {
q.setIdentity();
}
if (d < 0.000001f - 1.0f) {
// Generate an axis
Number3D axis = Number3D.cross(Number3D.getAxisVector(Axis.X), this);
if (axis.length() == 0) // pick another if colinear
axis = Number3D.cross(Number3D.getAxisVector(Axis.Y), this);
axis.normalize();
q.fromAngleAxis(MathUtil.radiansToDegrees(MathUtil.PI), axis);
} else {
float s = FloatMath.sqrt((1 + d) * 2);
float invs = 1f / s;
Number3D c = Number3D.cross(v0, v1);
q.x = c.x * invs;
q.y = c.y * invs;
q.z = c.z * invs;
q.w = s * 0.5f;
q.normalize();
}
return q;
}
/**
* Redefines this Rotation to a given Quaternion rotation. Note that quaternion-based rotation
* requires the quaternion to be normalized. Thus, rotation cannot be defined by a zero
* quaternion.
*
* @param quaternion The quaternion defining the rotation.
* @throws IllegalArgumentException if the given Quaternion is a zero quaternion.
*/
public void setQuaternion(Quaternion quaternion) {
// zero quaternion?
if (quaternion.isZeroQuaternion())
throw new IllegalArgumentException("Rotation cannot be defined by a zero quaternion.");
this.quaternion = quaternion;
}
public Quaternion multiply(Quaternion other) {
double a, b, c, d;
double x1, y1, z1;
double x2, y2, z2, w2;
x1 = v.getX();
y1 = v.getY();
z1 = v.getZ();
w2 = other.getW();
x2 = other.getX();
y2 = other.getY();
z2 = other.getZ();
a = w * w2 - x1 * x2 - y1 * y2 - z1 * z2;
b = w * x2 + x1 * w2 + y1 * z2 - z1 * y2;
c = w * y2 - x1 * z2 + y1 * w2 + z1 * x2;
d = w * z2 + x1 * y2 - y1 * x2 + z1 * w2;
return new Quaternion(a, b, c, d);
}
public void animateTransform(Quaternion transform) {
// mAnimateTransform = transform;
if (rot != null) {
rot.mulLeft(transform);
mTransform.set(rot);
}
Iterator<GLVertex> iter = mVertexList.iterator();
while (iter.hasNext()) {
GLVertex vertex = iter.next();
mEnv.transformVertex(vertex, mTransform);
}
}
public static Quaternion slerp(double amount, Quaternion value1, Quaternion value2) {
if ((value1 == null) || (value2 == null)) {
throw new IllegalArgumentException("Quaternion Is Null");
}
if (amount < 0.0) return value1;
else if (amount > 1.0) return value2;
double dot = value1.dot(value2);
double x2, y2, z2, w2;
if (dot < 0.0) {
dot = 0.0 - dot;
x2 = 0.0 - value2.x;
y2 = 0.0 - value2.y;
z2 = 0.0 - value2.z;
w2 = 0.0 - value2.w;
} else {
x2 = value2.x;
y2 = value2.y;
z2 = value2.z;
w2 = value2.w;
}
double t1, t2;
final double EPSILON = 0.0001;
if ((1.0 - dot) > EPSILON) // standard case (slerp)
{
double angle = Math.acos(dot);
double sinAngle = Math.sin(angle);
t1 = Math.sin((1.0 - amount) * angle) / sinAngle;
t2 = Math.sin(amount * angle) / sinAngle;
} else // just lerp
{
t1 = 1.0 - amount;
t2 = amount;
}
return new Quaternion(
(value1.x * t1) + (x2 * t2),
(value1.y * t1) + (y2 * t2),
(value1.z * t1) + (z2 * t2),
(value1.w * t1) + (w2 * t2));
}
public void transform(
final Number3D position, final Number3D scale, final Quaternion orientation) {
orientation.toRotationMatrix(mTmp);
m[0] = scale.x * mTmp[0];
m[1] = scale.y * mTmp[1];
m[2] = scale.z * mTmp[2];
m[3] = position.x;
m[4] = scale.x * mTmp[4];
m[5] = scale.y * mTmp[5];
m[6] = scale.z * mTmp[6];
m[7] = position.y;
m[8] = scale.x * mTmp[8];
m[9] = scale.y * mTmp[9];
m[10] = scale.z * mTmp[10];
m[11] = position.z;
m[12] = 0;
m[13] = 0;
m[14] = 0;
m[15] = 1;
}
public void setHoverPosition(GL10 gl, int nFlags, Planet m_Planet) {
gl.glMatrixMode(GL10.GL_MODELVIEW);
gl.glLoadIdentity();
Quaternion orientation = Miniglu.gluGetOrientation();
Vector3 vpLoc = new Vector3(0.0f, 0.0f, 0.0f);
vpLoc.x = m_Eyeposition[0];
vpLoc.y = m_Eyeposition[1];
vpLoc.z = m_Eyeposition[2];
Vector3 objectLoc = new Vector3(0.0f, 0.0f, 0.0f);
objectLoc.x = m_Planet.m_Pos[0];
objectLoc.y = m_Planet.m_Pos[1];
objectLoc.z = m_Planet.m_Pos[2];
Vector3 offset = new Vector3(0.0f, 0.0f, 0.0f);
offset.x = (objectLoc.x - vpLoc.x);
offset.y = (objectLoc.y - vpLoc.y);
offset.z = (objectLoc.z - vpLoc.z);
Vector3 offsetv = new Vector3(0.0f, 0.0f, 0.0f);
offsetv.z = Vector3Distance(objectLoc, vpLoc);
gl.glMatrixMode(GL10.GL_MODELVIEW);
// Rotate around the Y-axis.
float s = (float) Math.sin(0.0005);
float c = (float) Math.cos(0.0005);
Quaternion tempQ2 = new Quaternion(0, s, 0, c);
tempQ2 = tempQ2.mulThis(orientation);
orientation = tempQ2;
float matrix3[][] = new float[3][3];
matrix3 = orientation.toMatrix();
matrix3 = orientation.tranposeMatrix(matrix3);
offsetv = orientation.Matrix3MultiplyVector3(matrix3, offsetv);
m_Eyeposition[0] = (float) (objectLoc.x + offsetv.x);
m_Eyeposition[1] = (float) (objectLoc.y + offsetv.y);
m_Eyeposition[2] = (float) (objectLoc.z + offsetv.z);
lookAtTarget(gl, m_Planet);
}
/**
* Post multiplies this {@link Matrix4} with the rotation specified by the provided cardinal axis
* and angle.
*
* @param axis {@link Axis} The cardinal axis of rotation.
* @param angle double The angle of rotation in degrees.
* @return A reference to this {@link Matrix4} to facilitate chaining.
*/
@NonNull
public Matrix4 rotate(@NonNull Axis axis, double angle) {
return angle == 0 ? this : rotate(mQuat.fromAngleAxis(axis, angle));
}
public Quaternion rotate(double radians, Vector3d axis) {
Quaternion rotation = Quaternion.rotationQuat(radians, axis);
return (rotation.multiply(this));
}
/**
* Interpolates between this Rotation and a given Rotation by a given interpolation factor.
*
* @param rotation The terminal point of interpolation.
* @param factor The interpolation factor. This value will be clamped to the range [0.0, 1.0].
*/
public Rotation slerp(Rotation rotation, double factor) {
// store quaternion components for easy access
Quaternion startQuat = this.quaternion;
Quaternion endQuat = rotation.getQuaternion();
double x1 = startQuat.getX();
double y1 = startQuat.getY();
double z1 = startQuat.getZ();
double w1 = startQuat.getW();
double x2 = endQuat.getX();
double y2 = endQuat.getY();
double z2 = endQuat.getZ();
double w2 = endQuat.getW();
// calculate angle between start and end
double cosHalfTheta = w1 * w2 + x1 * x2 + y1 * y2 + z1 * z2;
// if start=end or start=-end, we can return start
if (Math.abs(cosHalfTheta) >= 1.0) return this;
// calculate temporary values
double halfTheta = Math.acos(cosHalfTheta);
double sinHalfTheta = Math.sqrt(1.0 - cosHalfTheta * cosHalfTheta);
// if theta is 180°, then the result is not fully defined
// we could rotate around any axis normal to start or end
if (Math.abs(sinHalfTheta) < 0.001) {
Vector3D v1 = startQuat.vectorComponent();
Vector3D v2 = endQuat.vectorComponent();
Vector3D vect = v1.add(v2).multiply(0.5);
double scalar = (w1 + w2) * 0.5;
return new Rotation(new Quaternion(vect, scalar));
}
double ratioA = Math.sin((1 - factor) * halfTheta) / sinHalfTheta;
double ratioB = Math.sin(factor * halfTheta) / sinHalfTheta;
// compute and return quaternion
Vector3D v1 = startQuat.vectorComponent();
Vector3D v2 = endQuat.vectorComponent();
Vector3D vect = v1.multiply(ratioA).add((v2).multiply(ratioB));
double scalar = (w1 * ratioA) + (w2 * ratioB);
return new Rotation(new Quaternion(vect, scalar));
}
public Vector3d rotateVector(Vector3d v) {
Quaternion q = new Quaternion(0, v);
Quaternion newQ = (this.multiply(q)).multiply(getInverse());
return newQ.getVector();
}
public Quaternion add(Quaternion other) {
return new Quaternion(
w + other.getW(), getX() + other.getX(), getY() + other.getY(), getZ() + other.getZ());
}
/**
* Sets this {@link Matrix4} to the rotation between two vectors. The incoming vectors should be
* normalized.
*
* @param x1 double The x component of the base vector.
* @param y1 double The y component of the base vector.
* @param z1 double The z component of the base vector.
* @param x2 double The x component of the target vector.
* @param y2 double The y component of the target vector.
* @param z2 double The z component of the target vector.
* @return A reference to this {@link Matrix4} to facilitate chaining.
*/
@NonNull
public Matrix4 setToRotation(double x1, double y1, double z1, double x2, double y2, double z2) {
return setAll(mQuat.fromRotationBetween(x1, y1, z1, x2, y2, z2));
}
/**
* Sets this {@link Matrix4} to the rotation between two {@link Vector3} objects.
*
* @param v1 {@link Vector3} The base vector. Should be normalized.
* @param v2 {@link Vector3} The target vector. Should be normalized.
* @return A reference to this {@link Matrix4} to facilitate chaining.
*/
@NonNull
public Matrix4 setToRotation(@NonNull Vector3 v1, @NonNull Vector3 v2) {
return setAll(mQuat.fromRotationBetween(v1, v2));
}
/**
* Sets this {@link Matrix4} to the specified rotation around the specified axis.
*
* @param x double The x component of the axis of rotation.
* @param y double The y component of the axis of rotation.
* @param z double The z component of the axis of rotation.
* @param angle double The rotation angle.
* @return A reference to this {@link Matrix4} to facilitate chaining.
*/
@NonNull
public Matrix4 setToRotation(double x, double y, double z, double angle) {
return angle == 0 ? identity() : setAll(mQuat.fromAngleAxis(x, y, z, angle));
}
/**
* Sets this {@link Matrix4} to the specified rotation around the specified cardinal axis.
*
* @param axis {@link Axis} The axis of rotation.
* @param angle double The rotation angle in degrees.
* @return A reference to this {@link Matrix4} to facilitate chaining.
*/
@NonNull
public Matrix4 setToRotation(@NonNull Axis axis, double angle) {
return angle == 0 ? identity() : setAll(mQuat.fromAngleAxis(axis, angle));
}
/**
* Post multiplies this {@link Matrix4} with the rotation between the two provided {@link
* Vector3}s.
*
* @param v1 {@link Vector3} The base vector.
* @param v2 {@link Vector3} The target vector.
* @return A reference to this {@link Matrix4} to facilitate chaining.
*/
@NonNull
public Matrix4 rotate(@NonNull Vector3 v1, @NonNull Vector3 v2) {
return rotate(mQuat.fromRotationBetween(v1, v2));
}
/**
* Sets this {@link Matrix4} to the rotation specified by the provided Euler angles.
*
* @param yaw double The yaw angle in degrees.
* @param pitch double The pitch angle in degrees.
* @param roll double The roll angle in degrees.
* @return A reference to this {@link Matrix4} to facilitate chaining.
*/
@NonNull
public Matrix4 setToRotation(double yaw, double pitch, double roll) {
return setAll(mQuat.fromEuler(yaw, pitch, roll));
}
/**
* Sets this {@link Matrix4} to a look at matrix with a direction and up {@link Vector3}. You can
* multiply this with a translation {@link Matrix4} to get a camera Model-View matrix.
*
* @param direction {@link Vector3} The look direction.
* @param up {@link Vector3} The up axis.
* @return A reference to this {@link Matrix4} to facilitate chaining.
*/
@NonNull
public Matrix4 setToLookAt(@NonNull Vector3 direction, @NonNull Vector3 up) {
mQuat.lookAt(direction, up);
return setAll(mQuat);
}
/**
* Sets the elements of this {@link Matrix4} based on the rotation represented by the provided
* quaternion elements.
*
* @param w double The w component of the quaternion.
* @param x double The x component of the quaternion.
* @param y double The y component of the quaternion.
* @param z double The z component of the quaternion.
* @return A reference to this {@link Matrix4} to facilitate chaining.
*/
@NonNull
public Matrix4 setAll(double w, double x, double y, double z) {
return setAll(mQuat.setAll(w, x, y, z));
}
/**
* Sets the elements of this {@link Matrix4} based on the rotation represented by the provided
* {@link Quaternion}.
*
* @param quat {@link Quaternion} The {@link Quaternion} to represent.
* @return A reference to this {@link Matrix4} to facilitate chaining.
*/
@NonNull
public Matrix4 setAll(@NonNull Quaternion quat) {
quat.toRotationMatrix(m);
return this;
}
/**
* Multiplies the vector by the given {@link Quaternion}.
*
* @return This vector for chaining
*/
public Vector3 mul(final Quaternion quat) {
return quat.transform(this);
}
/**
* Post multiplies this {@link Matrix4} with the rotation specified by the provided axis and
* angle.
*
* @param x double The x component of the axis of rotation.
* @param y double The y component of the axis of rotation.
* @param z double The z component of the axis of rotation.
* @param angle double The angle of rotation in degrees.
* @return A reference to this {@link Matrix4} to facilitate chaining.
*/
@NonNull
public Matrix4 rotate(double x, double y, double z, double angle) {
return angle == 0 ? this : rotate(mQuat.fromAngleAxis(x, y, z, angle));
}
