Fixed hinge constraint breaking.

Fixed constraint not being created if rigid body did not initially exist.
Changed constraint position & axis to be serialized in world space for reliability.

Docs/ScriptAPI.dox

@@ -4067,9 +4067,7 @@ Properties:<br>
 - Node@ node (readonly)
 - ConstraintType constraintType
 - Vector3& position
-- Vector3& otherPosition
 - Vector3& axis
-- Vector3& otherAxis
 - float lowLimit
 - float highLimit
 - RigidBody@ ownBody (readonly)

Engine/Engine/PhysicsAPI.cpp

@@ -171,12 +171,8 @@ static void RegisterConstraint(asIScriptEngine* engine)
     engine->RegisterObjectMethod("Constraint", "ConstraintType get_constraintType() const", asMETHOD(Constraint, GetConstraintType), asCALL_THISCALL);
     engine->RegisterObjectMethod("Constraint", "void set_position(const Vector3&in)", asMETHOD(Constraint, SetPosition), asCALL_THISCALL);
     engine->RegisterObjectMethod("Constraint", "const Vector3& get_position() const", asMETHOD(Constraint, GetPosition), asCALL_THISCALL);
-    engine->RegisterObjectMethod("Constraint", "void set_otherPosition(const Vector3&in)", asMETHOD(Constraint, SetOtherPosition), asCALL_THISCALL);
-    engine->RegisterObjectMethod("Constraint", "const Vector3& get_otherPosition() const", asMETHOD(Constraint, GetOtherPosition), asCALL_THISCALL);
     engine->RegisterObjectMethod("Constraint", "void set_axis(const Vector3&in)", asMETHOD(Constraint, SetAxis), asCALL_THISCALL);
     engine->RegisterObjectMethod("Constraint", "const Vector3& get_axis() const", asMETHOD(Constraint, GetAxis), asCALL_THISCALL);
-    engine->RegisterObjectMethod("Constraint", "void set_otherAxis(const Vector3&in)", asMETHOD(Constraint, SetOtherAxis), asCALL_THISCALL);
-    engine->RegisterObjectMethod("Constraint", "const Vector3& get_otherAxis() const", asMETHOD(Constraint, GetOtherAxis), asCALL_THISCALL);
     engine->RegisterObjectMethod("Constraint", "float get_lowLimit() const", asMETHOD(Constraint, GetLowLimit), asCALL_THISCALL);
     engine->RegisterObjectMethod("Constraint", "void set_lowLimit(float)", asMETHOD(Constraint, SetLowLimit), asCALL_THISCALL);
     engine->RegisterObjectMethod("Constraint", "float get_highLimit() const", asMETHOD(Constraint, GetHighLimit), asCALL_THISCALL);

Engine/Physics/Constraint.cpp

@@ -55,9 +55,7 @@ Constraint::Constraint(Context* context) :
     constraint_(0),
     type_(CONSTRAINT_POINT),
     position_(Vector3::ZERO),
-    otherPosition_(Vector3::ZERO),
-    axis_(Vector3::ZERO),
-    otherAxis_(Vector3::ZERO),
+    axis_(Vector3::FORWARD),
     lowLimit_(DEFAULT_LOW_LIMIT),
     highLimit_(DEFAULT_HIGH_LIMIT),
     otherBodyNodeID_(0),
@@ -79,13 +77,11 @@ void Constraint::RegisterObject(Context* context)
     context->RegisterFactory<Constraint>();
     
     ENUM_ATTRIBUTE(Constraint, "Constraint Type", type_, typeNames, CONSTRAINT_POINT, AM_DEFAULT);
-    ATTRIBUTE(Constraint, VAR_VECTOR3, "Position", position_, Vector3::ZERO, AM_DEFAULT);
-    ATTRIBUTE(Constraint, VAR_VECTOR3, "Axis", axis_, Vector3::ZERO, AM_DEFAULT);
+    REF_ACCESSOR_ATTRIBUTE(Constraint, VAR_VECTOR3, "Position", GetPosition, SetPosition, Vector3, Vector3::ZERO, AM_DEFAULT | AM_LATESTDATA);
+    REF_ACCESSOR_ATTRIBUTE(Constraint, VAR_VECTOR3, "Axis", GetAxis, SetAxis, Vector3, Vector3::FORWARD, AM_DEFAULT | AM_LATESTDATA);
+    ACCESSOR_ATTRIBUTE(Constraint, VAR_FLOAT, "Low Limit", GetLowLimit, SetLowLimit, float, DEFAULT_LOW_LIMIT, AM_DEFAULT);
+    ACCESSOR_ATTRIBUTE(Constraint, VAR_FLOAT, "High Limit", GetHighLimit, SetHighLimit, float, DEFAULT_HIGH_LIMIT, AM_DEFAULT);
     ATTRIBUTE(Constraint, VAR_INT, "Other Body NodeID", otherBodyNodeID_, 0, AM_DEFAULT | AM_NODEID);
