Changeset 2374 for code/branches/physics/src/orxonox/objects/worldentities/MovableEntity.cc

Timestamp:

Dec 10, 2008, 1:38:17 PM (17 years ago)

Author:

rgrieder

Message:

Trying to synchronise phyiscs over the network.

• Removed derivation of CollisionShape from WorldEntity (BaseObject instead).

File:

• code/branches/physics/src/orxonox/objects/worldentities/MovableEntity.cc (modified) (15 diffs)

code/branches/physics/src/orxonox/objects/worldentities/MovableEntity.cc

@@ -33 +33 @@
 
 #include "util/Debug.h"
+#include "util/MathConvert.h"
 #include "util/Exception.h"
 #include "core/CoreIncludes.h"
@@ -45 +46 @@
         RegisterObject(MovableEntity);
 
-        this->velocity_ = Vector3::ZERO;
+        this->linearAcceleration_  = Vector3::ZERO;
+        this->linearVelocity_      = Vector3::ZERO;
+        this->angularAcceleration_ = Vector3::ZERO;
+        this->angularVelocity_     = Vector3::ZERO;
 
         this->registerVariables();
@@ -58 +62 @@
         SUPER(MovableEntity, XMLPort, xmlelement, mode);
 
-        XMLPortParamTemplate(MovableEntity, "velocity", setVelocity, getVelocity, xmlelement, mode, const Vector3&);
+        XMLPortParamTemplate(MovableEntity, "velocity",     setVelocity,     getVelocity,     xmlelement, mode, const Vector3&);
+        XMLPortParamTemplate(MovableEntity, "rotationaxis", setRotationAxis, getRotationAxis, xmlelement, mode, const Vector3&);
+        XMLPortParam(MovableEntity, "rotationrate", setRotationRate, getRotationRate, xmlelement, mode);
     }
 
@@ -65 +71 @@
     }
 
+    void MovableEntity::tick(float dt)
+    {
+        if (this->isActive())
+        {
+            // Check whether Bullet doesn't do the physics for us
+            if (!this->isDynamic())
+            {
+                // Linear part
+                this->linearVelocity_.x += this->linearAcceleration_.x * dt;
+                this->linearVelocity_.y += this->linearAcceleration_.y * dt;
+                this->linearVelocity_.z += this->linearAcceleration_.z * dt;
+                linearVelocityChanged(true);
+                this->node_->translate(this->linearVelocity_ * dt);
+                positionChanged(true);
+
+                // Angular part
+                // Note: angularVelocity_ is a Quaternion with w = 0 while angularAcceleration_ is a Vector3
+                this->angularVelocity_.x += angularAcceleration_.x * dt;
+                this->angularVelocity_.y += angularAcceleration_.y * dt;
+                this->angularVelocity_.z += angularAcceleration_.z * dt;
+                angularVelocityChanged(true);
+                // Calculate new orientation with quaternion derivative. This is about 30% faster than with angle/axis method.
+                float mult = dt * 0.5;
+                // TODO: this could be optimized by writing it out. The calls currently create 4 new Quaternions!
+                Quaternion newOrientation(0.0f, this->angularVelocity_.x * mult, this->angularVelocity_.y * mult, this->angularVelocity_.z * mult);
+                newOrientation = this->node_->getOrientation() + newOrientation * this->node_->getOrientation();
+                newOrientation.normalise();
+                this->node_->setOrientation(newOrientation);
+                orientationChanged(true);
+            }
+        }
+    }
+
     void MovableEntity::setPosition(const Vector3& position)
     {
         if (this->isDynamic())
         {
+            //if (this->isPhysicsRunning())
+            //    return;
             btTransform transf = this->physicalBody_->getWorldTransform();
             transf.setOrigin(btVector3(position.x, position.y, position.z));
@@ -74 +115 @@
         }
 
+        //COUT(0) << "setting position: " << position << std::endl;
         this->node_->setPosition(position);
-        positionChanged();
+        positionChanged(false);
     }
 
@@ -88 +130 @@
 
         this->node_->translate(distance, relativeTo);
-        positionChanged();
+        positionChanged(false);
     }
 
@@ -96 +138 @@
         {
             btTransform transf = this->physicalBody_->getWorldTransform();
-            transf.setRotation(btQuaternion(orientation.w, orientation.x, orientation.y, orientation.z));
+            transf.setRotation(btQuaternion(orientation.x, orientation.y, orientation.z, orientation.w));
             this->physicalBody_->setWorldTransform(transf);
         }
 
         this->node_->setOrientation(orientation);
-        orientationChanged();
+        orientationChanged(false);
     }
 
@@ -111 +153 @@
                                                           to any other space than TS_LOCAL.");
             btTransform transf = this->physicalBody_->getWorldTransform();
-            this->physicalBody_->setWorldTransform(transf * btTransform(btQuaternion(rotation.w, rotation.x, rotation.y, rotation.z)));
+            this->physicalBody_->setWorldTransform(transf * btTransform(btQuaternion(rotation.x, rotation.y, rotation.z, rotation.w)));
         }
 
         this->node_->rotate(rotation, relativeTo);
