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(); }