BEPUphysics.BroadPhaseEntries.MobileCollidables.CompoundHelper.RemoveReposition C# (CSharp) Method

        static void RemoveReposition(Entity compound, ref ShapeDistributionInformation distributionInfo, float weight, float removedWeight, ref Vector3 removedCenter)
        {
            //The compounds are not aligned with the original's position yet.
            //In order to align them, first look at the centers the split method computed.
            //They are offsets from the center of the original shape in local space.
            //These can be used to reposition the objects in world space.
            Vector3 weightedA, weightedB;
            Vector3.Multiply(ref distributionInfo.Center, weight, out weightedA);
            Vector3.Multiply(ref removedCenter, removedWeight, out weightedB);
            Vector3 newLocalCenter;
            Vector3.Add(ref weightedA, ref weightedB, out newLocalCenter);
            Vector3.Divide(ref newLocalCenter, weight + removedWeight, out newLocalCenter);

            Vector3 localOffset;
            Vector3.Subtract(ref distributionInfo.Center, ref newLocalCenter, out localOffset);

            Vector3 originalPosition = compound.position;

            Vector3 offset = Vector3.Transform(localOffset, compound.orientation);
            compound.Position = originalPosition + offset;

            Vector3 originalLinearVelocity = compound.linearVelocity;
            Vector3 originalAngularVelocity = compound.angularVelocity;
            compound.AngularVelocity = originalAngularVelocity;
            compound.LinearVelocity = originalLinearVelocity + Vector3.Cross(originalAngularVelocity, offset);
        }