Index: /code/branches/physics/src/orxonox/objects/Scene.cc
===================================================================
--- /code/branches/physics/src/orxonox/objects/Scene.cc	(revision 2305)
+++ /code/branches/physics/src/orxonox/objects/Scene.cc	(revision 2306)
@@ -172,6 +172,8 @@
     void Scene::tick(float dt)
     {
+        // TODO: This is not stable! If physics cannot be calculated real time anymore,
+        //       framerate will drop exponentially.
         if (physicalWorld_)
-            physicalWorld_->stepSimulation(dt,(int)(dt/0.0166666f));
+            physicalWorld_->stepSimulation(dt,(int)(dt/0.0166666f + 1.0f));
     }
 
Index: /code/branches/physics/src/orxonox/objects/worldentities/MovableEntity.cc
===================================================================
--- /code/branches/physics/src/orxonox/objects/worldentities/MovableEntity.cc	(revision 2305)
+++ /code/branches/physics/src/orxonox/objects/worldentities/MovableEntity.cc	(revision 2306)
@@ -32,4 +32,5 @@
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 
+#include "util/Debug.h"
 #include "util/Exception.h"
 #include "core/CoreIncludes.h"
@@ -220,5 +221,7 @@
         this->node_->setOrientation(Quaternion(worldTrans.getRotation().w(), worldTrans.getRotation().x(), worldTrans.getRotation().y(), worldTrans.getRotation().z()));
         const btVector3& velocity = this->physicalBody_->getLinearVelocity();
-        this->velocity_ = Vector3(velocity.x(), velocity.y(), velocity.z());
+        this->velocity_.x = velocity.x();
+        this->velocity_.y = velocity.y();
+        this->velocity_.z = velocity.z();
         velocityChanged();
         positionChanged();
Index: /code/branches/physics/src/orxonox/objects/worldentities/WorldEntity.cc
===================================================================
--- /code/branches/physics/src/orxonox/objects/worldentities/WorldEntity.cc	(revision 2305)
+++ /code/branches/physics/src/orxonox/objects/worldentities/WorldEntity.cc	(revision 2306)
@@ -75,5 +75,6 @@
         this->collisionShape_ = 0;
         this->mass_ = 0;
-        updateCollisionType();
+        this->childMass_ = 0;
+        this->collisionType_ = None;
 
         this->registerVariables();
@@ -151,4 +152,5 @@
             {
                 // static to static/kinematic/dynamic --> merge shapes
+                this->childMass_ += object->getMass();
                 this->attachCollisionShape(object->getCollisionShape());
                 // Remove the btRigidBody from the child object.
@@ -195,16 +197,4 @@
     }
 
-    void WorldEntity::mergeCollisionShape(CollisionShape* shape)
-    {
-        if (!this->collisionShape_)
-            this->collisionShape_ = new CompoundCollisionShape(this);
-        assert(this->collisionShape_->isCompoundShape());
-
-        // merge with transform
-        CompoundCollisionShape* compoundShape = static_cast<CompoundCollisionShape*>(this->collisionShape_);
-        assert(compoundShape);
-        compoundShape->addChildShape(shape);
-    }
-
     void WorldEntity::attachCollisionShape(CollisionShape* shape)
     {
@@ -213,7 +203,13 @@
         if (!this->collisionShape_ && shape->hasNoTransform())
         {
-            // Simply add the shape as is.
+            // Set local scaling right when adding it. It can include both scaling parameters
+            // and shape parameters (like sphere radius)
             shape->getCollisionShape()->setLocalScaling(shape->getTotalScaling());
             this->collisionShape_ = shape;
+            // recalculate inertia tensor
+            if (this->isDynamic())
+            {
+                internalSetMassProps();
+            }
         }
         else
@@ -246,8 +242,18 @@
     }
 
-    //BlinkingBillboard* WorldEntity::getAttachedAsdfObject(unsigned int index) const
-    //{
-    //    return 0;
-    //}
+    void WorldEntity::mergeCollisionShape(CollisionShape* shape)
+    {
+        if (!this->collisionShape_)
+            this->collisionShape_ = new CompoundCollisionShape(this);
+        assert(this->collisionShape_->isCompoundShape());
+
+        // merge with transform
+        CompoundCollisionShape* compoundShape = static_cast<CompoundCollisionShape*>(this->collisionShape_);
+        assert(compoundShape);
+        compoundShape->addChildShape(shape);
+
+        // recalculate inertia tensor
+        internalSetMassProps();
+    }
 
     void WorldEntity::setScale3D(const Vector3& scale)