-    ATTRIBUTE(Constraint, VAR_VECTOR3, "Other Body Position", otherPosition_, Vector3::ZERO, AM_DEFAULT);
-    ATTRIBUTE(Constraint, VAR_VECTOR3, "Other Body Axis", otherAxis_, Vector3::ZERO, AM_DEFAULT);
-    ATTRIBUTE(Constraint, VAR_FLOAT, "Low Limit", lowLimit_, DEFAULT_LOW_LIMIT, AM_DEFAULT);
-    ATTRIBUTE(Constraint, VAR_FLOAT, "High Limit", highLimit_, DEFAULT_HIGH_LIMIT, AM_DEFAULT);
     ATTRIBUTE(Constraint, VAR_BOOL, "Disable Collision", disableCollision_, false, AM_DEFAULT);
 }
 
@@ -93,8 +89,9 @@ void Constraint::OnSetAttribute(const AttributeInfo& attr, const Variant& src)
 {
     Component::OnSetAttribute(attr, src);
     
-    // Change of any attribute requires recreation of the constraint
-    recreateConstraint_ = true;
+    // Change of any non-accessor attribute requires recreation of the constraint
+    if (!attr.accessor_)
+        recreateConstraint_ = true;
 }
 
 void Constraint::ApplyAttributes()
@@ -158,16 +155,9 @@ void Constraint::SetPosition(const Vector3& position)
     if (position != position_)
     {
         position_ = position;
-        CreateConstraint();
-    }
-}
-
-void Constraint::SetOtherPosition(const Vector3& position)
-{
-    if (position != otherPosition_)
-    {
-        otherPosition_ = position;
-        CreateConstraint();
+        /// \todo Optimize and do not recreate the constraint
+        if (constraint_)
+            CreateConstraint();
     }
 }
 
@@ -176,41 +166,79 @@ void Constraint::SetAxis(const Vector3& axis)
     if (axis != axis_)
     {
         axis_ = axis;
-        if (type_ == CONSTRAINT_HINGE)
+        /// \todo Optimize and do not recreate the constraint
+        if (constraint_ && constraint_->getConstraintType() == HINGE_CONSTRAINT_TYPE)
             CreateConstraint();
     }
 }
 
-void Constraint::SetOtherAxis(const Vector3& axis)
+void Constraint::SetLowLimit(float limit)
 {
-    if (axis != otherAxis_)
+    if (limit != lowLimit_)
     {
-        otherAxis_ = axis;
-        if (type_ == CONSTRAINT_HINGE)
-            CreateConstraint();
+        lowLimit_ = limit;
+        
+        if (constraint_ && constraint_->getConstraintType() == HINGE_CONSTRAINT_TYPE)
+        {
+            btHingeConstraint* hingeConstraint = static_cast<btHingeConstraint*>(constraint_);
+            hingeConstraint->setLimit(lowLimit_ * M_DEGTORAD, highLimit_ * M_DEGTORAD);
+        }
     }
 }
 
-void Constraint::SetLowLimit(float limit)
+void Constraint::SetHighLimit(float limit)
 {
-    lowLimit_ = limit;
-    
-    if (constraint_ && constraint_->getConstraintType() == HINGE_CONSTRAINT_TYPE)
+    if (limit != highLimit_)
     {
-        btHingeConstraint* hingeConstraint = static_cast<btHingeConstraint*>(constraint_);
-        hingeConstraint->setLimit(lowLimit_ * M_DEGTORAD, highLimit_ * M_DEGTORAD);
+        highLimit_ = limit;
+        
+        if (constraint_ && constraint_->getConstraintType() == HINGE_CONSTRAINT_TYPE)
+        {
+            btHingeConstraint* hingeConstraint = static_cast<btHingeConstraint*>(constraint_);
+            hingeConstraint->setLimit(lowLimit_ * M_DEGTORAD, highLimit_ * M_DEGTORAD);
+        }
     }
 }
 
