public void InitVelocityConstraint(float dt)
        {
            var body = GetComponent <PhysicsBody>();

            float pbc;

            ConstraintUtil.VelocityConstraintBias(body.Mass, ConstraintParams, dt, out pbc, out m_sbc);

            Vector3 r = transform.rotation * GetLocalAnchor();

            Vector3 cPos = (transform.position + r) - GetTarget();
            Vector3 cVel = body.LinearVelocity + Vector3.Cross(body.AngularVelocity, r);

            m_cross = Matrix3x3.PostCross(r);
            Matrix3x3 k = body.InverseMass * Matrix3x3.Identity;

            if (EnableRotation)
            {
                k += m_cross * body.InverseInertiaWs * m_cross.Transposed;
            }

            k += m_sbc * Matrix3x3.Identity;

            m_positionErrorBias = pbc * cPos;
            m_effectiveMass     = k.Inverted;

            // TODO: warm starting
            m_lambda = Vector3.zero;
        }
        public void InitVelocityConstraint(float dt)
        {
            var body = GetComponent <PhysicsBody>();

            float pbc;

            ConstraintUtil.VelocityConstraintBias(body.Mass, ConstraintParams, dt, out pbc, out sbc);

            m_n = (Plane != null) ? Plane.transform.up : Vector3.up;
            m_p = (Plane != null) ? Plane.transform.position : transform.position;
            m_d = Vector3.Dot(transform.position - m_p, m_n) - Offset;

            if (m_d > 0.0f)
            {
                return;
            }

            Vector3 cPos = m_d * m_n;

            m_positionErrorBias = pbc * cPos + Restitution * Vector3.Project(-body.LinearVelocity, m_n);
            m_effectiveMass     = 1.0f / (body.InverseMass + sbc);

            // TODO: warm starting
            m_impulse = Vector3.zero;
        }