- cleaup adding/removing and tracking objects to the physics space

- warn user when objects are added or removed twice, do not allow this to happen

git-svn-id: https://jmonkeyengine.googlecode.com/svn/trunk@10337 75d07b2b-3a1a-0410-a2c5-0572b91ccdca

engine/src/bullet/com/jme3/bullet/PhysicsSpace.java

@@ -63,6 +63,7 @@ import java.util.logging.Logger;
  */
 public class PhysicsSpace {
     
+    private static final Logger logger = Logger.getLogger(PhysicsSpace.class.getName());
     public static final int AXIS_X = 0;
     public static final int AXIS_Y = 1;
     public static final int AXIS_Z = 2;
@@ -84,8 +85,13 @@ public class PhysicsSpace {
 //    private ConstraintSolver solver;
 //    private DefaultCollisionConfiguration collisionConfiguration;
 //    private Map<GhostObject, PhysicsGhostObject> physicsGhostNodes = new ConcurrentHashMap<GhostObject, PhysicsGhostObject>();
-    private Map<Long, PhysicsRigidBody> physicsNodes = new ConcurrentHashMap<Long, PhysicsRigidBody>();
+
-    private List<PhysicsJoint> physicsJoints = new LinkedList<PhysicsJoint>();
+    private Map<Long,  PhysicsGhostObject> physicsGhostObjects = new ConcurrentHashMap<Long, PhysicsGhostObject>();
+    private Map<Long,  PhysicsCharacter> physicsCharacters = new ConcurrentHashMap<Long, PhysicsCharacter>();
+    private Map<Long,  PhysicsRigidBody> physicsBodies = new ConcurrentHashMap<Long, PhysicsRigidBody>();
+    private Map<Long, PhysicsJoint> physicsJoints = new ConcurrentHashMap<Long, PhysicsJoint>();
+    private Map<Long, PhysicsVehicle> physicsVehicles = new ConcurrentHashMap<Long, PhysicsVehicle>();
+
     private List<PhysicsCollisionListener> collisionListeners = new LinkedList<PhysicsCollisionListener>();
     private List<PhysicsCollisionEvent> collisionEvents = new LinkedList<PhysicsCollisionEvent>();
     private Map<Integer, PhysicsCollisionGroupListener> collisionGroupListeners = new ConcurrentHashMap<Integer, PhysicsCollisionGroupListener>();
@@ -454,31 +460,23 @@ public class PhysicsSpace {
     public void addAll(Spatial spatial) {
         if (spatial.getControl(RigidBodyControl.class) != null) {
             RigidBodyControl physicsNode = spatial.getControl(RigidBodyControl.class);
-            if (!physicsNodes.containsValue(physicsNode)) {
+            physicsNode.setPhysicsSpace(this);
-                physicsNode.setPhysicsSpace(this);
-            }
             //add joints
             List<PhysicsJoint> joints = physicsNode.getJoints();
             for (Iterator<PhysicsJoint> it1 = joints.iterator(); it1.hasNext();) {
                 PhysicsJoint physicsJoint = it1.next();
                 //add connected physicsnodes if they are not already added
-                if (!physicsNodes.containsValue(physicsJoint.getBodyA())) {
+                if (physicsJoint.getBodyA() instanceof PhysicsControl) {
-                    if (physicsJoint.getBodyA() instanceof PhysicsControl) {
+                    add(physicsJoint.getBodyA());
-                        add(physicsJoint.getBodyA());
+                } else {
-                    } else {
+                    addRigidBody(physicsJoint.getBodyA());
-                        addRigidBody(physicsJoint.getBodyA());
-                    }
-                }
-                if (!physicsNodes.containsValue(physicsJoint.getBodyB())) {
-                    if (physicsJoint.getBodyA() instanceof PhysicsControl) {
-                        add(physicsJoint.getBodyB());
-                    } else {
-                        addRigidBody(physicsJoint.getBodyB());
-                    }
                 }
-                if (!physicsJoints.contains(physicsJoint)) {
+                if (physicsJoint.getBodyA() instanceof PhysicsControl) {
-                    addJoint(physicsJoint);
+                    add(physicsJoint.getBodyB());
+                } else {
+                    addRigidBody(physicsJoint.getBodyB());
                 }
+                addJoint(physicsJoint);
             }
         } else if (spatial.getControl(PhysicsControl.class) != null) {
             spatial.getControl(PhysicsControl.class).setPhysicsSpace(this);
@@ -501,31 +499,23 @@ public class PhysicsSpace {
     public void removeAll(Spatial spatial) {
         if (spatial.getControl(RigidBodyControl.class) != null) {
             RigidBodyControl physicsNode = spatial.getControl(RigidBodyControl.class);
-            if (physicsNodes.containsValue(physicsNode)) {
+            physicsNode.setPhysicsSpace(null);
-                physicsNode.setPhysicsSpace(null);
-            }
             //remove joints
             List<PhysicsJoint> joints = physicsNode.getJoints();
             for (Iterator<PhysicsJoint> it1 = joints.iterator(); it1.hasNext();) {
                 PhysicsJoint physicsJoint = it1.next();
                 //add connected physicsnodes if they are not already added
-                if (physicsNodes.containsValue(physicsJoint.getBodyA())) {
+                if (physicsJoint.getBodyA() instanceof PhysicsControl) {
-                    if (physicsJoint.getBodyA() instanceof PhysicsControl) {
+                    remove(physicsJoint.getBodyA());
-                        remove(physicsJoint.getBodyA());
+                } else {
-                    } else {
+                    removeRigidBody(physicsJoint.getBodyA());
-                        removeRigidBody(physicsJoint.getBodyA());
-                    }
-                }
-                if (physicsNodes.containsValue(physicsJoint.getBodyB())) {
-                    if (physicsJoint.getBodyA() instanceof PhysicsControl) {
-                        remove(physicsJoint.getBodyB());
-                    } else {
-                        removeRigidBody(physicsJoint.getBodyB());
-                    }
                 }
-                if (physicsJoints.contains(physicsJoint)) {
+                if (physicsJoint.getBodyA() instanceof PhysicsControl) {
-                    removeJoint(physicsJoint);
+                    remove(physicsJoint.getBodyB());
+                } else {
+                    removeRigidBody(physicsJoint.getBodyB());
                 }
+                removeJoint(physicsJoint);
             }
         } else if (spatial.getControl(PhysicsControl.class) != null) {
             spatial.getControl(PhysicsControl.class).setPhysicsSpace(null);
@@ -567,16 +557,31 @@ public class PhysicsSpace {
     private native void removeConstraint(long space, long id);
 
     private void addGhostObject(PhysicsGhostObject node) {
+        if(physicsGhostObjects.containsKey(node.getObjectId())){
+            logger.log(Level.WARNING, "GhostObject {0} already exists in PhysicsSpace, cannot add.", node);
+            return;
+        }
+        physicsGhostObjects.put(node.getObjectId(), node);
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Adding ghost object {0} to physics space.", Long.toHexString(node.getObjectId()));
         addCollisionObject(physicsSpaceId, node.getObjectId());
     }
 
     private void removeGhostObject(PhysicsGhostObject node) {
+        if(!physicsGhostObjects.containsKey(node.getObjectId())){
+            logger.log(Level.WARNING, "GhostObject {0} does not exist in PhysicsSpace, cannot remove.", node);
+            return;
+        }
+        physicsGhostObjects.remove(node.getObjectId());
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Removing ghost object {0} from physics space.", Long.toHexString(node.getObjectId()));
         removeCollisionObject(physicsSpaceId, node.getObjectId());
     }
 
     private void addCharacter(PhysicsCharacter node) {
+        if(physicsCharacters.containsKey(node.getObjectId())){
+            logger.log(Level.WARNING, "Character {0} already exists in PhysicsSpace, cannot add.", node);
+            return;
+        }
+        physicsCharacters.put(node.getObjectId(), node);
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Adding character {0} to physics space.", Long.toHexString(node.getObjectId()));
         addCharacterObject(physicsSpaceId, node.getObjectId());
         addAction(physicsSpaceId, node.getControllerId());
@@ -585,6 +590,11 @@ public class PhysicsSpace {
     }
 
     private void removeCharacter(PhysicsCharacter node) {
+        if(!physicsCharacters.containsKey(node.getObjectId())){
+            logger.log(Level.WARNING, "Character {0} does not exist in PhysicsSpace, cannot remove.", node);
+            return;
+        }
+        physicsCharacters.remove(node.getObjectId());
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Removing character {0} from physics space.", Long.toHexString(node.getObjectId()));
         removeAction(physicsSpaceId, node.getControllerId());
         removeCharacterObject(physicsSpaceId, node.getObjectId());
@@ -593,7 +603,11 @@ public class PhysicsSpace {
     }
 
     private void addRigidBody(PhysicsRigidBody node) {
-        physicsNodes.put(node.getObjectId(), node);
+        if(physicsBodies.containsKey(node.getObjectId())){
+            logger.log(Level.WARNING, "RigidBody {0} already exists in PhysicsSpace, cannot add.", node);
+            return;
+        }
+        physicsBodies.put(node.getObjectId(), node);
 
         //Workaround
         //It seems that adding a Kinematic RigidBody to the dynamicWorld prevent it from being non kinematic again afterward.
@@ -611,30 +625,44 @@ public class PhysicsSpace {
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Adding RigidBody {0} to physics space.", node.getObjectId());
         if (node instanceof PhysicsVehicle) {
             Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Adding vehicle constraint {0} to physics space.", Long.toHexString(((PhysicsVehicle) node).getVehicleId()));
+            physicsVehicles.put(((PhysicsVehicle) node).getVehicleId(), (PhysicsVehicle)node);
             addVehicle(physicsSpaceId, ((PhysicsVehicle) node).getVehicleId());
         }
     }
 
     private void removeRigidBody(PhysicsRigidBody node) {
+        if(!physicsBodies.containsKey(node.getObjectId())){
+            logger.log(Level.WARNING, "RigidBody {0} does not exist in PhysicsSpace, cannot remove.", node);
+            return;
+        }
         if (node instanceof PhysicsVehicle) {
             Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Removing vehicle constraint {0} from physics space.", Long.toHexString(((PhysicsVehicle) node).getVehicleId()));
+            physicsVehicles.remove(((PhysicsVehicle) node).getVehicleId());
             removeVehicle(physicsSpaceId, ((PhysicsVehicle) node).getVehicleId());
         }
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Removing RigidBody {0} from physics space.", Long.toHexString(node.getObjectId()));
-        physicsNodes.remove(node.getObjectId());
+        physicsBodies.remove(node.getObjectId());
         removeRigidBody(physicsSpaceId, node.getObjectId());
     }
 
     private void addJoint(PhysicsJoint joint) {
+        if(physicsJoints.containsKey(joint.getObjectId())){
+            logger.log(Level.WARNING, "Joint {0} already exists in PhysicsSpace, cannot add.", joint);
+            return;
+        }
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Adding Joint {0} to physics space.", Long.toHexString(joint.getObjectId()));
-        physicsJoints.add(joint);
+        physicsJoints.put(joint.getObjectId(), joint);
         addConstraintC(physicsSpaceId, joint.getObjectId(), !joint.isCollisionBetweenLinkedBodys());
 //        dynamicsWorld.addConstraint(joint.getObjectId(), !joint.isCollisionBetweenLinkedBodys());
     }
 
     private void removeJoint(PhysicsJoint joint) {
+        if(!physicsJoints.containsKey(joint.getObjectId())){
+            logger.log(Level.WARNING, "Joint {0} does not exist in PhysicsSpace, cannot remove.", joint);
+            return;
+        }
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Removing Joint {0} from physics space.", Long.toHexString(joint.getObjectId()));
-        physicsJoints.remove(joint);
+        physicsJoints.remove(joint.getObjectId());
         removeConstraint(physicsSpaceId, joint.getObjectId());
 //        dynamicsWorld.removeConstraint(joint.getObjectId());
     }
@@ -793,7 +821,7 @@ public class PhysicsSpace {
      * destroys the current PhysicsSpace so that a new one can be created
      */
     public void destroy() {
-        physicsNodes.clear();
+        physicsBodies.clear();
         physicsJoints.clear();
 
 //        dynamicsWorld.destroy();

engine/src/jbullet/com/jme3/bullet/PhysicsSpace.java

@@ -47,6 +47,8 @@ import com.bulletphysics.dynamics.InternalTickCallback;
 import com.bulletphysics.dynamics.RigidBody;
 import com.bulletphysics.dynamics.constraintsolver.ConstraintSolver;
 import com.bulletphysics.dynamics.constraintsolver.SequentialImpulseConstraintSolver;
+import com.bulletphysics.dynamics.constraintsolver.TypedConstraint;
+import com.bulletphysics.dynamics.vehicle.RaycastVehicle;
 import com.bulletphysics.extras.gimpact.GImpactCollisionAlgorithm;
 import com.jme3.app.AppTask;
 import com.jme3.asset.AssetManager;
@@ -81,6 +83,7 @@ import java.util.logging.Logger;
  */
 public class PhysicsSpace {
 
+    private static final Logger logger = Logger.getLogger(PhysicsSpace.class.getName());
     public static final int AXIS_X = 0;
     public static final int AXIS_Y = 1;
     public static final int AXIS_Z = 2;
@@ -100,13 +103,15 @@ public class PhysicsSpace {
     private CollisionDispatcher dispatcher;
     private ConstraintSolver solver;
     private DefaultCollisionConfiguration collisionConfiguration;
-//    private Map<GhostObject, PhysicsGhostObject> physicsGhostNodes = new ConcurrentHashMap<GhostObject, PhysicsGhostObject>();
+    private Map<PairCachingGhostObject, PhysicsGhostObject> physicsGhostObjects = new ConcurrentHashMap<PairCachingGhostObject, PhysicsGhostObject>();
-    private Map<RigidBody, PhysicsRigidBody> physicsNodes = new ConcurrentHashMap<RigidBody, PhysicsRigidBody>();
+    private Map<PairCachingGhostObject, PhysicsCharacter> physicsCharacters = new ConcurrentHashMap<PairCachingGhostObject, PhysicsCharacter>();
-    private List<PhysicsJoint> physicsJoints = new LinkedList<PhysicsJoint>();
+    private Map<RigidBody, PhysicsRigidBody> physicsBodies = new ConcurrentHashMap<RigidBody, PhysicsRigidBody>();
-    private List<PhysicsCollisionListener> collisionListeners = new LinkedList<PhysicsCollisionListener>();
+    private Map<TypedConstraint, PhysicsJoint> physicsJoints = new ConcurrentHashMap<TypedConstraint, PhysicsJoint>();
-    private List<PhysicsCollisionEvent> collisionEvents = new LinkedList<PhysicsCollisionEvent>();
+    private Map<RaycastVehicle, PhysicsVehicle> physicsVehicles = new ConcurrentHashMap<RaycastVehicle, PhysicsVehicle>();
     private Map<Integer, PhysicsCollisionGroupListener> collisionGroupListeners = new ConcurrentHashMap<Integer, PhysicsCollisionGroupListener>();
     private ConcurrentLinkedQueue<PhysicsTickListener> tickListeners = new ConcurrentLinkedQueue<PhysicsTickListener>();
+    private List<PhysicsCollisionListener> collisionListeners = new LinkedList<PhysicsCollisionListener>();
+    private List<PhysicsCollisionEvent> collisionEvents = new LinkedList<PhysicsCollisionEvent>();
     private PhysicsCollisionEventFactory eventFactory = new PhysicsCollisionEventFactory();
     private Vector3f worldMin = new Vector3f(-10000f, -10000f, -10000f);
     private Vector3f worldMax = new Vector3f(10000f, 10000f, 10000f);
@@ -428,31 +433,23 @@ public class PhysicsSpace {
     public void addAll(Spatial spatial) {
         if (spatial.getControl(RigidBodyControl.class) != null) {
             RigidBodyControl physicsNode = spatial.getControl(RigidBodyControl.class);
-            if (!physicsNodes.containsValue(physicsNode)) {
+            physicsNode.setPhysicsSpace(this);
-                physicsNode.setPhysicsSpace(this);
-            }
             //add joints
             List<PhysicsJoint> joints = physicsNode.getJoints();
             for (Iterator<PhysicsJoint> it1 = joints.iterator(); it1.hasNext();) {
                 PhysicsJoint physicsJoint = it1.next();
                 //add connected physicsnodes if they are not already added
-                if (!physicsNodes.containsValue(physicsJoint.getBodyA())) {
+                if (physicsJoint.getBodyA() instanceof PhysicsControl) {
-                    if (physicsJoint.getBodyA() instanceof PhysicsControl) {
+                    add(physicsJoint.getBodyA());
-                        add(physicsJoint.getBodyA());
+                } else {
-                    } else {
+                    addRigidBody(physicsJoint.getBodyA());
-                        addRigidBody(physicsJoint.getBodyA());
-                    }
-                }
-                if (!physicsNodes.containsValue(physicsJoint.getBodyB())) {
-                    if (physicsJoint.getBodyA() instanceof PhysicsControl) {
-                        add(physicsJoint.getBodyB());
-                    } else {
-                        addRigidBody(physicsJoint.getBodyB());
-                    }
                 }
-                if (!physicsJoints.contains(physicsJoint)) {
+                if (physicsJoint.getBodyA() instanceof PhysicsControl) {
-                    addJoint(physicsJoint);
+                    add(physicsJoint.getBodyB());
+                } else {
+                    addRigidBody(physicsJoint.getBodyB());
                 }
+                addJoint(physicsJoint);
             }
         } else if (spatial.getControl(PhysicsControl.class) != null) {
             spatial.getControl(PhysicsControl.class).setPhysicsSpace(this);
@@ -475,31 +472,23 @@ public class PhysicsSpace {
     public void removeAll(Spatial spatial) {
         if (spatial.getControl(RigidBodyControl.class) != null) {
             RigidBodyControl physicsNode = spatial.getControl(RigidBodyControl.class);
-            if (physicsNodes.containsValue(physicsNode)) {
+            physicsNode.setPhysicsSpace(null);
-                physicsNode.setPhysicsSpace(null);
-            }
             //remove joints
             List<PhysicsJoint> joints = physicsNode.getJoints();
             for (Iterator<PhysicsJoint> it1 = joints.iterator(); it1.hasNext();) {
                 PhysicsJoint physicsJoint = it1.next();
                 //add connected physicsnodes if they are not already added
-                if (physicsNodes.containsValue(physicsJoint.getBodyA())) {
+                if (physicsJoint.getBodyA() instanceof PhysicsControl) {
-                    if (physicsJoint.getBodyA() instanceof PhysicsControl) {
+                    remove(physicsJoint.getBodyA());
-                        remove(physicsJoint.getBodyA());
+                } else {
-                    } else {
+                    removeRigidBody(physicsJoint.getBodyA());
-                        removeRigidBody(physicsJoint.getBodyA());
-                    }
-                }
-                if (physicsNodes.containsValue(physicsJoint.getBodyB())) {
-                    if (physicsJoint.getBodyA() instanceof PhysicsControl) {
-                        remove(physicsJoint.getBodyB());
-                    } else {
-                        removeRigidBody(physicsJoint.getBodyB());
-                    }
                 }
-                if (physicsJoints.contains(physicsJoint)) {
+                if (physicsJoint.getBodyA() instanceof PhysicsControl) {
-                    removeJoint(physicsJoint);
+                    remove(physicsJoint.getBodyB());
+                } else {
+                    removeRigidBody(physicsJoint.getBodyB());
                 }
+                removeJoint(physicsJoint);
             }
         } else if (spatial.getControl(PhysicsControl.class) != null) {
             spatial.getControl(PhysicsControl.class).setPhysicsSpace(null);
@@ -515,30 +504,53 @@ public class PhysicsSpace {
     }
 
     private void addGhostObject(PhysicsGhostObject node) {
+        if(physicsGhostObjects.containsKey(node.getObjectId())){
+            logger.log(Level.WARNING, "GhostObject {0} already exists in PhysicsSpace, cannot add.", node);
+            return;
+        }
+        physicsGhostObjects.put(node.getObjectId(), node);
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Adding ghost object {0} to physics space.", node.getObjectId());
         dynamicsWorld.addCollisionObject(node.getObjectId());
     }
 
     private void removeGhostObject(PhysicsGhostObject node) {
+        if(!physicsGhostObjects.containsKey(node.getObjectId())){
+            logger.log(Level.WARNING, "GhostObject {0} does not exist in PhysicsSpace, cannot remove.", node);
+            return;
+        }
+        physicsGhostObjects.remove(node.getObjectId());
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Removing ghost object {0} from physics space.", node.getObjectId());
         dynamicsWorld.removeCollisionObject(node.getObjectId());
     }
 
     private void addCharacter(PhysicsCharacter node) {
+        if(physicsCharacters.containsKey(node.getObjectId())){
+            logger.log(Level.WARNING, "Character {0} already exists in PhysicsSpace, cannot add.", node);
+            return;
+        }
+        physicsCharacters.put(node.getObjectId(), node);
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Adding character {0} to physics space.", node.getObjectId());
-//        dynamicsWorld.addCollisionObject(node.getObjectId());
         dynamicsWorld.addCollisionObject(node.getObjectId(), CollisionFilterGroups.CHARACTER_FILTER, (short) (CollisionFilterGroups.STATIC_FILTER | CollisionFilterGroups.DEFAULT_FILTER));
         dynamicsWorld.addAction(node.getControllerId());
     }
 
     private void removeCharacter(PhysicsCharacter node) {
+        if(!physicsCharacters.containsKey(node.getObjectId())){
+            logger.log(Level.WARNING, "Character {0} does not exist in PhysicsSpace, cannot remove.", node);
+            return;
+        }
+        physicsCharacters.remove(node.getObjectId());
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Removing character {0} from physics space.", node.getObjectId());
         dynamicsWorld.removeAction(node.getControllerId());
         dynamicsWorld.removeCollisionObject(node.getObjectId());
     }
 
     private void addRigidBody(PhysicsRigidBody node) {
-        physicsNodes.put(node.getObjectId(), node);
+        if(physicsBodies.containsKey(node.getObjectId())){
+            logger.log(Level.WARNING, "RigidBody {0} already exists in PhysicsSpace, cannot add.", node);
+            return;
+        }
+        physicsBodies.put(node.getObjectId(), node);
 
         //Workaround
         //It seems that adding a Kinematic RigidBody to the dynamicWorld prevent it from being non kinematic again afterward.
@@ -557,29 +569,43 @@ public class PhysicsSpace {
         if (node instanceof PhysicsVehicle) {
             Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Adding vehicle constraint {0} to physics space.", ((PhysicsVehicle) node).getVehicleId());
             ((PhysicsVehicle) node).createVehicle(this);
+            physicsVehicles.put(((PhysicsVehicle) node).getVehicleId(), (PhysicsVehicle)node);
             dynamicsWorld.addVehicle(((PhysicsVehicle) node).getVehicleId());
         }
     }
 
     private void removeRigidBody(PhysicsRigidBody node) {
+        if(!physicsBodies.containsKey(node.getObjectId())){
+            logger.log(Level.WARNING, "RigidBody {0} does not exist in PhysicsSpace, cannot remove.", node);
+            return;
+        }
         if (node instanceof PhysicsVehicle) {
             Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Removing vehicle constraint {0} from physics space.", ((PhysicsVehicle) node).getVehicleId());
+            physicsVehicles.remove(((PhysicsVehicle) node).getVehicleId());
             dynamicsWorld.removeVehicle(((PhysicsVehicle) node).getVehicleId());
         }
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Removing RigidBody {0} from physics space.", node.getObjectId());
-        physicsNodes.remove(node.getObjectId());
+        physicsBodies.remove(node.getObjectId());
         dynamicsWorld.removeRigidBody(node.getObjectId());
     }
 
     private void addJoint(PhysicsJoint joint) {
+        if(physicsJoints.containsKey(joint.getObjectId())){
+            logger.log(Level.WARNING, "Joint {0} already exists in PhysicsSpace, cannot add.", joint);
+            return;
+        }
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Adding Joint {0} to physics space.", joint.getObjectId());
-        physicsJoints.add(joint);
+        physicsJoints.put(joint.getObjectId(), joint);
         dynamicsWorld.addConstraint(joint.getObjectId(), !joint.isCollisionBetweenLinkedBodys());
     }
 
     private void removeJoint(PhysicsJoint joint) {
+        if(!physicsJoints.containsKey(joint.getObjectId())){
+            logger.log(Level.WARNING, "Joint {0} does not exist in PhysicsSpace, cannot remove.", joint);
+            return;
+        }
         Logger.getLogger(PhysicsSpace.class.getName()).log(Level.FINE, "Removing Joint {0} from physics space.", joint.getObjectId());
-        physicsJoints.remove(joint);
+        physicsJoints.remove(joint.getObjectId());
         dynamicsWorld.removeConstraint(joint.getObjectId());
     }
 
@@ -734,7 +760,7 @@ public class PhysicsSpace {
      * destroys the current PhysicsSpace so that a new one can be created
      */
     public void destroy() {
-        physicsNodes.clear();
+        physicsBodies.clear();
         physicsJoints.clear();
 
         dynamicsWorld.destroy();