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 = VectorUtil.Integrate(transform.position, LinearVelocity, dt);
}
if (!LockRotation)
{
transform.rotation = QuaternionUtil.Integrate(transform.rotation, AngularVelocity, dt);
}
}
private void FixedUpdate()
{
if (Object == null)
{
return;
}
if (Target == null)
{
return;
}
float dt = Time.fixedDeltaTime;
Vector3 r = Object.transform.rotation * rLocal;
var t = Object.transform;
Matrix3x3 world2Local =
Matrix3x3.FromRows
(
t.TransformVector(new Vector3(1.0f, 0.0f, 0.0f)),
t.TransformVector(new Vector3(0.0f, 1.0f, 0.0f)),
t.TransformVector(new Vector3(0.0f, 0.0f, 1.0f))
);
Matrix3x3 inertiaInvWs = world2Local.Transposed * inertiaInvLs * world2Local;
// 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 * inertiaInvWs * s.Transposed;
Matrix3x3 effectiveMass = k.Inverted;
Vector3 lambda = effectiveMass * (-(cVel + (Beta / dt) * cPos));
// velocity correction
v += massInv * lambda;
a += (inertiaInvWs * s.Transposed) * lambda;
v *= 0.98f; // temp magic
a *= 0.98f; // temp magic
// integration
Object.transform.position += v * dt;
Object.transform.rotation = QuaternionUtil.Integrate(Object.transform.rotation, a, dt);
}