Index: code/branches/physics/src/orxonox/objects/collisionshapes/CollisionShape.cc
===================================================================
--- code/branches/physics/src/orxonox/objects/collisionshapes/CollisionShape.cc	(revision 2407)
+++ code/branches/physics/src/orxonox/objects/collisionshapes/CollisionShape.cc	(revision 2423)
@@ -76,12 +76,18 @@
     void CollisionShape::registerVariables()
     {
-        REGISTERDATA(this->parentID_, network::direction::toclient, new network::NetworkCallback<CollisionShape>(this, &CollisionShape::updateParent));
+        REGISTERDATA(this->parentID_, network::direction::toclient, new network::NetworkCallback<CollisionShape>(this, &CollisionShape::parentChanged));
+    }
+
+    void CollisionShape::parentChanged()
+    {
+        CompoundCollisionShape* parent = dynamic_cast<CompoundCollisionShape*>(Synchronisable::getSynchronisable(this->parentID_));
+        if (parent)
+            parent->addChildShape(this);
     }
 
     void CollisionShape::updateParent()
     {
-        CompoundCollisionShape* parent = dynamic_cast<CompoundCollisionShape*>(Synchronisable::getSynchronisable(this->parentID_));
-        if (parent)
-            parent->addChildShape(this);
+        if (this->parent_)
+            this->parent_->updateChildShape(this);
     }
 
@@ -95,4 +101,5 @@
     {
         ThrowException(NotImplemented, "Cannot set the scale of a collision shape: Not yet implemented.");
+        this->updateParent();
     }
 
@@ -100,4 +107,13 @@
     {
         ThrowException(NotImplemented, "Cannot set the scale of a collision shape: Not yet implemented.");
+        this->updateParent();
+    }
+
+    btVector3 CollisionShape::getLocalInertia(btScalar mass) const
+    {
+        btVector3 inertia(0, 0, 0);
+        if (this->collisionShape_)
+            this->collisionShape_->calculateLocalInertia(mass, inertia);
+        return inertia;
     }
 }
Index: code/branches/physics/src/orxonox/objects/collisionshapes/CollisionShape.h
===================================================================
--- code/branches/physics/src/orxonox/objects/collisionshapes/CollisionShape.h	(revision 2407)
+++ code/branches/physics/src/orxonox/objects/collisionshapes/CollisionShape.h	(revision 2423)
@@ -32,4 +32,5 @@
 #include "OrxonoxPrereqs.h"
 
+#include "LinearMath/btVector3.h"
 #include "util/Math.h"
 #include "core/BaseObject.h"
@@ -48,16 +49,16 @@
 
             inline void setPosition(const Vector3& position)
-                { this->position_ = position; }
+                { this->position_ = position; this->updateParent(); }
             inline const Vector3& getPosition() const
                 { return this->position_; }
 
             inline void setOrientation(const Quaternion& orientation)
-                { this->orientation_ = orientation; }
+                { this->orientation_ = orientation; this->updateParent(); }
             inline const Quaternion& getOrientation() const
                 { return this->orientation_; }
 
-            void yaw(const Degree& angle)   { this->orientation_ = this->orientation_ * Quaternion(angle, Vector3::UNIT_Y); }
-            void pitch(const Degree& angle) { this->orientation_ = this->orientation_ * Quaternion(angle, Vector3::UNIT_X); }
-            void roll(const Degree& angle)  { this->orientation_ = this->orientation_ * Quaternion(angle, Vector3::UNIT_Z); }
+            void yaw(const Degree& angle)   { this->setOrientation(this->orientation_ * Quaternion(angle, Vector3::UNIT_Y)); }
+            void pitch(const Degree& angle) { this->setOrientation(this->orientation_ * Quaternion(angle, Vector3::UNIT_X)); }
+            void roll(const Degree& angle)  { this->setOrientation(this->orientation_ * Quaternion(angle, Vector3::UNIT_Z)); }
 
             virtual void setScale3D(const Vector3& scale);
@@ -66,5 +67,7 @@
                 { return this->scale_; }
 
-            virtual inline btCollisionShape* getCollisionShape() const
+            btVector3 getLocalInertia(float mass) const;
+
+            inline btCollisionShape* getCollisionShape() const
                 { return this->collisionShape_; }
 
@@ -75,14 +78,16 @@
 
         protected:
-            btCollisionShape* collisionShape_;
+            virtual void updateParent();
+
+            btCollisionShape*       collisionShape_;
+            CompoundCollisionShape* parent_;
 
         private:
-            void updateParent();
+            void parentChanged();
 
