/** * vraci Quat na zaklade Eulerovych uhlu rotace kolem jednotlivych os * * @param a uhel rotace kolem osy x * @param b uhel rotace kolem osy y * @param c uhel rotace kolem osy z * @return nova instance Quat */ public static Quat fromEulerAngles(double a, double b, double c) { /* * Quat Qi = new Quat(Math.cos(a/2),Math.sin(a/2),0,0); Quat Qj = new * Quat(Math.cos(b/2),0,Math.sin(b/2),0); Quat Qk = new * Quat(Math.cos(c/2),0,0,Math.sin(c/2)); */ Quat Qi = Quat.fromEulerAngle(a, 1, 0, 0); Quat Qj = Quat.fromEulerAngle(b, 0, 1, 0); Quat Qk = Quat.fromEulerAngle(c, 0, 0, 1); return new Quat(Qk.mul(Qj).mul(Qi)); // this.renorm(); }
@Override void computeTangent(KeyFrame prev, KeyFrame next) { tgPVec = Vec.multiply(Vec.subtract(next.position(), prev.position()), 0.5f); tgQuat = Quat.squadTangent( (Quat) prev.orientation(), (Quat) orientation(), (Quat) next.orientation()); }
/** * Interpolate {@link #frame()} at time {@code time} (expressed in seconds). {@link * #interpolationTime()} is set to {@code time} and {@link #frame()} is set accordingly. * * <p>If you simply want to change {@link #interpolationTime()} but not the {@link #frame()} * state, use {@link #setInterpolationTime(float)} instead. */ public void interpolateAtTime(float time) { this.checkValidity(); setInterpolationTime(time); if ((keyFrameList.isEmpty()) || (frame() == null)) return; if (!valuesAreValid) updateModifiedFrameValues(); updateCurrentKeyFrameForTime(time); if (!splineCacheIsValid) updateSplineCache(); float alpha; float dt = keyFrameList.get(currentFrame2.nextIndex()).time() - keyFrameList.get(currentFrame1.nextIndex()).time(); if (Util.zero(dt)) alpha = 0.0f; else alpha = (time - keyFrameList.get(currentFrame1.nextIndex()).time()) / dt; Vec pos = Vec.add( keyFrameList.get(currentFrame1.nextIndex()).position(), Vec.multiply( Vec.add( keyFrameList.get(currentFrame1.nextIndex()).tgP(), Vec.multiply(Vec.add(pv1, Vec.multiply(pv2, alpha)), alpha)), alpha)); float mag = Util.lerp( keyFrameList.get(currentFrame1.nextIndex()).magnitude(), keyFrameList.get(currentFrame2.nextIndex()).magnitude(), alpha); Rotation q; if (gScene.is3D()) { q = Quat.squad( (Quat) keyFrameList.get(currentFrame1.nextIndex()).orientation(), ((KeyFrame3D) keyFrameList.get(currentFrame1.nextIndex())).tgQ(), ((KeyFrame3D) keyFrameList.get(currentFrame2.nextIndex())).tgQ(), (Quat) keyFrameList.get(currentFrame2.nextIndex()).orientation(), alpha); } else { q = new Rot( Util.lerp( keyFrameList.get(currentFrame1.nextIndex()).orientation().angle(), keyFrameList.get(currentFrame2.nextIndex()).orientation().angle(), (alpha))); } frame().setPositionWithConstraint(pos); frame().setRotationWithConstraint(q); frame().setMagnitude(mag); }
/** * sfericka interpolace pomoci kvaternionu * * @param q kvaternion * @param t vaha z intervalu <0;1> * @return nova instance Poin3D */ public Quat slerp(Quat q, double t) { double c = this.dot(q); if (c > 1.0) c = 1.0; else if (c < -1.0) c = -1.0; double uhel = Math.acos(c); if (Math.abs(uhel) < 1.0e-5) return new Quat(this); double s = 1 / Math.sin(uhel); if (t >= 1) return new Quat(this); else if (t <= 0) return new Quat(q); else return new Quat( this.renorm() .mul(Math.sin((1 - t) * uhel) * s) .add(q.renorm().mul(Math.sin(t * uhel) * s))) .renorm(); }
public Quat squad2(Quat q1, Quat q2, Quat q3, double t) { Quat s1 = this.quadrangle(q1, q2); Quat s2 = q2.quadrangle(this, q3); return new Quat(this.slerp(q2, t).slerp(s1.slerp(s2, t), (double) (2 * t * (1 - t)))); }
private Quat quadrangle(Quat q1, Quat q2) { Quat s1 = this.inv().mul(q1); Quat s2 = this.inv().mul(q2); return new Quat((s1.log().add(s2.log()).mul(-1 / 4)).exp()); }
/** * kubicka interpolace pomoci kvaternionu * * @param q kvaternion * @param t vaha z intervalu <0;1> * @return nova instance Poin3D */ public Quat squad(Quat q, Quat q1, Quat q2, double t) { return new Quat(this.slerp(q, t).slerp(q1.slerp(q2, t), (double) (2 * t * (1 - t)))); }
/** * linearni interpolace pomoci kvaternionu Lerp(Q1,Q2,t)=(1-t)Q1+tQ2 * * @param q kvaternion * @param t vaha z intervalu <0;1> * @return nova instance Poin3D */ public Quat lerp(Quat q, double t) { if (t >= 1) return new Quat(q); else if (t <= 0) return new Quat(this); else return new Quat((this.mul(1 - t)).add(q.mul(t))); }
/** Intenal use. Call {@link #checkValidity()} and if path is not valid recomputes it. */ protected void updatePath() { checkValidity(); if (!pathIsValid) { path.clear(); int nbSteps = 30; if (keyFrameList.isEmpty()) return; if (!valuesAreValid) updateModifiedFrameValues(); if (keyFrameList.get(0) == keyFrameList.get(keyFrameList.size() - 1)) // TODO experimenting really path.add( new Frame( keyFrameList.get(0).position(), keyFrameList.get(0).orientation(), keyFrameList.get(0).magnitude())); else { KeyFrame[] kf = new KeyFrame[4]; kf[0] = keyFrameList.get(0); kf[1] = kf[0]; int index = 1; kf[2] = (index < keyFrameList.size()) ? keyFrameList.get(index) : null; index++; kf[3] = (index < keyFrameList.size()) ? keyFrameList.get(index) : null; while (kf[2] != null) { Vec pdiff = Vec.subtract(kf[2].position(), kf[1].position()); Vec pvec1 = Vec.add(Vec.multiply(pdiff, 3.0f), Vec.multiply(kf[1].tgP(), (-2.0f))); pvec1 = Vec.subtract(pvec1, kf[2].tgP()); Vec pvec2 = Vec.add(Vec.multiply(pdiff, (-2.0f)), kf[1].tgP()); pvec2 = Vec.add(pvec2, kf[2].tgP()); for (int step = 0; step < nbSteps; ++step) { Frame frame = new Frame(); float alpha = step / (float) nbSteps; frame.setPosition( Vec.add( kf[1].position(), Vec.multiply( Vec.add( kf[1].tgP(), Vec.multiply(Vec.add(pvec1, Vec.multiply(pvec2, alpha)), alpha)), alpha))); if (gScene.is3D()) { frame.setOrientation( Quat.squad( (Quat) kf[1].orientation(), ((KeyFrame3D) kf[1]).tgQ(), ((KeyFrame3D) kf[2]).tgQ(), (Quat) kf[2].orientation(), alpha)); } else { // linear interpolation float start = kf[1].orientation().angle(); float stop = kf[2].orientation().angle(); frame.setOrientation(new Rot(start + (stop - start) * alpha)); } frame.setMagnitude(Util.lerp(kf[1].magnitude(), kf[2].magnitude(), alpha)); path.add(frame.get()); } // Shift kf[0] = kf[1]; kf[1] = kf[2]; kf[2] = kf[3]; index++; kf[3] = (index < keyFrameList.size()) ? keyFrameList.get(index) : null; } // Add last KeyFrame path.add(new Frame(kf[1].position(), kf[1].orientation(), kf[1].magnitude())); } pathIsValid = true; } }