Allow to query rigid body's center of mass.

When setting constraint positions, take center of mass offset into account.
Reapply constraint frames when the center of mass offset changes.

Bin/Data/Scripts/TestScene.as

@@ -530,8 +530,8 @@ void CreateRagdollBone(Node@ root, const String&in boneName, ShapeType type, con
     // any components, it is important that the LOCAL creation mode is specified.
     RigidBody@ body = boneNode.CreateComponent("RigidBody", LOCAL);
     body.mass = 1.0;
-    body.linearDamping = 0.2;
-    body.angularDamping = 0.9;
+    body.linearDamping = 0.05;
+    body.angularDamping = 0.85;
     body.linearRestThreshold = 1.5;
     body.angularRestThreshold = 2.5;
 

Bin/Data/Scripts/TestSceneOld.as

@@ -669,8 +669,8 @@ void CreateRagdollBone(Node@ root, const String&in boneName, ShapeType type, con
     // any components, it is important that the LOCAL creation mode is specified.
     RigidBody@ body = boneNode.CreateComponent("RigidBody", LOCAL);
     body.mass = 1.0;
-    body.linearDamping = 0.2;
-    body.angularDamping = 0.9;
+    body.linearDamping = 0.05;
+    body.angularDamping = 0.85;
     body.linearRestThreshold = 1.5;
     body.angularRestThreshold = 2.5;
 

Engine/Engine/PhysicsAPI.cpp

@@ -161,6 +161,7 @@ static void RegisterRigidBody(asIScriptEngine* engine)
     engine->RegisterObjectMethod("RigidBody", "bool get_useGravity() const", asMETHOD(RigidBody, GetUseGravity), asCALL_THISCALL);
     engine->RegisterObjectMethod("RigidBody", "void set_gravityOverride(const Vector3&in)", asMETHOD(RigidBody, SetGravityOverride), asCALL_THISCALL);
     engine->RegisterObjectMethod("RigidBody", "const Vector3& get_gravityOverride() const", asMETHOD(RigidBody, GetGravityOverride), asCALL_THISCALL);
+    engine->RegisterObjectMethod("RigidBody", "const Vector3& get_centerOfMass() const", asMETHOD(RigidBody, GetCenterOfMass), asCALL_THISCALL);
     engine->RegisterObjectMethod("RigidBody", "void set_phantom(bool)", asMETHOD(RigidBody, SetPhantom), asCALL_THISCALL);
     engine->RegisterObjectMethod("RigidBody", "bool get_phantom() const", asMETHOD(RigidBody, IsPhantom), asCALL_THISCALL);
     engine->RegisterObjectMethod("RigidBody", "void set_kinematic(bool)", asMETHOD(RigidBody, SetKinematic), asCALL_THISCALL);

Engine/Physics/CollisionShape.cpp

@@ -671,7 +671,7 @@ BoundingBox CollisionShape::GetWorldBoundingBox() const
         return BoundingBox();
 }
 
-void CollisionShape::NotifyRigidBody()
+void CollisionShape::NotifyRigidBody(bool updateMass)
 {
     btCompoundShape* compound = GetParentCompoundShape();
     if (node_ && shape_ && compound)
@@ -697,7 +697,8 @@ void CollisionShape::NotifyRigidBody()
         }
         
         // Finally tell the rigid body to update its mass
-        rigidBody_->UpdateMass();
+        if (updateMass)
+            rigidBody_->UpdateMass();
     }
 }
 

Engine/Physics/CollisionShape.h

@@ -200,8 +200,8 @@ public:
     /// Return world-space bounding box.
     BoundingBox GetWorldBoundingBox() const;
     
-    /// Update the new collision shape to the RigidBody, and tell it to update its mass.
-    void NotifyRigidBody();
+    /// Update the new collision shape to the RigidBody.
+    void NotifyRigidBody(bool updateMass = true);
     /// Set model attribute.
     void SetModelAttr(ResourceRef value);
     /// Return model attribute.

Engine/Physics/Constraint.cpp