-            Vector3           position_;
-            Quaternion        orientation_;
-            Vector3           scale_;
-            CompoundCollisionShape* parent_;
-            unsigned int      parentID_;
+            Vector3                 position_;
+            Quaternion              orientation_;
+            Vector3                 scale_;
+            unsigned int            parentID_;
     };
 }
Index: code/branches/physics/src/orxonox/objects/collisionshapes/CompoundCollisionShape.cc
===================================================================
--- code/branches/physics/src/orxonox/objects/collisionshapes/CompoundCollisionShape.cc	(revision 2407)
+++ code/branches/physics/src/orxonox/objects/collisionshapes/CompoundCollisionShape.cc	(revision 2423)
@@ -32,7 +32,9 @@
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
 
+#include "util/Exception.h"
 #include "core/CoreIncludes.h"
 #include "core/XMLPort.h"
 #include "tools/BulletConversions.h"
+#include "objects/worldentities/WorldEntity.h"
 
 namespace orxonox
@@ -50,5 +52,9 @@
     {
         if (this->isInitialized())
+        {
+            // Detatch all children first
+            this->removeAllChildShapes();
             delete this->compoundShape_;
+        }
     }
 
@@ -60,21 +66,14 @@
     }
 
-    btCollisionShape* CompoundCollisionShape::getCollisionShape() const
+    void CompoundCollisionShape::addChildShape(CollisionShape* shape)
     {
-        // Note: Returning collisionShape_ means that it's the only one and has no transform.
-        //       So we can get rid of the additional overhead with the compound shape.
-        if (this->collisionShape_)
-            return this->collisionShape_;
-        else if (!this->empty())
-            return this->compoundShape_;
-        else
-            return 0;
-    }
-
-    void CompoundCollisionShape::addChildShape(CollisionShape* shape, bool bWorldEntityRoot)
-    {
-        if (!shape)
+        if (!shape || static_cast<CollisionShape*>(this) == shape)
             return;
-        this->childShapes_.push_back(shape);
+        if (this->childShapes_.find(shape) != this->childShapes_.end())
+        {
+            ThrowException(NotImplemented, "Warning: Attaching a CollisionShape twice is not yet supported.");
+            return;
+        }
+        this->childShapes_[shape] = shape->getCollisionShape();
 
         if (shape->getCollisionShape())
@@ -84,27 +83,110 @@
             this->compoundShape_->addChildShape(transf, shape->getCollisionShape());
 
-            if (this->childShapes_.size() == 1 && !this->childShapes_[0]->hasTransform())
-            {
-                // --> Only shape to be added, no transform; add it directly
-                this->collisionShape_ = shape->getCollisionShape();
-            }
-            else
-            {
-                // Make sure we use the compound shape when returning the btCollisionShape
-                this->collisionShape_ = 0;
-            }
+            this->updatePublicShape();
         }
 
         // network synchro