-void Constraint::SetHighLimit(float limit)
+const Vector3& Constraint::GetPosition() const
 {
-    highLimit_ = limit;
+    // If the constraint exists and connects two rigid bodies, update position from it
+    /// \todo If there is constraint error, the two bodies may disagree, resulting in drift with successive load/save
+    if (constraint_ && ownBody_ && otherBody_)
+    {
+        switch (constraint_->getConstraintType())
+        {
+        case POINT2POINT_CONSTRAINT_TYPE:
+            {
+                btPoint2PointConstraint* pointConstraint = static_cast<btPoint2PointConstraint*>(constraint_);
+                position_ = ToVector3(ownBody_->GetBody()->getCenterOfMassTransform() * pointConstraint->getPivotInA());
+            }
+            break;
+            
+        case HINGE_CONSTRAINT_TYPE:
+            {
+                btHingeConstraint* hingeConstraint = static_cast<btHingeConstraint*>(constraint_);
+                position_ = ToVector3(ownBody_->GetBody()->getCenterOfMassTransform() * hingeConstraint->getAFrame().getOrigin());
+            }
+            break;
+        }
+    }
     
-    if (constraint_ && constraint_->getConstraintType() == HINGE_CONSTRAINT_TYPE)
+    return position_;
+}
+
+const Vector3& Constraint::GetAxis() const
+{
+    // If the constraint exists and connects two rigid bodies, update axis from it
+    /// \todo If there is constraint error, the two bodies may disagree, resulting in drift with successive load/save
+    if (constraint_ && ownBody_ && otherBody_ && constraint_->getConstraintType() == HINGE_CONSTRAINT_TYPE)
     {
         btHingeConstraint* hingeConstraint = static_cast<btHingeConstraint*>(constraint_);
-        hingeConstraint->setLimit(lowLimit_ * M_DEGTORAD, highLimit_ * M_DEGTORAD);
+        btTransform worldFrame = ownBody_->GetBody()->getCenterOfMassTransform() * hingeConstraint->getAFrame();
+        axis_ = ToVector3(worldFrame.getBasis().getColumn(0));
     }
+    
+    return axis_;
 }
 
 void Constraint::ReleaseConstraint()
@@ -239,6 +267,10 @@ void Constraint::OnNodeSet(Node* node)
                 LOGERROR("No physics world component in scene, can not create constraint");
         }
         node->AddListener(this);
+        
+        // Set node world position as initial position, then try to create the constraint initially
+        position_ = node->GetWorldPosition();
+        CreateConstraint();
     }
 }
 
