public Quaternion2 Subtract(Quaternion2 q) { if ((this._coord != q._coord)) { q = q.ConvertTo(this._coord); } Quaternion2 m = new Quaternion2(this.Coord); m.W = _w - q.W; m.X = _x - q.X; m.Y = _y - q.Y; m.Z = _z - q.Z; return(m); }
public Quaternion2 Add(Quaternion2 q) { if ((this._coord != q._coord)) { q = q.ConvertTo(this._coord); } Quaternion2 m = new Quaternion2(this.Coord); m.W = _w + q.W; m.X = _x + q.X; m.Y = _y + q.Y; m.Z = _z + q.Z; return(m); }
public Quaternion2 Mult(Quaternion2 q) { if ((this._coord != q._coord)) { q = q.ConvertTo(this._coord); } Quaternion2 m = new Quaternion2(this.Coord); m.W = W * q.W - X * q.X - Y * q.Y - Z * q.Z; m.X = W * q.X + X * q.W + Y * q.Z - Z * q.Y; m.Y = W * q.Y - X * q.Z + Y * q.W + Z * q.X; m.Z = W * q.Z + X * q.Y - Y * q.X + Z * q.W; return(m); }
/// <summary> /// Spherical linear interpolation of two rotations. /// </summary> /// <param name="q1">Initial rotation</param> /// <param name="q2">Final rotation</param> /// <param name="t">Interpolation parameter within range [0, 1]</param> public static Quaternion2 SLERP(Quaternion2 q1, Quaternion2 q2, double t) { // Algorithm from https://en.wikipedia.org/wiki/Slerp Quaternion2 qq1 = q1.Copy().Normalized; Quaternion2 qq2 = q2.Copy().Normalized; if (qq2.Coord != qq1.Coord) { qq2 = qq2.ConvertTo(qq1.Coord); } double dot = qq1.W * qq2.W + qq1.X * qq2.X + qq1.Y * qq2.Y + qq1.Z * qq2.Z; const double threshold = 0.9995; if (Abs(dot) > threshold) { // Using linear interpolation if two rotations are close Quaternion2 res = qq1 + t * (qq2 - qq1); res.Normalize(); return(res); } //Make sure to choose shortest path if (dot < 0.0) { qq2 = -qq2; dot = -dot; } double theta_0 = Acos(dot); double theta = theta_0 * t; qq2 = qq2 - qq1 * dot; qq2.Normalize(); return(qq1 * Cos(theta) + qq2 * Sin(theta)); }