- public static Quaternion QuaternionSLERP(Quaternion qStart, Quaternion qEnd, double pos)
- {
- Quaternion Result = new Quaternion();
- Quaternion A = new Quaternion();
- A = qStart;
- A.Normalize();
- Quaternion B = new Quaternion();
- B= qEnd;
- B.Normalize();
- double cost = Vector3.VectorAngleCos(A.GetImagPart(), B.GetImagPart());
- bool bflip;
- if (cost < 0)
- {
- cost = -cost;
- bflip = true;
- }
- else
- {
- bflip = false;
- }
- double beta;
- if (1 - cost < 0.00000001)
- {
- beta = 1 - pos;
- }
- else
- {
- double theta = Math.Acos(cost);
- double phi = theta;
- double sint = Math.Sin(theta);
- beta = Math.Sin(theta - pos * phi) / sint;
- pos = Math.Sin(pos * phi) / sint;
- }
- if (bflip)
- {
- pos = -pos;
- }
- Result.X = beta * A.X + pos * B.X;
- Result.Y = beta * A.Y + pos * B.Y;
- Result.Z = beta * A.Z + pos * B.Z;
- Result.W = beta * A.W + pos * B.W;
- Result.Normalize();
- return Result;
- }
- }