public static double3x3 quatToMatrix(quaternion_d q)
{
q = math.normalize(q);
// Precalculate coordinate products
double x = q.value.x * 2.0F;
double y = q.value.y * 2.0F;
double z = q.value.z * 2.0F;
double xx = q.value.x * x;
double yy = q.value.y * y;
double zz = q.value.z * z;
double xy = q.value.x * y;
double xz = q.value.x * z;
double yz = q.value.y * z;
double wx = q.value.w * x;
double wy = q.value.w * y;
double wz = q.value.w * z;
// Calculate 3x3 matrix from orthonormal basis
var m = new double3x3
{
m0 = new double3(1.0f - (yy + zz), xy + wz, xz - wy),
m1 = new double3(xy - wz, 1.0f - (xx + zz), yz + wx),
m2 = new double3(xz + wy, yz - wx, 1.0f - (xx + yy))
};
return(m);
}
public static quaternion_d mul(quaternion_d lhs, quaternion_d rhs)
{
return(new quaternion_d(
lhs.value.w * rhs.value.x + lhs.value.x * rhs.value.w + lhs.value.y * rhs.value.z - lhs.value.z * rhs.value.y,
lhs.value.w * rhs.value.y + lhs.value.y * rhs.value.w + lhs.value.z * rhs.value.x - lhs.value.x * rhs.value.z,
lhs.value.w * rhs.value.z + lhs.value.z * rhs.value.w + lhs.value.x * rhs.value.y - lhs.value.y * rhs.value.x,
lhs.value.w * rhs.value.w - lhs.value.x * rhs.value.x - lhs.value.y * rhs.value.y - lhs.value.z * rhs.value.z));
}
public static quaternion_d normalize(quaternion_d q)
{
double4 value = q.value;
double len = math.dot(value, value);
// note we use float4 comparison here because this gives us -1 / 0 which is necessary for select.
//return select(quatIdentity(), q*rsqrt(len), len > float4(epsilon_normal()));
value = math.select_d(quaternion.identity.value, value * math.rsqrt(len), len > math.epsilon_normal);
return(new quaternion_d(value));
}
public static double4x4 rottrans(quaternion_d q, double3 t)
{
var m3x3 = quatToMatrix(q);
var m = new double4x4
{
m0 = new double4(m3x3.m0, 0.0f),
m1 = new double4(m3x3.m1, 0.0f),
m2 = new double4(m3x3.m2, 0.0f),
m3 = new double4(t, 1.0f)
};
return(m);
}
public static double3 mul(quaternion_d rotation, double3 position)
{
double x = rotation.value.x * 2F;
double y = rotation.value.y * 2F;
double z = rotation.value.z * 2F;
double xx = rotation.value.x * x;
double yy = rotation.value.y * y;
double zz = rotation.value.z * z;
double xy = rotation.value.x * y;
double xz = rotation.value.x * z;
double yz = rotation.value.y * z;
double wx = rotation.value.w * x;
double wy = rotation.value.w * y;
double wz = rotation.value.w * z;
double3 res;
res.x = (1F - (yy + zz)) * position.x + (xy - wz) * position.y + (xz + wy) * position.z;
res.y = (xy + wz) * position.x + (1F - (xx + zz)) * position.y + (yz - wx) * position.z;
res.z = (xz - wy) * position.x + (yz + wx) * position.y + (1F - (xx + yy)) * position.z;
return(res);
}
public static double3 up(quaternion_d q)
{
return(mul(q, new double3(0, 1, 0)));
}
public static double3 forward(quaternion_d q)
{
return(mul(q, new double3(0, 0, 1)));
}
public static quaternion_d lerp(quaternion_d lhs, quaternion_d rhs, double t)
{
throw new System.NotImplementedException();
// var res = math.normalize(lhs.value + t * (math.chgsign(rhs.value, math.dot(lhs.value, rhs.value)) - rhs.value));
// return new quaternion_d(res);
}
public static quaternion_d slerp(quaternion_d lhs, quaternion_d rhs, double t)
{
throw new System.NotImplementedException();
}