@@ -270,17 +276,36 @@
     {
         this->mass_ = mass;
-        if (!hasPhysics())
-            COUT(2) << "Warning: Setting the mass of a WorldEntity with no physics has no effect." << std::endl;
+        if (!this->hasPhysics())
+            COUT(3) << "Warning: Setting the mass of a WorldEntity with no physics has no effect." << std::endl;
         else if (this->physicalBody_->isInWorld())
-            COUT(2) << "Warning: Cannot set the physical mass at run time. Storing temporarily." << std::endl;
-        else
-        {
-            if (this->collisionType_ != Dynamic)
-                COUT(2) << "Warning: Cannot set the physical mass of a static or kinematic object. Storing temporarily." << std::endl;
-            else if (mass == 0.0f)
-                COUT(2) << "Warning: Cannot set physical mass of a dynamic object to zero. Storing temporarily." << std::endl;
-            else
-                this->physicalBody_->setMassProps(mass, btVector3(0,0,0));
-        }
+            COUT(2) << "Warning: Cannot set the physical mass at run time. Storing new mass." << std::endl;
+        else
+            internalSetMassProps();
+    }
+
+    void WorldEntity::internalSetMassProps()
+    {
+        assert(hasPhysics());
+
+        if ((this->isKinematic() || this->isStatic()) && (this->mass_ + this->childMass_) != 0.0f)
+        {
+            // Mass non zero is a bad idea for kinematic and static objects
+            this->physicalBody_->setMassProps(0.0f, btVector3(0, 0, 0));
+        }
+        else if (this->isDynamic() && (this->mass_ + this->childMass_) == 0.0f)
+        {
+            // Mass zero is not such a good idea for dynamic objects
+            this->physicalBody_->setMassProps(1.0f, this->getLocalInertia(1.0f));
+        }
+        else
+            this->physicalBody_->setMassProps(this->mass_, this->getLocalInertia(this->mass_ + this->childMass_));
+    }
+
+    btVector3 WorldEntity::getLocalInertia(btScalar mass) const
+    {
+        btVector3 inertia(0, 0, 0);
+        if (this->collisionShape_)
+            this->collisionShape_->getCollisionShape()->calculateLocalInertia(mass, inertia);
+        return inertia;
     }
 
@@ -300,4 +325,8 @@
         if (type != None && this->collisionType_ == None)
         {
+            // Check whether there was some scaling applied.
+            if (!this->node_->getScale().positionEquals(Vector3(1, 1, 1), 0.001))
+                ThrowException(NotImplemented, "Cannot create a physical body if there is scaling applied to the node: Not yet implemented.");
+
             // Create new rigid body
             btCollisionShape* temp = 0;
@@ -337,25 +366,8 @@
         // update our variable for faster checks
         updateCollisionType();
-
-        // Mass non zero is a bad idea for kinematic and static objects
-        if ((type == Kinematic || type == Static) && this->mass_ != 0.0f)
-            this->setMass(0.0f);
-        // Mass zero is not such a good idea for dynamic objects
-        else if (type == Dynamic && this->mass_ == 0.0f)
-            this->setMass(1.0f);
-        else if (hasPhysics())
-            this->physicalBody_->setMassProps(this->mass_, btVector3(0,0,0));
-    }
-
-    void WorldEntity::updateCollisionType()
-    {
-        if (!this->physicalBody_)
-            this->collisionType_ = None;
-        else if (this->physicalBody_->isKinematicObject())
-            this->collisionType_ = Kinematic;
-        else if (this->physicalBody_->isStaticObject())
-            this->collisionType_ = Static;
-        else
-            this->collisionType_ = Dynamic;
+        assert(this->collisionType_ == type);
+
+        // update mass and inertia tensor
+        internalSetMassProps(); // type is not None! See case None: in switch
     }
 
@@ -395,4 +407,16 @@
     }
 
