BEPUphysics.CollisionShapes.CompoundShape.TransformContribution C# (CSharp) Method

        public static void TransformContribution(ref RigidTransform transform, ref Vector3 center, ref Matrix3x3 baseContribution, float weight, out Matrix3x3 contribution)
        {
            Matrix3x3 rotation;
            Matrix3x3.CreateFromQuaternion(ref transform.Orientation, out rotation);
            Matrix3x3 temp;

            //TODO: Verify contribution

            //Do angular transformed contribution first...
            Matrix3x3.MultiplyTransposed(ref rotation, ref baseContribution, out temp);
            Matrix3x3.Multiply(ref temp, ref rotation, out temp);

            contribution = temp;

            //Now add in the offset from the origin.
            Vector3 offset;
            Vector3.Subtract(ref transform.Position, ref center, out offset);
            Matrix3x3 innerProduct;
            Matrix3x3.CreateScale(offset.LengthSquared(), out innerProduct);
            Matrix3x3 outerProduct;
            Matrix3x3.CreateOuterProduct(ref offset, ref offset, out outerProduct);

            Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out temp);

            Matrix3x3.Add(ref contribution, ref temp, out contribution);
            Matrix3x3.Multiply(ref contribution, weight, out contribution);

        }