-        orientationChanged();
+        orientationChanged(false);
     }
 
@@ -130 +172 @@
 
         this->node_->yaw(angle, relativeTo);
-        orientationChanged();
+        orientationChanged(false);
     }
 
@@ -145 +187 @@
 
         this->node_->pitch(angle, relativeTo);
-        orientationChanged();
+        orientationChanged(false);
     }
 
@@ -160 +202 @@
 
         this->node_->roll(angle, relativeTo);
-        orientationChanged();
+        orientationChanged(false);
     }
 
@@ -175 +217 @@
 
         this->node_->lookAt(target, relativeTo, localDirectionVector);
-        orientationChanged();
+        orientationChanged(false);
     }
 
@@ -190 +232 @@
 
         this->node_->setDirection(direction, relativeTo, localDirectionVector);
-        orientationChanged();
+        orientationChanged(false);
     }
 
@@ -196 +238 @@
     {
         if (this->isDynamic())
-        {
             this->physicalBody_->setLinearVelocity(btVector3(velocity.x, velocity.y, velocity.z));
-        }
-
-        this->velocity_ = velocity;
-        velocityChanged();
+
+        this->linearVelocity_ = velocity;
+        linearVelocityChanged(false);
+    }
+
+    void MovableEntity::setAngularVelocity(const Vector3& velocity)
+    {
+        if (this->isDynamic())
+            this->physicalBody_->setAngularVelocity(btVector3(velocity.x, velocity.y, velocity.z));
+
+        this->angularVelocity_ = velocity;
+        angularVelocityChanged(false);
+    }
+
+    void MovableEntity::setAcceleration(const Vector3& acceleration)
+    {
+        if (this->isDynamic())
+            this->physicalBody_->applyCentralForce(btVector3(acceleration.x * this->getMass(), acceleration.y * this->getMass(), acceleration.z * this->getMass()));
+
+        this->linearAcceleration_ = acceleration;
+    }
+
+    void MovableEntity::setAngularAcceleration(const Vector3& acceleration)
+    {
+        if (this->isDynamic())
+        {
+            btVector3 inertia(btVector3(1, 1, 1) / this->physicalBody_->getInvInertiaDiagLocal());
+            this->physicalBody_->applyTorque(btVector3(acceleration.x, acceleration.y, acceleration.z) * inertia);
+        }
+
+        this->angularAcceleration_ = acceleration;
     }
 
@@ -220 +288 @@
         this->node_->setPosition(Vector3(worldTrans.getOrigin().x(), worldTrans.getOrigin().y(), worldTrans.getOrigin().z()));
         this->node_->setOrientation(Quaternion(worldTrans.getRotation().w(), worldTrans.getRotation().x(), worldTrans.getRotation().y(), worldTrans.getRotation().z()));
-        const btVector3& velocity = this->physicalBody_->getLinearVelocity();
-        this->velocity_.x = velocity.x();
-        this->velocity_.y = velocity.y();
-        this->velocity_.z = velocity.z();
-        velocityChanged();
-        positionChanged();
-        orientationChanged();
+        COUT(0) << "setting world transform: " << omni_cast<std::string>(node_->getPosition()) << std::endl;
+        this->linearVelocity_.x = this->physicalBody_->getLinearVelocity().x();
+        this->linearVelocity_.y = this->physicalBody_->getLinearVelocity().y();
+        this->linearVelocity_.z = this->physicalBody_->getLinearVelocity().z();
+        this->angularVelocity_.x = this->physicalBody_->getAngularVelocity().x();
+        this->angularVelocity_.y = this->physicalBody_->getAngularVelocity().y();
+        this->angularVelocity_.z = this->physicalBody_->getAngularVelocity().z();
+        linearVelocityChanged(true);
+        angularVelocityChanged(true);
+        positionChanged(true);
+        orientationChanged(true);
     }
 
     void MovableEntity::getWorldTransform(btTransform& worldTrans) const
     {
-        // We use a kinematic body 
+        // We use a kinematic body
         worldTrans.setOrigin(btVector3(node_->getPosition().x, node_->getPosition().y, node_->getPosition().z));
-        worldTrans.setRotation(btQuaternion(node_->getOrientation().w, node_->getOrientation().x, node_->getOrientation().y, node_->getOrientation().z));
+        worldTrans.setRotation(btQuaternion(node_->getOrientation().x, node_->getOrientation().y, node_->getOrientation().z, node_->getOrientation().w));
         if (this->isDynamic())
         {
             // This function gets called only once for dynamic objects to set the initial conditions
-            // We have to set the velocity too.
-            this->physicalBody_->setLinearVelocity(btVector3(velocity_.x, velocity_.y, velocity_.z));
+            // We have to set the velocities too.
+            this->physicalBody_->setLinearVelocity(btVector3(linearVelocity_.x, linearVelocity_.y, linearVelocity_.z));
+            this->physicalBody_->setAngularVelocity(btVector3(angularVelocity_.x, angularVelocity_.y, angularVelocity_.z));
         }
     }