private void Update() { if (Box == null) { return; } float dt = Time.deltaTime; Box.transform.rotation.ToAngleAxis(out float qAngle, out Vector3 qAxis); Vector3 c = Mathf.Deg2Rad * qAngle * qAxis; c.x = 0f; c.y = 0f; Matrix3x3 effectiveMass = inertia; Vector3 jV = a; jV.x = 0f; jV.y = 0f; Vector3 lambda = effectiveMass * (-(jV + (Beta / dt) * c)); a += inertiaInv * lambda; //a *= 0.9f; // temp magic cheat // integration Quaternion q = QuaternionUtil.AxisAngle(VectorUtil.NormalizeSafe(a, Vector3.forward), a.magnitude * dt); Box.transform.rotation = q * Box.transform.rotation; }
void Update() { Vector3 axis = Vector3.down; float angularSpeed = 1.0f; float dt = m_timeScale * Time.fixedDeltaTime; // render Quaternion baseRot = Quaternion.FromToRotation(Vector3.one, Vector3.up); DebugUtil.DrawBox(Vector3.zero, m_quatClosedForm * baseRot, Vector3.one, m_closedColor, true, DebugUtil.Style.FlatShaded); DebugUtil.DrawBox(Vector3.zero, m_quatPower * baseRot, 1.05f * Vector3.one, m_powerColor, true, DebugUtil.Style.Wireframe); DebugUtil.DrawBox(Vector3.zero, m_quatDerivative * baseRot, 1.1f * Vector3.one, m_derivativeColor, true, DebugUtil.Style.Wireframe); // integrate Quaternion v = QuaternionUtil.AxisAngle(axis, angularSpeed); Vector3 o = angularSpeed * axis; m_quatClosedForm = QuaternionUtil.Normalize(QuaternionUtil.AxisAngle(axis, (m_angle += angularSpeed * dt))); m_quatPower = QuaternionUtil.Normalize(QuaternionUtil.Integrate(m_quatPower, v, dt)); m_quatDerivative = QuaternionUtil.Normalize(QuaternionUtil.Integrate(m_quatDerivative, o, dt)); if (Input.GetKey(KeyCode.Space)) { Reset(); } }
public void Integrate(float dt) { if (!LockPosition) { transform.position += LinearVelocity * dt;; } if (!LockRotation) { Quaternion q = QuaternionUtil.AxisAngle(VectorUtil.NormalizeSafe(AngularVelocity, Vector3.forward), AngularVelocity.magnitude * dt); transform.rotation = q * transform.rotation; } }
private void Update() { if (Object == null) { return; } if (Target == null) { return; } float dt = Time.deltaTime; Vector3 r = Object.transform.rotation * rLocal; // gravity v += Gravity * dt; // constraint errors Vector3 cPos = (Object.transform.position + r) - Target.transform.position; Vector3 cVel = v + Vector3.Cross(a, r); // constraint resolution Matrix3x3 s = Matrix3x3.Skew(-r); Matrix3x3 k = massInv * Matrix3x3.Identity + s * inertiaInv * s.Transposed; Matrix3x3 effectiveMass = k.Inverted; Vector3 lambda = effectiveMass * (-(cVel + (Beta / dt) * cPos)); // velocity correction v += massInv * lambda; a += (inertiaInv * s.Transposed) * lambda; v *= 0.98f; // temp magic cheat a *= 0.98f; // temp magic cheat // integration Object.transform.position += v * dt; Quaternion q = QuaternionUtil.AxisAngle(VectorUtil.NormalizeSafe(a, Vector3.forward), a.magnitude * dt); Object.transform.rotation = q * Object.transform.rotation; }