Calculate proper center of mass shift for rigid body. This makes eg. ragdolls work better.

Fixed CollisionShape & Constraint constantly triggering scale update on parented rigid bodies due to minor inaccuracy in world scale.
Tweaked ragdolls.

Bin/Data/Scripts/TestScene.as

@@ -489,28 +489,28 @@ void CreateRagdoll(AnimatedModel@ model)
 {
     Node@ root = model.node;
 
-    CreateRagdollBone(root, "Bip01_Pelvis", SHAPE_BOX, Vector3(0.2, 0.25, 0.2), Vector3(0.15, 0, 0), Quaternion(0, 0, 0));
-    CreateRagdollBone(root, "Bip01_Spine1", SHAPE_BOX, Vector3(0.3, 0.2, 0.25), Vector3(0.15, 0, 0), Quaternion(0, 0, 0));
+    CreateRagdollBone(root, "Bip01_Pelvis", SHAPE_BOX, Vector3(0.3, 0.2, 0.25), Vector3(0, 0, 0), Quaternion(0, 0, 0));
+    CreateRagdollBone(root, "Bip01_Spine1", SHAPE_BOX, Vector3(0.35, 0.2, 0.3), Vector3(0.15, 0, 0), Quaternion(0, 0, 0));
     CreateRagdollBone(root, "Bip01_L_Thigh", SHAPE_CAPSULE, Vector3(0.175, 0.45, 0.175), Vector3(0.25, 0, 0), Quaternion(0, 0, 90));
     CreateRagdollBone(root, "Bip01_R_Thigh", SHAPE_CAPSULE, Vector3(0.175, 0.45, 0.175), Vector3(0.25, 0, 0), Quaternion(0, 0, 90));
     CreateRagdollBone(root, "Bip01_L_Calf", SHAPE_CAPSULE, Vector3(0.15, 0.55, 0.15), Vector3(0.25, 0, 0), Quaternion(0, 0, 90));
     CreateRagdollBone(root, "Bip01_R_Calf", SHAPE_CAPSULE, Vector3(0.15, 0.55, 0.15), Vector3(0.25, 0, 0), Quaternion(0, 0, 90));
-    CreateRagdollBone(root, "Bip01_Head", SHAPE_SPHERE, Vector3(0.25, 0.25, 0.25), Vector3(0.1, 0, 0), Quaternion(0, 0, 0));
-    CreateRagdollBone(root, "Bip01_L_UpperArm", SHAPE_CAPSULE, Vector3(0.125, 0.35, 0.125), Vector3(0.1, 0, 0), Quaternion(0, 0, 90));
-    CreateRagdollBone(root, "Bip01_R_UpperArm", SHAPE_CAPSULE, Vector3(0.125, 0.35, 0.125), Vector3(0.1, 0, 0), Quaternion(0, 0, 90));
-    CreateRagdollBone(root, "Bip01_L_Forearm", SHAPE_CAPSULE, Vector3(0.1, 0.3, 0.1), Vector3(0.15, 0, 0), Quaternion(0, 0, 90));
-    CreateRagdollBone(root, "Bip01_R_Forearm", SHAPE_CAPSULE, Vector3(0.1, 0.3, 0.1), Vector3(0.15, 0, 0), Quaternion(0, 0, 90));
+    CreateRagdollBone(root, "Bip01_Head", SHAPE_BOX, Vector3(0.2, 0.2, 0.2), Vector3(0.1, 0, 0), Quaternion(0, 0, 0));
+    CreateRagdollBone(root, "Bip01_L_UpperArm", SHAPE_CAPSULE, Vector3(0.15, 0.35, 0.15), Vector3(0.1, 0, 0), Quaternion(0, 0, 90));
+    CreateRagdollBone(root, "Bip01_R_UpperArm", SHAPE_CAPSULE, Vector3(0.15, 0.35, 0.15), Vector3(0.1, 0, 0), Quaternion(0, 0, 90));
+    CreateRagdollBone(root, "Bip01_L_Forearm", SHAPE_CAPSULE, Vector3(0.125, 0.4, 0.125), Vector3(0.2, 0, 0), Quaternion(0, 0, 90));
+    CreateRagdollBone(root, "Bip01_R_Forearm", SHAPE_CAPSULE, Vector3(0.125, 0.4, 0.125), Vector3(0.2, 0, 0), Quaternion(0, 0, 90));
     
-    CreateRagdollConstraint(root, "Bip01_L_Thigh", "Bip01_Pelvis", CONSTRAINT_CONETWIST, Vector3(0, 0, -1), Vector3(0, 0, 1), Vector2(35, 25), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_R_Thigh", "Bip01_Pelvis", CONSTRAINT_CONETWIST, Vector3(0, 0, -1), Vector3(0, 0, 1), Vector2(35, 25), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_L_Calf", "Bip01_L_Thigh", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(45, 0), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_R_Calf", "Bip01_R_Thigh", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(45, 0), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_Spine1", "Bip01_Pelvis", CONSTRAINT_HINGE, Vector3(0, 0, 1), Vector3(0, 0, 1), Vector2(25, 0), Vector2(-10, 0));
-    CreateRagdollConstraint(root, "Bip01_Head", "Bip01_Spine1", CONSTRAINT_CONETWIST, Vector3(0, 0, 1), Vector3(0, 0, 1), Vector2(35, 25), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_L_UpperArm", "Bip01_Spine1", CONSTRAINT_CONETWIST, Vector3(0, -1, 0), Vector3(0, 1, 0), Vector2(35, 25), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_R_UpperArm", "Bip01_Spine1", CONSTRAINT_CONETWIST, Vector3(0, -1, 0), Vector3(0, 1, 0), Vector2(35, 25), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_L_Forearm", "Bip01_L_UpperArm", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(45, 0), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_R_Forearm", "Bip01_R_UpperArm", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(45, 0), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_L_Thigh", "Bip01_Pelvis", CONSTRAINT_CONETWIST, Vector3(0, 0, -1), Vector3(0, 0, 1), Vector2(45, 45), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_R_Thigh", "Bip01_Pelvis", CONSTRAINT_CONETWIST, Vector3(0, 0, -1), Vector3(0, 0, 1), Vector2(45, 45), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_L_Calf", "Bip01_L_Thigh", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(90, 0), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_R_Calf", "Bip01_R_Thigh", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(90, 0), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_Spine1", "Bip01_Pelvis", CONSTRAINT_HINGE, Vector3(0, 0, 1), Vector3(0, 0, 1), Vector2(45, 0), Vector2(-10, 0));
+    CreateRagdollConstraint(root, "Bip01_Head", "Bip01_Spine1", CONSTRAINT_CONETWIST, Vector3(1, 0, 0), Vector3(1, 0, 0), Vector2(0, 30), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_L_UpperArm", "Bip01_Spine1", CONSTRAINT_CONETWIST, Vector3(0, -1, 0), Vector3(0, 1, 0), Vector2(45, 45), Vector2(0, 0), false);
+    CreateRagdollConstraint(root, "Bip01_R_UpperArm", "Bip01_Spine1", CONSTRAINT_CONETWIST, Vector3(0, -1, 0), Vector3(0, 1, 0), Vector2(45, 45), Vector2(0, 0), false);
+    CreateRagdollConstraint(root, "Bip01_L_Forearm", "Bip01_L_UpperArm", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(90, 0), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_R_Forearm", "Bip01_R_UpperArm", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(90, 0), Vector2(0, 0));
 
     // Disable animation from all bones (both physical and non-physical) to not interfere
     Skeleton@ skel = model.skeleton;
@@ -543,7 +543,8 @@ void CreateRagdollBone(Node@ root, const String&in boneName, ShapeType type, con
 }
 
 void CreateRagdollConstraint(Node@ root, const String&in boneName, const String&in parentName, ConstraintType type,
-    const Vector3&in axis, const Vector3&in parentAxis, const Vector2&in highLimit, const Vector2&in lowLimit)
+    const Vector3&in axis, const Vector3&in parentAxis, const Vector2&in highLimit, const Vector2&in lowLimit,
+    bool disableCollision = true)
 {
     Node@ boneNode = root.GetChild(boneName, true);
     Node@ parentNode = root.GetChild(parentName, true);
@@ -552,7 +553,7 @@ void CreateRagdollConstraint(Node@ root, const String&in boneName, const String&
 
     Constraint@ constraint = boneNode.CreateComponent("Constraint", LOCAL);
     constraint.constraintType = type;
-    constraint.disableCollision = true;
+    constraint.disableCollision = disableCollision;
     // The connected body must be specified before setting the world position
     constraint.otherBody = parentNode.GetComponent("RigidBody");
     constraint.worldPosition = boneNode.worldPosition;

Bin/Data/Scripts/TestSceneOld.as

@@ -628,28 +628,28 @@ void CreateRagdoll(AnimatedModel@ model)
 {
     Node@ root = model.node;
 
-    CreateRagdollBone(root, "Bip01_Pelvis", SHAPE_BOX, Vector3(0.2, 0.25, 0.2), Vector3(0.15, 0, 0), Quaternion(0, 0, 0));
-    CreateRagdollBone(root, "Bip01_Spine1", SHAPE_BOX, Vector3(0.3, 0.2, 0.25), Vector3(0.15, 0, 0), Quaternion(0, 0, 0));
+    CreateRagdollBone(root, "Bip01_Pelvis", SHAPE_BOX, Vector3(0.3, 0.2, 0.25), Vector3(0, 0, 0), Quaternion(0, 0, 0));
+    CreateRagdollBone(root, "Bip01_Spine1", SHAPE_BOX, Vector3(0.35, 0.2, 0.3), Vector3(0.15, 0, 0), Quaternion(0, 0, 0));
     CreateRagdollBone(root, "Bip01_L_Thigh", SHAPE_CAPSULE, Vector3(0.175, 0.45, 0.175), Vector3(0.25, 0, 0), Quaternion(0, 0, 90));
     CreateRagdollBone(root, "Bip01_R_Thigh", SHAPE_CAPSULE, Vector3(0.175, 0.45, 0.175), Vector3(0.25, 0, 0), Quaternion(0, 0, 90));
     CreateRagdollBone(root, "Bip01_L_Calf", SHAPE_CAPSULE, Vector3(0.15, 0.55, 0.15), Vector3(0.25, 0, 0), Quaternion(0, 0, 90));
     CreateRagdollBone(root, "Bip01_R_Calf", SHAPE_CAPSULE, Vector3(0.15, 0.55, 0.15), Vector3(0.25, 0, 0), Quaternion(0, 0, 90));
-    CreateRagdollBone(root, "Bip01_Head", SHAPE_SPHERE, Vector3(0.25, 0.25, 0.25), Vector3(0.1, 0, 0), Quaternion(0, 0, 0));
-    CreateRagdollBone(root, "Bip01_L_UpperArm", SHAPE_CAPSULE, Vector3(0.125, 0.35, 0.125), Vector3(0.1, 0, 0), Quaternion(0, 0, 90));
-    CreateRagdollBone(root, "Bip01_R_UpperArm", SHAPE_CAPSULE, Vector3(0.125, 0.35, 0.125), Vector3(0.1, 0, 0), Quaternion(0, 0, 90));
-    CreateRagdollBone(root, "Bip01_L_Forearm", SHAPE_CAPSULE, Vector3(0.1, 0.3, 0.1), Vector3(0.15, 0, 0), Quaternion(0, 0, 90));
-    CreateRagdollBone(root, "Bip01_R_Forearm", SHAPE_CAPSULE, Vector3(0.1, 0.3, 0.1), Vector3(0.15, 0, 0), Quaternion(0, 0, 90));
+    CreateRagdollBone(root, "Bip01_Head", SHAPE_BOX, Vector3(0.2, 0.2, 0.2), Vector3(0.1, 0, 0), Quaternion(0, 0, 0));
+    CreateRagdollBone(root, "Bip01_L_UpperArm", SHAPE_CAPSULE, Vector3(0.15, 0.35, 0.15), Vector3(0.1, 0, 0), Quaternion(0, 0, 90));
+    CreateRagdollBone(root, "Bip01_R_UpperArm", SHAPE_CAPSULE, Vector3(0.15, 0.35, 0.15), Vector3(0.1, 0, 0), Quaternion(0, 0, 90));
+    CreateRagdollBone(root, "Bip01_L_Forearm", SHAPE_CAPSULE, Vector3(0.125, 0.4, 0.125), Vector3(0.2, 0, 0), Quaternion(0, 0, 90));
+    CreateRagdollBone(root, "Bip01_R_Forearm", SHAPE_CAPSULE, Vector3(0.125, 0.4, 0.125), Vector3(0.2, 0, 0), Quaternion(0, 0, 90));
     
-    CreateRagdollConstraint(root, "Bip01_L_Thigh", "Bip01_Pelvis", CONSTRAINT_CONETWIST, Vector3(0, 0, -1), Vector3(0, 0, 1), Vector2(35, 25), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_R_Thigh", "Bip01_Pelvis", CONSTRAINT_CONETWIST, Vector3(0, 0, -1), Vector3(0, 0, 1), Vector2(35, 25), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_L_Calf", "Bip01_L_Thigh", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(45, 0), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_R_Calf", "Bip01_R_Thigh", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(45, 0), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_Spine1", "Bip01_Pelvis", CONSTRAINT_HINGE, Vector3(0, 0, 1), Vector3(0, 0, 1), Vector2(25, 0), Vector2(-10, 0));
-    CreateRagdollConstraint(root, "Bip01_Head", "Bip01_Spine1", CONSTRAINT_CONETWIST, Vector3(0, 0, 1), Vector3(0, 0, 1), Vector2(35, 25), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_L_UpperArm", "Bip01_Spine1", CONSTRAINT_CONETWIST, Vector3(0, -1, 0), Vector3(0, 1, 0), Vector2(35, 25), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_R_UpperArm", "Bip01_Spine1", CONSTRAINT_CONETWIST, Vector3(0, -1, 0), Vector3(0, 1, 0), Vector2(35, 25), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_L_Forearm", "Bip01_L_UpperArm", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(45, 0), Vector2(0, 0));
-    CreateRagdollConstraint(root, "Bip01_R_Forearm", "Bip01_R_UpperArm", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(45, 0), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_L_Thigh", "Bip01_Pelvis", CONSTRAINT_CONETWIST, Vector3(0, 0, -1), Vector3(0, 0, 1), Vector2(45, 45), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_R_Thigh", "Bip01_Pelvis", CONSTRAINT_CONETWIST, Vector3(0, 0, -1), Vector3(0, 0, 1), Vector2(45, 45), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_L_Calf", "Bip01_L_Thigh", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(90, 0), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_R_Calf", "Bip01_R_Thigh", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(90, 0), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_Spine1", "Bip01_Pelvis", CONSTRAINT_HINGE, Vector3(0, 0, 1), Vector3(0, 0, 1), Vector2(45, 0), Vector2(-10, 0));
+    CreateRagdollConstraint(root, "Bip01_Head", "Bip01_Spine1", CONSTRAINT_CONETWIST, Vector3(1, 0, 0), Vector3(1, 0, 0), Vector2(0, 30), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_L_UpperArm", "Bip01_Spine1", CONSTRAINT_CONETWIST, Vector3(0, -1, 0), Vector3(0, 1, 0), Vector2(45, 45), Vector2(0, 0), false);
+    CreateRagdollConstraint(root, "Bip01_R_UpperArm", "Bip01_Spine1", CONSTRAINT_CONETWIST, Vector3(0, -1, 0), Vector3(0, 1, 0), Vector2(45, 45), Vector2(0, 0), false);
+    CreateRagdollConstraint(root, "Bip01_L_Forearm", "Bip01_L_UpperArm", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(90, 0), Vector2(0, 0));
+    CreateRagdollConstraint(root, "Bip01_R_Forearm", "Bip01_R_UpperArm", CONSTRAINT_HINGE, Vector3(0, 0, -1), Vector3(0, 0, -1), Vector2(90, 0), Vector2(0, 0));
 
     // Disable animation from all bones (both physical and non-physical) to not interfere
     Skeleton@ skel = model.skeleton;
@@ -682,7 +682,8 @@ void CreateRagdollBone(Node@ root, const String&in boneName, ShapeType type, con
 }
 
 void CreateRagdollConstraint(Node@ root, const String&in boneName, const String&in parentName, ConstraintType type,
-    const Vector3&in axis, const Vector3&in parentAxis, const Vector2&in highLimit, const Vector2&in lowLimit)
+    const Vector3&in axis, const Vector3&in parentAxis, const Vector2&in highLimit, const Vector2&in lowLimit,
+    bool disableCollision = true)
 {
     Node@ boneNode = root.GetChild(boneName, true);
     Node@ parentNode = root.GetChild(parentName, true);
@@ -691,7 +692,7 @@ void CreateRagdollConstraint(Node@ root, const String&in boneName, const String&
 
     Constraint@ constraint = boneNode.CreateComponent("Constraint", LOCAL);
     constraint.constraintType = type;
-    constraint.disableCollision = true;
+    constraint.disableCollision = disableCollision;
     // The connected body must be specified before setting the world position
     constraint.otherBody = parentNode.GetComponent("RigidBody");
     constraint.worldPosition = boneNode.worldPosition;

Engine/Physics/CollisionShape.cpp

@@ -762,7 +762,7 @@ void CollisionShape::OnNodeSet(Node* node)
 void CollisionShape::OnMarkedDirty(Node* node)
 {
     Vector3 newWorldScale = node_->GetWorldScale();
-    if (!newWorldScale.Equals(cachedWorldScale_) && shape_)
+    if (HasWorldScaleChanged(cachedWorldScale_, newWorldScale) && shape_)
     {
         // Physics operations are not safe from worker threads
         Scene* scene = GetScene();

Engine/Physics/Constraint.cpp

@@ -413,7 +413,7 @@ void Constraint::OnNodeSet(Node* node)
 void Constraint::OnMarkedDirty(Node* node)
 {
     /// \todo This does not catch the connected body node's scale changing
-    if (!node->GetWorldScale().Equals(cachedWorldScale_))
+    if (HasWorldScaleChanged(cachedWorldScale_, node->GetWorldScale()))
         ApplyFrames();
 }
 

Engine/Physics/PhysicsUtils.h

@@ -51,4 +51,11 @@ inline Quaternion ToQuaternion(const btQuaternion& quaternion)
     return Quaternion(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z());
 }
 
+inline bool HasWorldScaleChanged(const Vector3& oldWorldScale, const Vector3& newWorldScale)
+{
+    Vector3 delta = newWorldScale - oldWorldScale;
+    float dot = delta.DotProduct(delta);
+    return dot > 0.01f;
+}
+
 }

Engine/Physics/RigidBody.cpp

@@ -65,7 +65,9 @@ RigidBody::RigidBody(Context* context) :
     Component(context),
     body_(0),
     compoundShape_(0),
+    shiftedCompoundShape_(0),
     gravityOverride_(Vector3::ZERO),
+    centerOfMassShift_(Vector3::ZERO),
     mass_(DEFAULT_MASS),
     collisionLayer_(DEFAULT_COLLISION_LAYER),
     collisionMask_(DEFAULT_COLLISION_MASK),
@@ -80,6 +82,7 @@ RigidBody::RigidBody(Context* context) :
     inWorld_(false)
 {
     compoundShape_ = new btCompoundShape();
+    shiftedCompoundShape_ = new btCompoundShape();
 }
 
 RigidBody::~RigidBody()
@@ -91,6 +94,8 @@ RigidBody::~RigidBody()
 
     delete compoundShape_;
     compoundShape_ = 0;
+    delete shiftedCompoundShape_;
+    shiftedCompoundShape_ = 0;
 }
 
 void RigidBody::RegisterObject(Context* context)
@@ -98,8 +103,8 @@ void RigidBody::RegisterObject(Context* context)
     context->RegisterFactory<RigidBody>(PHYSICS_CATEGORY);
 
     ACCESSOR_ATTRIBUTE(RigidBody, VAR_BOOL, "Is Enabled", IsEnabled, SetEnabled, bool, true, AM_DEFAULT);
-    ACCESSOR_ATTRIBUTE(RigidBody, VAR_VECTOR3, "Physics Position", GetPosition, SetPosition, Vector3, Vector3::ZERO, AM_FILE | AM_NOEDIT);
     ACCESSOR_ATTRIBUTE(RigidBody, VAR_QUATERNION, "Physics Rotation", GetRotation, SetRotation, Quaternion, Quaternion::IDENTITY, AM_FILE | AM_NOEDIT);
+    ACCESSOR_ATTRIBUTE(RigidBody, VAR_VECTOR3, "Physics Position", GetPosition, SetPosition, Vector3, Vector3::ZERO, AM_FILE | AM_NOEDIT);
     ATTRIBUTE(RigidBody, VAR_FLOAT, "Mass", mass_, DEFAULT_MASS, AM_DEFAULT);
     ACCESSOR_ATTRIBUTE(RigidBody, VAR_FLOAT, "Friction", GetFriction, SetFriction, float, DEFAULT_FRICTION, AM_DEFAULT);
     ACCESSOR_ATTRIBUTE(RigidBody, VAR_FLOAT, "Restitution", GetRestitution, SetRestitution, float, DEFAULT_RESTITUTION, AM_DEFAULT);
@@ -157,15 +162,15 @@ void RigidBody::getWorldTransform(btTransform &worldTrans) const
     {
         lastPosition_ = node_->GetWorldPosition();
         lastRotation_ = node_->GetWorldRotation();
-        worldTrans.setOrigin(ToBtVector3(lastPosition_));
+        worldTrans.setOrigin(ToBtVector3(lastPosition_) + ToBtVector3(lastRotation_ * centerOfMassShift_));
         worldTrans.setRotation(ToBtQuaternion(lastRotation_));
     }
 }
 
 void RigidBody::setWorldTransform(const btTransform &worldTrans)
 {
-    Vector3 newWorldPosition = ToVector3(worldTrans.getOrigin());
     Quaternion newWorldRotation = ToQuaternion(worldTrans.getRotation());
+    Vector3 newWorldPosition = ToVector3(worldTrans.getOrigin()) - newWorldRotation * centerOfMassShift_;
     RigidBody* parentRigidBody = 0;
 
     // It is possible that the RigidBody component has been kept alive via a shared pointer,
@@ -202,7 +207,7 @@ void RigidBody::DrawDebugGeometry(DebugRenderer* debug, bool depthTest)
         physicsWorld_->SetDebugDepthTest(depthTest);
 
         btDiscreteDynamicsWorld* world = physicsWorld_->GetWorld();
-        world->debugDrawObject(body_->getWorldTransform(), compoundShape_, IsActive() ? btVector3(1.0f, 1.0f, 1.0f) :
+        world->debugDrawObject(body_->getWorldTransform(), shiftedCompoundShape_, IsActive() ? btVector3(1.0f, 1.0f, 1.0f) :
             btVector3(0.0f, 1.0f, 0.0f));
 
         physicsWorld_->SetDebugRenderer(0);
@@ -226,8 +231,8 @@ void RigidBody::SetPosition(Vector3 position)
     if (body_)
     {
         btTransform& worldTrans = body_->getWorldTransform();
-        worldTrans.setOrigin(ToBtVector3(position));
-
+        worldTrans.setOrigin(ToBtVector3(position + ToQuaternion(worldTrans.getRotation()) * centerOfMassShift_));
+        
         // When forcing the physics position, set also interpolated position so that there is no jitter
         btTransform interpTrans = body_->getInterpolationWorldTransform();
         interpTrans.setOrigin(worldTrans.getOrigin());
@@ -262,9 +267,9 @@ void RigidBody::SetTransform(const Vector3& position, const Quaternion& rotation
     if (body_)
     {
         btTransform& worldTrans = body_->getWorldTransform();
-        worldTrans.setOrigin(ToBtVector3(position));
         worldTrans.setRotation(ToBtQuaternion(rotation));
-
+        worldTrans.setOrigin(ToBtVector3(position) + ToBtVector3(rotation * centerOfMassShift_));
+        
         // When forcing the physics position, set also interpolated position so that there is no jitter
         btTransform interpTrans = body_->getInterpolationWorldTransform();
         interpTrans.setOrigin(worldTrans.getOrigin());
@@ -546,7 +551,10 @@ void RigidBody::Activate()
 Vector3 RigidBody::GetPosition() const
 {
     if (body_)
-        return ToVector3(body_->getWorldTransform().getOrigin());
+    {
+        const btTransform& transform = body_->getWorldTransform();
+        return ToVector3(transform.getOrigin()) - ToQuaternion(transform.getRotation()) * centerOfMassShift_;
+    }
     else
         return Vector3::ZERO;
 }
@@ -710,9 +718,44 @@ void RigidBody::UpdateMass()
 {
     if (body_)
     {
+        btTransform principal;
+        principal.setRotation(btQuaternion::getIdentity());
+        principal.setOrigin(btVector3(0.0f, 0.0f, 0.0f));
+        
+        // Calculate center of mass shift from all the collision shapes
+        unsigned numShapes = compoundShape_->getNumChildShapes();
+        if (numShapes)
+        {
+            PODVector<float> masses(numShapes);
+            for (unsigned i = 0; i < numShapes; ++i)
+            {
+                // The actual mass does not matter, divide evenly between child shapes
+                masses[i] = 1.0f;
+            }
+            
+            btVector3 inertia(0.0f, 0.0f, 0.0f);
+            compoundShape_->calculatePrincipalAxisTransform(&masses[0], principal, inertia);
+        }
+        
+        // Add child shapes to shifted compound shape with adjusted offset
+        while (shiftedCompoundShape_->getNumChildShapes())
+            shiftedCompoundShape_->removeChildShapeByIndex(0);
+        for (unsigned i = 0; i < numShapes; ++i)
+        {
+            btTransform adjusted = compoundShape_->getChildTransform(i);
+            adjusted.setOrigin(adjusted.getOrigin() - principal.getOrigin());
+            shiftedCompoundShape_->addChildShape(adjusted, compoundShape_->getChildShape(i));
+        }
+        
+        // Reapply rigid body position with new center of mass shift
+        Vector3 oldPosition = GetPosition();
+        centerOfMassShift_ = ToVector3(principal.getOrigin());
+        SetPosition(oldPosition);
+        
+        // Calculate final inertia
         btVector3 localInertia(0.0f, 0.0f, 0.0f);
         if (mass_ > 0.0f)
-            compoundShape_->calculateLocalInertia(mass_, localInertia);
+            shiftedCompoundShape_->calculateLocalInertia(mass_, localInertia);
         body_->setMassProps(mass_, localInertia);
         body_->updateInertiaTensor();
     }
@@ -807,16 +850,16 @@ void RigidBody::OnMarkedDirty(Node* node)
         Vector3 newPosition = node_->GetWorldPosition();
         Quaternion newRotation = node_->GetWorldRotation();
 
-        if (!newPosition.Equals(lastPosition_))
-        {
-            lastPosition_ = newPosition;
-            SetPosition(newPosition);
-        }
         if (!newRotation.Equals(lastRotation_))
         {
             lastRotation_ = newRotation;
             SetRotation(newRotation);
         }
+        if (!newPosition.Equals(lastPosition_))
+        {
+            lastPosition_ = newPosition;
+            SetPosition(newPosition);
+        }
     }
 }
 
@@ -863,7 +906,7 @@ void RigidBody::AddBodyToWorld()
     {
         // Correct inertia will be calculated below
         btVector3 localInertia(0.0f, 0.0f, 0.0f);
-        body_ = new btRigidBody(mass_, this, compoundShape_, localInertia);
+        body_ = new btRigidBody(mass_, this, shiftedCompoundShape_, localInertia);
         body_->setUserPointer(this);
 
         // Check for existence of the SmoothedTransform component, which should be created by now in network client mode.

Engine/Physics/RigidBody.h

@@ -236,12 +236,16 @@ private:
     btRigidBody* body_;
     /// Bullet compound collision shape.
     btCompoundShape* compoundShape_;
+    /// Bullet shifted compound collision shape.
+    btCompoundShape* shiftedCompoundShape_;
     /// Physics world.
     WeakPtr<PhysicsWorld> physicsWorld_;
     /// Constraints that refer to this rigid body.
     PODVector<Constraint*> constraints_;
     /// Gravity override vector.
     Vector3 gravityOverride_;
+    /// Center of mass shift vector.
+    Vector3 centerOfMassShift_;
     /// Mass.
     float mass_;
     /// Attribute buffer for network replication.