@@ -112,7 +112,7 @@ void Constraint::OnSetAttribute(const AttributeInfo& attr, const Variant& src)
         if (attr.offset_ == offsetof(Constraint, position_) && constraint_ && !otherBody_)
         {
             btTransform ownBody = constraint_->getRigidBodyA().getWorldTransform();
-            btVector3 worldPos = ownBody * ToBtVector3(position_ * cachedWorldScale_);
+            btVector3 worldPos = ownBody * ToBtVector3(position_ * cachedWorldScale_ - ownBody_->GetCenterOfMass());
             otherPosition_ = ToVector3(worldPos);
         }
         
@@ -293,10 +293,13 @@ void Constraint::SetWorldPosition(const Vector3& position)
         btTransform ownBodyInverse = constraint_->getRigidBodyA().getWorldTransform().inverse();
         btTransform otherBodyInverse = constraint_->getRigidBodyB().getWorldTransform().inverse();
         btVector3 worldPos = ToBtVector3(position);
-        position_ = ToVector3(ownBodyInverse * worldPos) / cachedWorldScale_;
+        position_ = (ToVector3(ownBodyInverse * worldPos) + ownBody_->GetCenterOfMass()) / cachedWorldScale_;
         otherPosition_ = ToVector3(otherBodyInverse * worldPos);
         if (otherBody_)
+        {
+            otherPosition_ += otherBody_->GetCenterOfMass();
             otherPosition_ /= otherBody_->GetNode()->GetWorldScale();
+        }
         ApplyFrames();
         MarkNetworkUpdate();
     }
@@ -363,7 +366,7 @@ Vector3 Constraint::GetWorldPosition() const
     if (constraint_)
     {
         btTransform ownBody = constraint_->getRigidBodyA().getWorldTransform();
-        return ToVector3(ownBody * ToBtVector3(position_ * cachedWorldScale_));
+        return ToVector3(ownBody * ToBtVector3(position_ * cachedWorldScale_ - ownBody_->GetCenterOfMass()));
     }
     else
         return Vector3::ZERO;
@@ -386,6 +389,60 @@ void Constraint::ReleaseConstraint()
     }
 }
 
