diff --git a/src/02-quaternion/Quaternion.ts b/src/02-quaternion/Quaternion.ts index bbff27552bb2726e7f65403db15ddd82e437f726..fc1aeacfb40fe7c92b82fbc61b8702aa27c8fcf8 100644 --- a/src/02-quaternion/Quaternion.ts +++ b/src/02-quaternion/Quaternion.ts @@ -102,45 +102,46 @@ export class Quaternion { return Quaternion.add(Quaternion.multiplyScalar(qa, 1 - t), Quaternion.multiplyScalar(qb, t)); } else { const theta = Math.acos(cosTheta); - const thetap = theta * t; - const qperp = Quaternion.normalize(Quaternion.subtract(qb, Quaternion.multiplyScalar(qa, cosTheta))); - return Quaternion.add(Quaternion.multiplyScalar(qa, Math.cos(thetap)), Quaternion.multiplyScalar(qperp, Math.sin(thetap))); + const sinTheta = Math.sin(theta); + const sinThetaA = Math.sin(theta * (1 - t)); + const sinThetaB = Math.sin(theta * t); + return Quaternion.add(Quaternion.multiplyScalar(qa, sinThetaA / sinTheta), Quaternion.multiplyScalar(qb, sinThetaB / sinTheta)); } } - // public static slerp(qa: Quaternion, qb: Quaternion, t: number): Quaternion { + public static slerpCG3(qa: Quaternion, qb: Quaternion, t: number): Quaternion { - // let cq1 = qa.clone(); - // let cq2 = qb.clone(); + let cq1 = qa.clone(); + let cq2 = qb.clone(); - // cq1 = Quaternion.normalize(cq1); - // cq2 = Quaternion.normalize(cq2); + cq1 = Quaternion.normalize(cq1); + cq2 = Quaternion.normalize(cq2); - // let dot = Quaternion.dot(cq1, cq2); + let dot = Quaternion.dot(cq1, cq2); - // if (dot < 0.0) { - // cq2 = Quaternion.inverse(cq2); - // dot = -dot; - // } + if (dot < 0.0) { + cq2 = Quaternion.inverse(cq2); + dot = -dot; + } - // if (dot > 0.9995) { - // return Quaternion.add(cq1, Quaternion.multiplyScalar( - // Quaternion.subtract(cq2, cq1), t)); - // } + if (dot > 0.9995) { + return Quaternion.add(cq1, Quaternion.multiplyScalar( + Quaternion.subtract(cq2, cq1), t)); + } - // const theta0 = Math.acos(dot); - // const theta = theta0 * t; + const theta0 = Math.acos(dot); + const theta = theta0 * t; - // const sinTheta0 = Math.sin(theta0); - // const sinTheta = Math.sin(theta); - // const s0 = Math.cos(theta0) - dot * sinTheta / sinTheta0; - // const s1 = sinTheta / sinTheta0; + const sinTheta0 = Math.sin(theta0); + const sinTheta = Math.sin(theta); + const s0 = Math.cos(theta0) - dot * sinTheta / sinTheta0; + const s1 = sinTheta / sinTheta0; - // return Quaternion.add( - // Quaternion.multiplyScalar(cq1, s0), - // Quaternion.multiplyScalar(cq2, s1) - // ) - // } + return Quaternion.add( + Quaternion.multiplyScalar(cq1, s0), + Quaternion.multiplyScalar(cq2, s1) + ) + } public static add(qa: Quaternion, qb: Quaternion): Quaternion { return new Quaternion(qa._x + qb._x, qa._y + qb._y, qa._z + qb._z, qa._w + qb._w);