public static void DrawArc(Vector3 center, Vector3 from, Vector3 normal, float angle, float radius, int numSegments, Color color, bool depthTest = true) { if (numSegments <= 0) { return; } from.Normalize(); from *= radius; var aVert = new Vector3[numSegments + 1]; aVert[0] = center + from; float numSegmentsInv = 1.0f / numSegments; Quaternion rotStep = QuaternionUtil.AxisAngle(normal, angle * numSegmentsInv); Vector3 vec = rotStep * from; for (int i = 1; i <= numSegments; ++i) { aVert[i] = center + vec; vec = rotStep * vec; } DrawLineStrip(aVert, color, depthTest); }
// master sterp function public static Quaternion Sterp ( Quaternion a, Quaternion b, Vector3 twistAxis, float tSwing, float tTwist, SterpMode mode, out Quaternion swing, out Quaternion twist ) { Quaternion q = b * Quaternion.Inverse(a); Quaternion swingFull; Quaternion twistFull; QuaternionUtil.DecomposeSwingTwist(q, twistAxis, out swingFull, out twistFull); switch (mode) { default: case SterpMode.Nlerp: swing = Nlerp(Quaternion.identity, swingFull, tSwing); twist = Nlerp(Quaternion.identity, twistFull, tTwist); break; case SterpMode.Slerp: swing = Quaternion.Slerp(Quaternion.identity, swingFull, tSwing); twist = Quaternion.Slerp(Quaternion.identity, twistFull, tTwist); break; } return(twist * swing); }
protected override void Draw(Color color, DebugUtil.Style style, bool depthTest) { Quaternion startRot = QuaternionUtil.AxisAngle(Vector3.forward, StartAngle * MathUtil.Deg2Rad); DebugUtil.DrawArc ( transform.position, transform.rotation * startRot * Vector3.right, transform.rotation * Vector3.forward, ArcAngle * MathUtil.Deg2Rad, Radius, NumSegments, color, depthTest ); }
public Quaternion TrackExponential(Quaternion targetValue, float halfLife, float deltaTime) { if (halfLife < MathUtil.Epsilon) { VelocityVec = QuaternionUtil.ToVector4(Quaternion.identity); ValueVec = QuaternionUtil.ToVector4(targetValue); return(targetValue); } float angularFrequency = 0.6931472f / halfLife; float dampingRatio = 1.0f; return(TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime)); }
public Quaternion TrackHalfLife(Quaternion targetValue, float frequencyHz, float halfLife, float deltaTime) { if (halfLife < MathUtil.Epsilon) { VelocityVec = QuaternionUtil.ToVector4(Quaternion.identity); ValueVec = QuaternionUtil.ToVector4(targetValue); return(targetValue); } float angularFrequency = frequencyHz * MathUtil.TwoPi; float dampingRatio = 0.6931472f / (angularFrequency * halfLife); return(TrackDampingRatio(targetValue, angularFrequency, dampingRatio, deltaTime)); }
public Quaternion TrackDampingRatio(Quaternion targetValue, float angularFrequency, float dampingRatio, float deltaTime) { if (angularFrequency < MathUtil.Epsilon) { VelocityVec = QuaternionUtil.ToVector4(Quaternion.identity); return(QuaternionUtil.FromVector4(ValueVec)); } Vector4 targetValueVec = QuaternionUtil.ToVector4(targetValue); // keep in same hemisphere for shorter track delta if (Vector4.Dot(ValueVec, targetValueVec) < 0.0f) { targetValueVec = -targetValueVec; } Vector4 delta = targetValueVec - ValueVec; float f = 1.0f + 2.0f * deltaTime * dampingRatio * angularFrequency; float oo = angularFrequency * angularFrequency; float hoo = deltaTime * oo; float hhoo = deltaTime * hoo; float detInv = 1.0f / (f + hhoo); Vector4 detX = f * ValueVec + deltaTime * VelocityVec + hhoo * targetValueVec; Vector4 detV = VelocityVec + hoo * delta; VelocityVec = detV * detInv; ValueVec = detX * detInv; if (VelocityVec.magnitude < MathUtil.Epsilon && delta.magnitude < MathUtil.Epsilon) { VelocityVec = QuaternionUtil.ToVector4(Quaternion.identity);; ValueVec = targetValueVec; } return(QuaternionUtil.FromVector4(ValueVec)); }
public void Reset(Quaternion initValue, Quaternion initVelocity) { ValueVec = QuaternionUtil.ToVector4(initValue); VelocityVec = QuaternionUtil.ToVector4(initVelocity); }
public void Reset(Quaternion initValue) { ValueVec = QuaternionUtil.ToVector4(initValue); VelocityVec = Vector4.zero; }
public void Reset() { ValueVec = QuaternionUtil.ToVector4(Quaternion.identity); VelocityVec = Vector4.zero; }