-        if (!bWorldEntityRoot)
-            shape->setParent(this, this->getObjectID());
+        shape->setParent(this, this->getObjectID());
+    }
+
+    void CompoundCollisionShape::removeChildShape(CollisionShape* shape)
+    {
+        if (this->childShapes_.find(shape) != this->childShapes_.end())
+        {
+            shape->setParent(0, (unsigned int)-1);
+            this->childShapes_.erase(shape);
+            if (shape->getCollisionShape())
+                this->compoundShape_->removeChildShape(shape->getCollisionShape());
+
+            this->updatePublicShape();
+        }
+    }
+
+    void CompoundCollisionShape::removeAllChildShapes()
+    {
+        while (this->childShapes_.size() > 0)
+            this->removeChildShape(this->childShapes_.begin()->first);
+    }
+
+    void CompoundCollisionShape::updateChildShape(CollisionShape* shape)
+    {
+        if (!shape)
+            return;
+        std::map<CollisionShape*, btCollisionShape*>::iterator it = this->childShapes_.find(shape);
+        if (it == this->childShapes_.end())
+        {
+            CCOUT(2) << "Warning: Cannot update child shape: Instance not a child." << std::endl;
+            return;
+        }
+
+        // Remove old btCollisionShape, stored in the children map
+        if (it->second)
+            this->compoundShape_->removeChildShape(it->second);
+        if (shape->getCollisionShape())
+        {
+            // Only actually attach if we didn't pick a CompoundCollisionShape with no content
+            btTransform transf(omni_cast<btQuaternion>(shape->getOrientation()), omni_cast<btVector3>(shape->getPosition()));
+            this->compoundShape_->addChildShape(transf, shape->getCollisionShape());
+            it->second = shape->getCollisionShape();
+        }
+
+        this->updatePublicShape();
+    }
+
+    void CompoundCollisionShape::updatePublicShape()
+    {
+        btCollisionShape* primitive = 0;
+        bool bPrimitive = true;
+        bool bEmpty = true;
+        for (std::map<CollisionShape*, btCollisionShape*>::const_iterator it = this->childShapes_.begin(); it != this->childShapes_.end(); ++it)
+        {
+            if (it->second)
+            {
+                bEmpty = false;
+                if (!it->first->hasTransform())
+                    primitive = it->second;
+                else
+                    bPrimitive = false;
+            }
+        }
+        if (bEmpty)
+            this->collisionShape_ = 0;
+        else if (bPrimitive)
+        {
+            // --> Only one shape to be added, no transform; return it directly
+            this->collisionShape_ = primitive;
+        }
+        else
+        {
+            // Make sure we use the compound shape when returning a btCollisionShape
+            this->collisionShape_ = this->compoundShape_;
+        }
+        this->updateParent();
+    }
+
+    void CompoundCollisionShape::updateParent()
+    {
+        if (this->parent_)
+            this->parent_->updateChildShape(this);
+        else
+        {
+            // We can do this, because the CompoundCollisionShape of a WorldEntity always belongs to it,
+            // as long as its lifetime.
+            WorldEntity* parent = dynamic_cast<WorldEntity*>(this->getCreator());
+            if (parent)
+                parent->notifyCollisionShapeChanged();
+        }
     }
 
     CollisionShape* CompoundCollisionShape::getChildShape(unsigned int index) const
     {
-        if (index < this->childShapes_.size())
-            return this->childShapes_[index];
-        else
-            return 0;
+        unsigned int i = 0;
+        for (std::map<CollisionShape*, btCollisionShape*>::const_iterator it = this->childShapes_.begin(); it != this->childShapes_.end(); ++it)
+        {
+            if (i == index)
+                return it->first;
+            ++i;
+        }
+        return 0;
     }
 }
Index: code/branches/physics/src/orxonox/objects/collisionshapes/CompoundCollisionShape.h
===================================================================
--- code/branches/physics/src/orxonox/objects/collisionshapes/CompoundCollisionShape.h	(revision 2407)
+++ code/branches/physics/src/orxonox/objects/collisionshapes/CompoundCollisionShape.h	(revision 2423)
@@ -45,15 +45,19 @@
             virtual void XMLPort(Element& xmlelement, XMLPort::Mode mode);
 
-            void addChildShape(CollisionShape* shape, bool bWorldEntityRoot = false);
+            void addChildShape(CollisionShape* shape);
+            void removeChildShape(CollisionShape* shape);
+            void removeAllChildShapes();
             CollisionShape* getChildShape(unsigned int index) const;
 
-            virtual btCollisionShape* getCollisionShape() const;
+            void updateChildShape(CollisionShape* shape);
 
-            inline bool empty() const
-                { return this->childShapes_.size() == 0; }
+        protected:
+            virtual void updateParent();
 
         private:
-            btCompoundShape*             compoundShape_;
-            std::vector<CollisionShape*> childShapes_;
+            void updatePublicShape();
+
+            btCompoundShape* compoundShape_;
+            std::map<CollisionShape*, btCollisionShape*> childShapes_;
     };
 }
Index: code/branches/physics/src/orxonox/objects/collisionshapes/PlaneCollisionShape.cc
===================================================================
--- code/branches/physics/src/orxonox/objects/collisionshapes/PlaneCollisionShape.cc	(revision 2407)
+++ code/branches/physics/src/orxonox/objects/collisionshapes/PlaneCollisionShape.cc	(revision 2423)
@@ -74,4 +74,5 @@
             delete this->collisionShape_;
         this->collisionShape_ = new btStaticPlaneShape(omni_cast<btVector3>(this->normal_), this->offset_);
+        this->updateParent();
     }
 }
Index: code/branches/physics/src/orxonox/objects/collisionshapes/SphereCollisionShape.cc
===================================================================
--- code/branches/physics/src/orxonox/objects/collisionshapes/SphereCollisionShape.cc	(revision 2407)
+++ code/branches/physics/src/orxonox/objects/collisionshapes/SphereCollisionShape.cc	(revision 2423)
@@ -72,5 +72,7 @@
         if (this->collisionShape_)
             delete this->collisionShape_;
+        // When we recreate the shape, we have to inform the parent about this to update the shape
         this->collisionShape_ = new btSphereShape(this->radius_);
+        this->updateParent();
     }
 }
