public Quaternion(Mathd.Quaternion quat)
{
vector_ = new Mathd.Vector3();
scalar_ = quat.w;
vector_.x = quat.x;
vector_.y = quat.y;
vector_.z = quat.z;
}
// Rotates a Vector3 by a Quaterion
public Mathd.Vector3 Rotate(Mathd.Quaternion quatRotation)
{
Mathd.Matrix3 roatationMatrix = new Mathd.Matrix3(
1 - (2 * (quatRotation.y * quatRotation.y)) - (2 * (quatRotation.z * quatRotation.z)),
2 * ((quatRotation.x * quatRotation.y) + (quatRotation.w * quatRotation.z)),
2 * ((quatRotation.x * quatRotation.z) - (quatRotation.w * quatRotation.y)),
2 * ((quatRotation.x * quatRotation.y) - (quatRotation.w * quatRotation.z)),
1 - (2 * (quatRotation.x * quatRotation.x)) - (2 * (quatRotation.z * quatRotation.z)),
2 * ((quatRotation.x * quatRotation.z) + (quatRotation.w * quatRotation.y)),
2 * ((quatRotation.x * quatRotation.z) + (quatRotation.w * quatRotation.y)),
2 * ((quatRotation.y * quatRotation.z) - (quatRotation.w * quatRotation.x)),
1 - (2 * (quatRotation.x * quatRotation.x)) - (2 * (quatRotation.y * quatRotation.y))
);
float newX = (roatationMatrix.col0[0] + roatationMatrix.col0[1] + roatationMatrix.col0[2]) * x;
float newY = (roatationMatrix.col1[0] + roatationMatrix.col1[1] + roatationMatrix.col1[2]) * y;
float newZ = (roatationMatrix.col2[0] + roatationMatrix.col2[1] + roatationMatrix.col2[2]) * x;
return(new Mathd.Vector3(newX, newY, newZ));
}
// Multiplication
// (lhs and rhs need to be oposite way around for left handed ?)
static public Mathd.DualQuaternion operator *(Mathd.DualQuaternion lhs, Mathd.DualQuaternion rhs)
{
Mathd.Quaternion newRealPart = rhs.real * lhs.real;
Mathd.Quaternion newDualPart = (rhs.real * lhs.dual) + (rhs.dual * lhs.real);
return(new Mathd.DualQuaternion(newRealPart, newDualPart));
}
// Scalar Multiplication
static public Mathd.DualQuaternion operator *(Mathd.DualQuaternion dualQuat, float scalar)
{
Mathd.Quaternion newRealPart = dualQuat.real * scalar;
Mathd.Quaternion newDualPart = dualQuat.dual * scalar;
return(new Mathd.DualQuaternion(newRealPart, newDualPart));
}
// public DualQuaternion (UnityEngine.Vector3 position, UnityEngine.Quaternion rotation) {
// Mathd.Vector3 mathdPos = new Mathd.Vector3(position.x, position.y, position.z);
// Mathd.Quaternion mathdQuat = new Mathd.Quaternion(rotation.w, rotation.x, rotation.y, rotation.z);
// }
// ARETHMETIC OPERATIONS //
// Addition
static public Mathd.DualQuaternion operator +(Mathd.DualQuaternion lhs, Mathd.DualQuaternion rhs)
{
Mathd.Quaternion newRealPart = lhs.real + rhs.real;
Mathd.Quaternion newDualPart = lhs.dual + rhs.dual;
return(new Mathd.DualQuaternion(newRealPart, newDualPart));
}
// This is the one to use for building a transform
// Based on example from: http://wscg.zcu.cz/wscg2012/short/A29-full.pdf
public DualQuaternion(Mathd.Vector3 position, Mathd.Quaternion rotation)
{
real = rotation.normalised;
dual = 0.5f * (new Mathd.Quaternion(0, position) * real);
}
public DualQuaternion(float real_w, float real_x, float real_y, float real_z,
float dual_w, float dual_x, float dual_y, float dual_z)
{
real = new Mathd.Quaternion(real_x, real_y, real_z, real_w).normalised;
dual = new Mathd.Quaternion(dual_x, dual_y, dual_z, dual_w);
}
public DualQuaternion(Quaternion real_part, Quaternion dual_part)
{
// real = real_part * (1 / real_part.magnitude);
real = real_part.normalised;
dual = dual_part;
}
// CONSTRUCTORS //
public DualQuaternion()
{
real = new Quaternion(1, 0, 0, 0);
dual = new Quaternion(0, 0, 0, 0);
}
// Cross Product (Left handed)
// Note: Not the same as the quaternion product
static public Mathd.Quaternion CrossProduct(Mathd.Quaternion lhs, Mathd.Quaternion rhs)
{
return(new Mathd.Quaternion(0, Mathd.Vector3.CrossProduct(lhs.vector, rhs.vector)));
}
// Dot Product
// The angle between two quaternions
// If the same quaternion is used both sides = magnitude of the quaternion, squared
static public float DotProduct(Mathd.Quaternion lhs, Mathd.Quaternion rhs)
{
return((lhs.scalar * rhs.scalar) + Mathd.Vector3.DotProduct(lhs.vector, rhs.vector));
// (lhs.w * rhs.w) + (lhs.x * rhs.x) + (lhs.y * rhs.y) + (lhs.z + rhs.z);
}
