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;
}