+    void WorldEntity::updateCollisionType()
+    {
+        if (!this->physicalBody_)
+            this->collisionType_ = None;
+        else if (this->physicalBody_->isKinematicObject())
+            this->collisionType_ = Kinematic;
+        else if (this->physicalBody_->isStaticObject())
+            this->collisionType_ = Static;
+        else
+            this->collisionType_ = Dynamic;
+    }
+
     bool WorldEntity::checkPhysics() const
     {
Index: /code/branches/physics/src/orxonox/objects/worldentities/WorldEntity.h
===================================================================
--- /code/branches/physics/src/orxonox/objects/worldentities/WorldEntity.h	(revision 2305)
+++ /code/branches/physics/src/orxonox/objects/worldentities/WorldEntity.h	(revision 2306)
@@ -108,5 +108,5 @@
                 { this->setDirection(Vector3(x, y, z), relativeTo, localDirectionVector); }
 
-            void setScale3D(const Vector3& scale);
+            virtual void setScale3D(const Vector3& scale);
             inline void setScale3D(float x, float y, float z)
                 { this->setScale3D(Vector3(x, y, z)); }
@@ -119,5 +119,5 @@
                 { Vector3 scale = this->getScale3D(); return (scale.x == scale.y && scale.x == scale.z) ? scale.x : 1; }
 
-            void scale3D(const Vector3& scale);
+            virtual void scale3D(const Vector3& scale);
             inline void scale3D(float x, float y, float z)
                 { this->scale3D(Vector3(x, y, z)); }
@@ -182,16 +182,16 @@
             };
 
-            bool hasPhysics() const { return getCollisionType() != None; }
-
-            CollisionType getCollisionType() const { return this->collisionType_; }
-            void setCollisionType(CollisionType type);
-
-            void setCollisionTypeStr(const std::string& type);
-            std::string getCollisionTypeStr() const;
-
+            bool hasPhysics()  const { return getCollisionType() != None     ; }
             bool isStatic()    const { return getCollisionType() == Static   ; }
             bool isKinematic() const { return getCollisionType() == Kinematic; }
             bool isDynamic()   const { return getCollisionType() == Dynamic  ; }
 
+            inline CollisionType getCollisionType() const
+                { return this->collisionType_; }
+            void setCollisionType(CollisionType type);
+
+            void setCollisionTypeStr(const std::string& type);
+            std::string getCollisionTypeStr() const;
+
             void setMass(float mass);
             inline float getMass() const
@@ -201,19 +201,20 @@
             CollisionShape* getAttachedCollisionShape(unsigned int index) const;
 
-            CollisionShape* getCollisionShape() { return this->collisionShape_; }
-            btRigidBody* getPhysicalBody() { return this->physicalBody_; }
+            inline CollisionShape* getCollisionShape()
+                { return this->collisionShape_; }
+            inline btRigidBody* getPhysicalBody()
+                { return this->physicalBody_; }
 
         protected:
-            //virtual btCollisionShape* getCollisionShape() = 0;
-            //virtual void attachPhysicalObject(WorldEntity* object);
-
             virtual bool isCollisionTypeLegal(CollisionType type) const = 0;
+
+            btRigidBody*  physicalBody_;
+
+        private:
+            void updateCollisionType();
+            void mergeCollisionShape(CollisionShape* shape);
+            void internalSetMassProps();
+            btVector3 getLocalInertia(btScalar mass) const;
             bool checkPhysics() const;
-            void updateCollisionType();
-
-            btRigidBody*  physicalBody_;
-
-        private:
-            void mergeCollisionShape(CollisionShape* shape);
 
             CollisionType                collisionType_;
@@ -221,4 +222,5 @@
             CollisionShape*              collisionShape_;
             btScalar                     mass_;
+            btScalar                     childMass_;
     };
 }
Index: /code/branches/physics/src/orxonox/objects/worldentities/collisionshapes/CollisionShape.cc
===================================================================
--- /code/branches/physics/src/orxonox/objects/worldentities/collisionshapes/CollisionShape.cc	(revision 2305)
+++ /code/branches/physics/src/orxonox/objects/worldentities/collisionshapes/CollisionShape.cc	(revision 2306)
@@ -78,3 +78,13 @@
         return omni_cast<btVector3>(this->node_->getScale());
     }
+
+    void CollisionShape::setScale3D(const Vector3& scale)
+    {
+        ThrowException(NotImplemented, "Cannot set the scale of a collision shape: Not yet implemented.");
+    }
+
+    void CollisionShape::scale3D(const Vector3& scale)
+    {
+        ThrowException(NotImplemented, "Cannot set the scale of a collision shape: Not yet implemented.");
+    }
 }
Index: /code/branches/physics/src/orxonox/objects/worldentities/collisionshapes/CollisionShape.h
===================================================================
--- /code/branches/physics/src/orxonox/objects/worldentities/collisionshapes/CollisionShape.h	(revision 2305)
+++ /code/branches/physics/src/orxonox/objects/worldentities/collisionshapes/CollisionShape.h	(revision 2306)
@@ -55,4 +55,7 @@
 
         private:
+            virtual void setScale3D(const Vector3& scale);
+            virtual void scale3D(const Vector3& scale);
+
             bool isCollisionTypeLegal(WorldEntity::CollisionType type) const;
     };