@@ -259,9 +291,13 @@ void Constraint::CreateConstraint()
     {
     case CONSTRAINT_POINT:
         if (otherBody)
-            constraint_ = new btPoint2PointConstraint(*ownBody, *otherBody, ToBtVector3(position_), ToBtVector3(otherPosition_));
+        {
+            constraint_ = new btPoint2PointConstraint(*ownBody, *otherBody, ownBody->getCenterOfMassTransform().inverse() *
+                ToBtVector3(position_), otherBody->getCenterOfMassTransform().inverse() * ToBtVector3(position_));
+        }
         else
-            constraint_ = new btPoint2PointConstraint(*ownBody, ToBtVector3(position_));
+            constraint_ = new btPoint2PointConstraint(*ownBody, ownBody->getCenterOfMassTransform().inverse() *
+                ToBtVector3(position_));
         break;
         
     case CONSTRAINT_HINGE:
@@ -269,13 +305,21 @@ void Constraint::CreateConstraint()
             btHingeConstraint* hingeConstraint;
             if (otherBody)
             {
-                constraint_ = hingeConstraint = new btHingeConstraint(*ownBody, *otherBody, ToBtVector3(position_),
-                    ToBtVector3(otherPosition_), ToBtVector3(axis_.Normalized()), ToBtVector3(otherAxis_.Normalized()));
+                btTransform ownBodyInverse = ownBody->getCenterOfMassTransform().inverse();
+                btTransform otherBodyInverse = otherBody->getCenterOfMassTransform().inverse();
+                Vector3 axis = axis_.Normalized();
+                
+                constraint_ = hingeConstraint = new btHingeConstraint(*ownBody, *otherBody, ownBodyInverse *
+                    ToBtVector3(position_), otherBodyInverse * ToBtVector3(position_), ownBodyInverse.getBasis() *
+                    ToBtVector3(axis), otherBodyInverse.getBasis() * ToBtVector3(axis));
             }
             else
             {
-                constraint_ = hingeConstraint = new btHingeConstraint(*ownBody, ToBtVector3(position_),
-                    ToBtVector3(axis_.Normalized()));
+                btTransform ownBodyInverse = ownBody->getCenterOfMassTransform().inverse();
+                Vector3 axis = axis_.Normalized();
+                
+                constraint_ = hingeConstraint = new btHingeConstraint(*ownBody, ownBodyInverse * ToBtVector3(position_),
+                    ownBodyInverse.getBasis() * ToBtVector3(axis));
             }
             
             hingeConstraint->setLimit(lowLimit_ * M_DEGTORAD, highLimit_ * M_DEGTORAD);

Engine/Physics/Constraint.h

@@ -42,6 +42,8 @@ class Constraint : public Component
 {
     OBJECT(Constraint);
     
+    friend class RigidBody;
+    
 public:
     /// Construct.
     Constraint(Context* context);
@@ -63,14 +65,10 @@ public:
     void SetConstraintType(ConstraintType type);
     /// %Set other body to connect to. Set to null to connect to the static world.
     void SetOtherBody(RigidBody* body);
-    /// %Set position relative to own body.
+    /// %Set constraint world-space position.
     void SetPosition(const Vector3& position);
-    /// %Set position relative to other body.
-    void SetOtherPosition(const Vector3& position);
-    /// %Set axis relative to own body.
+    /// %Set constraint world-space axis.
     void SetAxis(const Vector3& axis);
-    /// %Set axis relative to other body.
-    void SetOtherAxis(const Vector3& axis);
     /// %Set low limit.
     void SetLowLimit(float limit);
     /// %Set high limit.
@@ -86,14 +84,10 @@ public:
     RigidBody* GetOwnBody() const { return ownBody_; }
     /// Return the other rigid body. May be null if connected to the static world.
     RigidBody* GetOtherBody() const { return otherBody_; }
-    /// Return position relative to own body.
-    const Vector3& GetPosition() const { return position_; }
-    /// Return position relative to other body.
-    const Vector3& GetOtherPosition() const { return otherPosition_; }
-    /// Return axis relative to own body.
-    const Vector3& GetAxis() const { return axis_; }
-    /// Return axis relative to other body.
-    const Vector3& GetOtherAxis() const { return otherAxis_; }
+    /// Return constraint world-space position.
+    const Vector3& GetPosition() const;
+    /// Return constraint world-space axis.
+    const Vector3& GetAxis() const;
     /// Return low limit.
     float GetLowLimit() const { return lowLimit_; }
     /// Return high limit.
@@ -120,14 +114,10 @@ private:
     btTypedConstraint* constraint_;
     /// Constraint type.
     ConstraintType type_;
-    /// Constraint position relative to own body.
-    Vector3 position_;
-    /// Constraint position relative to other body.
-    Vector3 otherPosition_;
-    /// Constraint axis relative to own body.
-    Vector3 axis_;
-    /// Constraint axis relative to other body.
-    Vector3 otherAxis_;
+    /// Constraint world-space position.
+    mutable Vector3 position_;
+    /// Constraint world-space axis.
+    mutable Vector3 axis_;
     /// Low limit.
     float lowLimit_;
     /// High limit.

Engine/Physics/RigidBody.cpp

@@ -23,6 +23,7 @@
 
 #include "Precompiled.h"
 #include "CollisionShape.h"
+#include "Constraint.h"
 #include "Context.h"
 #include "Log.h"
 #include "MemoryBuffer.h"
@@ -791,12 +792,19 @@ 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
         PODVector<CollisionShape*> shapes;
-        node_->GetDerivedComponents<CollisionShape>(shapes);
+        node_->GetComponents<CollisionShape>(shapes);
         for (PODVector<CollisionShape*>::Iterator i = shapes.Begin(); i != shapes.End(); ++i)
         {
             massUpdated = true;
             (*i)->NotifyRigidBody();
         }
+        
+        // Check if this node contains Constraint components that were waiting for the rigid body to be created, and signal them
+        // to create themselves now
+        PODVector<Constraint*> constraints;
+        node_->GetComponents<Constraint>(constraints);
+        for (PODVector<Constraint*>::Iterator i = constraints.Begin(); i != constraints.End(); ++i)
+            (*i)->CreateConstraint();
     }
     
     if (!massUpdated)

ThirdParty/Bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -13,6 +13,8 @@ subject to the following restrictions:
 3. This notice may not be removed or altered from any source distribution.
 */
 
+// Modified by Lasse Öörni for Urho3D
+
 //#define COMPUTE_IMPULSE_DENOM 1
 //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
 
@@ -1202,7 +1204,10 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 		const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
 		btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
 		constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
-		if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
+		
+		// Urho3D: if constraint has infinity breaking threshold, do not break no matter what
+		btScalar breakingThreshold = constr->getBreakingImpulseThreshold();
+		if (breakingThreshold < SIMD_INFINITY && btFabs(solverConstr.m_appliedImpulse)>=breakingThreshold)
 		{
 			constr->setEnabled(false);
 		}