+void Constraint::ApplyFrames()
+{
+    if (!constraint_)
+        return;
+    
+    if (node_)
+        cachedWorldScale_ = node_->GetWorldScale();
+    
+    Vector3 ownBodyScaledPosition = position_ * cachedWorldScale_ - ownBody_->GetCenterOfMass();
+    Vector3 otherBodyScaledPosition = otherBody_ ? otherPosition_ * otherBody_->GetNode()->GetWorldScale() -
+        otherBody_->GetCenterOfMass() : otherPosition_;
+    
+    switch (constraint_->getConstraintType())
+    {
+    case POINT2POINT_CONSTRAINT_TYPE:
+        {
+            btPoint2PointConstraint* pointConstraint = static_cast<btPoint2PointConstraint*>(constraint_);
+            pointConstraint->setPivotA(ToBtVector3(ownBodyScaledPosition));
+            pointConstraint->setPivotB(ToBtVector3(otherBodyScaledPosition));
+        }
+        break;
+        
+    case HINGE_CONSTRAINT_TYPE:
+        {
+            btHingeConstraint* hingeConstraint = static_cast<btHingeConstraint*>(constraint_);
+            btTransform ownFrame(ToBtQuaternion(rotation_), ToBtVector3(ownBodyScaledPosition));
+            btTransform otherFrame(ToBtQuaternion(otherRotation_), ToBtVector3(otherBodyScaledPosition));
+            hingeConstraint->setFrames(ownFrame, otherFrame);
+        }
+        break;
+        
+    case SLIDER_CONSTRAINT_TYPE:
+        {
+            btSliderConstraint* sliderConstraint = static_cast<btSliderConstraint*>(constraint_);
+            btTransform ownFrame(ToBtQuaternion(rotation_), ToBtVector3(ownBodyScaledPosition));
+            btTransform otherFrame(ToBtQuaternion(otherRotation_), ToBtVector3(otherBodyScaledPosition));
+            sliderConstraint->setFrames(ownFrame, otherFrame);
+        }
+        break;
+        
+    case CONETWIST_CONSTRAINT_TYPE:
+        {
+            btConeTwistConstraint* coneTwistConstraint = static_cast<btConeTwistConstraint*>(constraint_);
+            btTransform ownFrame(ToBtQuaternion(rotation_), ToBtVector3(ownBodyScaledPosition));
+            btTransform otherFrame(ToBtQuaternion(otherRotation_), ToBtVector3(otherBodyScaledPosition));
+            coneTwistConstraint->setFrames(ownFrame, otherFrame);
+        }
+        break;
+
+    default:
+        break;
+    }
+}
+
 void Constraint::OnNodeSet(Node* node)
 {
     if (node)
@@ -435,9 +492,9 @@ void Constraint::CreateConstraint()
     if (!otherBody)
         otherBody = &btTypedConstraint::getFixedBody();
     
-    Vector3 ownBodyScaledPosition = position_ * cachedWorldScale_;
-    Vector3 otherBodyScaledPosition = otherBody_ ? otherPosition_ * otherBody_->GetNode()->GetWorldScale() :
-        otherPosition_;
+    Vector3 ownBodyScaledPosition = position_ * cachedWorldScale_ - ownBody_->GetCenterOfMass();
+    Vector3 otherBodyScaledPosition = otherBody_ ? otherPosition_ * otherBody_->GetNode()->GetWorldScale() -
+        otherBody_->GetCenterOfMass() : otherPosition_;
     
     switch (constraintType_)
     {
@@ -493,60 +550,6 @@ void Constraint::CreateConstraint()
     framesDirty_ = false;
 }
 
-void Constraint::ApplyFrames()
-{
-    if (!constraint_)
-        return;
-    
-    if (node_)
-        cachedWorldScale_ = node_->GetWorldScale();
-    
-    Vector3 ownBodyScaledPosition = position_ * cachedWorldScale_;
-    Vector3 otherBodyScaledPosition = otherBody_ ? otherPosition_ * otherBody_->GetNode()->GetWorldScale() :
-        otherPosition_;
-    
-    switch (constraint_->getConstraintType())
-    {
-    case POINT2POINT_CONSTRAINT_TYPE:
-        {
-            btPoint2PointConstraint* pointConstraint = static_cast<btPoint2PointConstraint*>(constraint_);
-            pointConstraint->setPivotA(ToBtVector3(ownBodyScaledPosition));
-            pointConstraint->setPivotB(ToBtVector3(otherBodyScaledPosition));
-        }
-        break;
-        
-    case HINGE_CONSTRAINT_TYPE:
-        {
-            btHingeConstraint* hingeConstraint = static_cast<btHingeConstraint*>(constraint_);
-            btTransform ownFrame(ToBtQuaternion(rotation_), ToBtVector3(ownBodyScaledPosition));
-            btTransform otherFrame(ToBtQuaternion(otherRotation_), ToBtVector3(otherBodyScaledPosition));
-            hingeConstraint->setFrames(ownFrame, otherFrame);
-        }
-        break;
-        
-    case SLIDER_CONSTRAINT_TYPE:
-        {
-            btSliderConstraint* sliderConstraint = static_cast<btSliderConstraint*>(constraint_);
-            btTransform ownFrame(ToBtQuaternion(rotation_), ToBtVector3(ownBodyScaledPosition));
-            btTransform otherFrame(ToBtQuaternion(otherRotation_), ToBtVector3(otherBodyScaledPosition));
-            sliderConstraint->setFrames(ownFrame, otherFrame);
-        }
-        break;
-        
-    case CONETWIST_CONSTRAINT_TYPE:
-        {
-            btConeTwistConstraint* coneTwistConstraint = static_cast<btConeTwistConstraint*>(constraint_);
-            btTransform ownFrame(ToBtQuaternion(rotation_), ToBtVector3(ownBodyScaledPosition));
-            btTransform otherFrame(ToBtQuaternion(otherRotation_), ToBtVector3(otherBodyScaledPosition));
-            coneTwistConstraint->setFrames(ownFrame, otherFrame);
-        }
-        break;
-
-    default:
-        break;
-    }
-}
-
 void Constraint::ApplyLimits()
 {
     if (!constraint_)

Engine/Physics/Constraint.h

@@ -130,6 +130,8 @@ public:
     
     /// Release the constraint.
     void ReleaseConstraint();
+    /// Apply constraint frames.
+    void ApplyFrames();
     
 protected:
     /// Handle node being assigned.
@@ -140,8 +142,6 @@ protected:
 private:
     /// Create the constraint.
     void CreateConstraint();
-    /// Apply constraint frames.
-    void ApplyFrames();
     /// Apply high and low constraint limits.
     void ApplyLimits();
     

Engine/Physics/RigidBody.cpp

@@ -67,7 +67,7 @@ RigidBody::RigidBody(Context* context) :
     compoundShape_(0),
     shiftedCompoundShape_(0),
     gravityOverride_(Vector3::ZERO),
-    centerOfMassShift_(Vector3::ZERO),
+    centerOfMass_(Vector3::ZERO),
     mass_(DEFAULT_MASS),
     collisionLayer_(DEFAULT_COLLISION_LAYER),
     collisionMask_(DEFAULT_COLLISION_MASK),
@@ -162,7 +162,7 @@ void RigidBody::getWorldTransform(btTransform &worldTrans) const
     {
         lastPosition_ = node_->GetWorldPosition();
         lastRotation_ = node_->GetWorldRotation();
-        worldTrans.setOrigin(ToBtVector3(lastPosition_) + ToBtVector3(lastRotation_ * centerOfMassShift_));
+        worldTrans.setOrigin(ToBtVector3(lastPosition_ + lastRotation_ * centerOfMass_));
         worldTrans.setRotation(ToBtQuaternion(lastRotation_));
     }
 }
@@ -170,7 +170,7 @@ void RigidBody::getWorldTransform(btTransform &worldTrans) const
 void RigidBody::setWorldTransform(const btTransform &worldTrans)
 {
     Quaternion newWorldRotation = ToQuaternion(worldTrans.getRotation());
-    Vector3 newWorldPosition = ToVector3(worldTrans.getOrigin()) - newWorldRotation * centerOfMassShift_;
+    Vector3 newWorldPosition = ToVector3(worldTrans.getOrigin()) - newWorldRotation * centerOfMass_;
     RigidBody* parentRigidBody = 0;
 
     // It is possible that the RigidBody component has been kept alive via a shared pointer,
@@ -231,7 +231,7 @@ void RigidBody::SetPosition(Vector3 position)
     if (body_)
     {
         btTransform& worldTrans = body_->getWorldTransform();
-        worldTrans.setOrigin(ToBtVector3(position + ToQuaternion(worldTrans.getRotation()) * centerOfMassShift_));
+        worldTrans.setOrigin(ToBtVector3(position + ToQuaternion(worldTrans.getRotation()) * centerOfMass_));
         
         // When forcing the physics position, set also interpolated position so that there is no jitter
         btTransform interpTrans = body_->getInterpolationWorldTransform();
@@ -268,7 +268,7 @@ void RigidBody::SetTransform(const Vector3& position, const Quaternion& rotation
     {
         btTransform& worldTrans = body_->getWorldTransform();
         worldTrans.setRotation(ToBtQuaternion(rotation));
-        worldTrans.setOrigin(ToBtVector3(position) + ToBtVector3(rotation * centerOfMassShift_));
+        worldTrans.setOrigin(ToBtVector3(position + rotation * centerOfMass_));
         
         // When forcing the physics position, set also interpolated position so that there is no jitter
         btTransform interpTrans = body_->getInterpolationWorldTransform();
@@ -553,7 +553,7 @@ Vector3 RigidBody::GetPosition() const
     if (body_)
     {
         const btTransform& transform = body_->getWorldTransform();
-        return ToVector3(transform.getOrigin()) - ToQuaternion(transform.getRotation()) * centerOfMassShift_;
+        return ToVector3(transform.getOrigin()) - ToQuaternion(transform.getRotation()) * centerOfMass_;
     }
     else
         return Vector3::ZERO;
@@ -749,7 +749,7 @@ void RigidBody::UpdateMass()
         
         // Reapply rigid body position with new center of mass shift
         Vector3 oldPosition = GetPosition();
-        centerOfMassShift_ = ToVector3(principal.getOrigin());
+        centerOfMass_ = ToVector3(principal.getOrigin());
         SetPosition(oldPosition);
         
         // Calculate final inertia
@@ -758,6 +758,15 @@ void RigidBody::UpdateMass()
             shiftedCompoundShape_->calculateLocalInertia(mass_, localInertia);
         body_->setMassProps(mass_, localInertia);
         body_->updateInertiaTensor();
+        
+        // Reapply constraint positions for new center of mass shift
+        if (node_)
+        {
+            PODVector<Constraint*> constraints;
+            node_->GetComponents<Constraint>(constraints);
+            for (PODVector<Constraint*>::Iterator i = constraints.Begin(); i != constraints.End(); ++i)
+                (*i)->ApplyFrames();
+        }
     }
 }
 
@@ -852,10 +861,12 @@ void RigidBody::OnMarkedDirty(Node* node)
 
         if (!newRotation.Equals(lastRotation_))
         {
+            // Due to possible center of mass offset, if rotation changes, update position also
             lastRotation_ = newRotation;
-            SetRotation(newRotation);
+            lastPosition_ = newPosition;
+            SetTransform(newPosition, newRotation);
         }
-        if (!newPosition.Equals(lastPosition_))
+        else if (!newPosition.Equals(lastPosition_))
         {
             lastPosition_ = newPosition;
             SetPosition(newPosition);
@@ -898,8 +909,6 @@ void RigidBody::AddBodyToWorld()
     if (mass_ < 0.0f)
         mass_ = 0.0f;
 
-    bool massUpdated = false;
-
     if (body_)
         RemoveBodyFromWorld();
     else
@@ -920,14 +929,11 @@ void RigidBody::AddBodyToWorld()
         }
 
         // Check if CollisionShapes already exist in the node and add them to the compound shape.
-        // Note: NotifyRigidBody() will cause mass to be updated
+        // Do not update mass yet, but do it once all shapes have been added
         PODVector<CollisionShape*> shapes;
         node_->GetComponents<CollisionShape>(shapes);
         for (PODVector<CollisionShape*>::Iterator i = shapes.Begin(); i != shapes.End(); ++i)
-        {
-            massUpdated = true;
-            (*i)->NotifyRigidBody();
-        }
+            (*i)->NotifyRigidBody(false);
 
         // Check if this node contains Constraint components that were waiting for the rigid body to be created, and signal them
         // to create themselves now
@@ -937,9 +943,7 @@ void RigidBody::AddBodyToWorld()
             (*i)->CreateConstraint();
     }
 
-    if (!massUpdated)
-        UpdateMass();
-
+    UpdateMass();
     UpdateGravity();
 
     int flags = body_->getCollisionFlags();

Engine/Physics/RigidBody.h

@@ -184,6 +184,8 @@ public:
     bool GetUseGravity() const { return useGravity_; }
     /// Return gravity override. If zero (default), uses the physics world's gravity.
     const Vector3& GetGravityOverride() const { return gravityOverride_; }
+    /// Return center of mass offset.
+    const Vector3& GetCenterOfMass() const { return centerOfMass_; }
     /// Return kinematic mode flag.
     bool IsKinematic() const { return kinematic_; }
     /// Return phantom mode flag.
@@ -236,7 +238,7 @@ private:
     btRigidBody* body_;
     /// Bullet compound collision shape.
     btCompoundShape* compoundShape_;
-    /// Bullet shifted compound collision shape.
+    /// Compound collision shape with center of mass offset applied.
     btCompoundShape* shiftedCompoundShape_;
     /// Physics world.
     WeakPtr<PhysicsWorld> physicsWorld_;
@@ -244,8 +246,8 @@ private:
     PODVector<Constraint*> constraints_;
     /// Gravity override vector.
     Vector3 gravityOverride_;
-    /// Center of mass shift vector.
-    Vector3 centerOfMassShift_;
+    /// Center of mass offset.
+    Vector3 centerOfMass_;
     /// Mass.
     float mass_;
     /// Attribute buffer for network replication.