Michael Ludwig avatar Michael Ludwig committed 51cac9f

Convert CollisionBody and RigidBody into component definitions, refactor packages in preparation for controller re-write.

Comments (0)

Files changed (24)

ferox-physics/src/main/java/com/ferox/physics/collision/AxisSweepCollisionManager.java

-package com.ferox.physics.collision;
-
-public class AxisSweepCollisionManager {
-
-}

ferox-physics/src/main/java/com/ferox/physics/collision/Collidable.java

-package com.ferox.physics.collision;
-
-import java.util.BitSet;
-
-import com.ferox.math.ReadOnlyMatrix4f;
-import com.ferox.math.AffineTransform;
-import com.ferox.math.bounds.ReadOnlyAxisAlignedBox;
-
-/**
- * <p>
- * Collidable represents an instance of an object in a physics simulation
- * capable of being collided with. It has both {@link Shape shape} and a
- * {@link ReadOnlyMatrix4f transform} to describe its local geometry and its
- * world position and orientation. Additionally, it has pseudo-physical
- * parameters for determining its surface's friction coefficients and collision
- * response (e.g. elastic or inelastic).
- * </p>
- * <p>
- * Collidables also have configurable "groups" which can restrict the sets of
- * objects that can collide with each other. This is useful for supporting
- * no-clip like features in multiplyer games for members of the same team. Each
- * Collidable belongs to some number of integer groups; they also have a bit
- * mask specifying which groups they can collide against. When considering a
- * pair of Collidables for collision, the pair can collide if any of either
- * instance's groups are in the opposite's collision mask. A Collidable with no
- * group cannot collide with anything.
- * </p>
- * <p>
- * Individual instances of Collidable are not thread-safe.
- * </p>
- * 
- * @author Michael Ludwig
- */
-public class Collidable {
-    private final BitSet collisionGroups;
-    private final BitSet collisionMask;
-
-    private final AffineTransform worldTransform;
-    private final ReadOnlyAxisAlignedBox worldAabb;
-    private Shape bounds;
-    
-    private float restitution;
-    private float friction;
-
-    /**
-     * Create a Collidable initially placed at the given position and
-     * orientation stored in <tt>t</tt>, using the provided Shape. It is assumed
-     * that the transform is an affine transform. It is recommended that
-     * Collidables share the same instance of Shapes when possible. The created
-     * Collidable initially uses a friction value of .5 and a restitution of 0.
-     * 
-     * @param t The initial location
-     * @param shape The initial shape
-     * @throws NullPointerException if t or shape are null
-     */
-    public Collidable(ReadOnlyMatrix4f t, Shape shape) {
-        if (t == null)
-            throw new NullPointerException("AffineTransform cannot be null");
-        if (shape == null)
-            throw new NullPointerException("Shape cannot be null");
-        
-        worldTransform = new AffineTransform(t);
-        bounds = shape;
-        worldAabb = new ReadOnlyAxisAlignedBox();
-        
-        collisionGroups = new BitSet();
-        collisionMask = new BitSet();
-        
-        // every Collidable starts out in a group
-        setMemberOfGroup(0, true);
-        setCollidesWithGroup(0, true);
-        
-        setFriction(.5f);
-        setRestitution(0f);
-        updateBounds();
-    }
-
-    /**
-     * Return the friction coefficient for the surface of this Collidable. This
-     * is a pseudo-physical value combining both static and kinetic friction. A
-     * value of 0 represents no friction and higher values represent rougher
-     * surfaces.
-     * 
-     * @return The current friction coefficient
-     */
-    public float getFriction() {
-        return friction;
-    }
-
-    /**
-     * Set the friction coefficient. See {@link #getFriction()} for more
-     * details.
-     * 
-     * @param friction The new friction value
-     * @throws IllegalArgumentException if friction is less than 0
-     */
-    public void setFriction(float friction) {
-        if (friction < 0f)
-            throw new IllegalArgumentException("Friction coefficient must be at least 0, not: " + friction);
-        this.friction = friction;
-    }
-
-    /**
-     * Get the restitution coefficient of this Collidable. Restitution is an
-     * approximation of the elasticity of a surface. A restitution of 0
-     * represents a fully inelastic collision. Higher values represent more
-     * elastic collisions.
-     * 
-     * @return The current restitution coefficient
-     */
-    public float getRestitution() {
-        return restitution;
-    }
-
-    /**
-     * Set the new restitution coefficient. See {@link #getRestitution()} for
-     * more details.
-     * 
-     * @param restitution The new restitution value
-     * @throws IllegalArgumentException if restitution is less than 0
-     */
-    public void setRestitution(float restitution) {
-        if (restitution < 0f)
-            throw new IllegalArgumentException("Restitution coefficient must be at least 0, not: " + restitution);
-        this.restitution = restitution;
-    }
-
-    /**
-     * Compares the collision groups and masks of this Collidable and
-     * <tt>other</tt> and returns true if the two instances are capable of
-     * colliding. It is always true that
-     * <code>objA.canCollide(objB) == objB.canCollide(objA)</code>.
-     * 
-     * @param other The other Collidable to check
-     * @return True if the pair can collide
-     * @throws NullPointerException if other is null
-     */
-    public boolean canCollide(Collidable other) {
-        if (other == null)
-            throw new NullPointerException("Collidable cannot be null");
-        
-        return collisionGroups.intersects(other.collisionMask) ||
-               collisionMask.intersects(other.collisionGroups);
-    }
-
-    /**
-     * Check if this Collidable is the member of the given integer group.
-     * 
-     * @param group The group, must be at least 0
-     * @return True if the Collidable is in the group
-     * @throws IndexOutOfBoundsException if the group is negative
-     */
-    public boolean isMemberOfGroup(int group) {
-        return collisionGroups.get(group);
-    }
-
-    /**
-     * Check if this Collidable is allowed to collide with specified group.
-     * 
-     * @param group The group to check
-     * @return True if the Collidable can collide with the group
-     * @throws IndexOutOfBoundsException if the group is negative
-     */
-    public boolean canCollideWithGroup(int group) {
-        return collisionMask.get(group);
-    }
-
-    /**
-     * Set whether or not this Collidable is the member of the provided group.
-     * 
-     * @param group The group that this Collidable is added to or removed from
-     * @param member True if the Collidable should be a member
-     * @throws IndexOutOfBoundsException if group is negative
-     */
-    public void setMemberOfGroup(int group, boolean member) {
-        collisionGroups.set(group, member);
-    }
-    
-    /**
-     * Set whether or not this Collidable can collide with the provided group.
-     * 
-     * @param group The group that this Collidable can collide against
-     * @param member True if the Collidable should collide with the group
-     * @throws IndexOutOfBoundsException if group is negative
-     */
-    public void setCollidesWithGroup(int group, boolean collide) {
-        collisionMask.set(group, collide);
-    }
-
-    /**
-     * Return the matrix instance holding this Collidable's world transform. The
-     * returned instance can be modified at any time as this is not a defensive
-     * copy.
-     * 
-     * @return The world transform of the Collidable
-     */
-    public ReadOnlyMatrix4f getTransform() {
-        return worldTransform;
-    }
-
-    /**
-     * Assign a new Shape to use for this Collidable.
-     * 
-     * @param shape The new Shape
-     * @throws NullPointerException if shape is null
-     */
-    public void setShape(Shape shape) {
-        if (shape == null)
-            throw new NullPointerException("Shape cannot be null");
-        bounds = shape;
-        updateBounds();
-    }
-
-    /**
-     * Return the current Shape of this Collidable.
-     * 
-     * @return The shape of this Collidable
-     */
-    public Shape getShape() {
-        return bounds;
-    }
-
-    /**
-     * Copy <tt>t</tt> into this Collidable's transform, updating its location
-     * and orientation. This will also recompute the Collidable's world bounds.
-     * 
-     * @param t The transform to copy
-     * @throws NullPointerException if t is null
-     */
-    public void setTransform(ReadOnlyMatrix4f t) {
-        if (t == null)
-            throw new NullPointerException("AffineTransform cannot be null");
-        worldTransform.set(t);
-        updateBounds();
-    }
-
-    /**
-     * Return the current world bounds of this Collidable. This is computed
-     * based off of the Shape's local bounds and the current world transform.
-     * The returned instance can be updated at any point in time to reflect
-     * changes to the shape or transform, and should be considered read-only.
-     * 
-     * @return The world bounds of this Collidable
-     */
-    public ReadOnlyAxisAlignedBox getWorldBounds() {
-        return worldAabb;
-    }
-    
-    private void updateBounds() {
-        bounds.getBounds().transform(worldTransform, worldAabb);
-    }
-}

ferox-physics/src/main/java/com/ferox/physics/collision/CollisionAlgorithm.java

  * CollisionAlgorithm is an interface encapsulating the narrow-phase of a
  * collision detection system. CollisionAlgorithm implementations are
  * responsible for computing two vectors within world space. Each vector
- * represents the closest point on one {@link Collidable} to the other.
+ * represents the closest point on one {@link CollisionBody} to the other.
  * Implementations must handle cases where the two objects are intersecting each
  * other as well.
  * 

ferox-physics/src/main/java/com/ferox/physics/collision/CollisionBody.java

+package com.ferox.physics.collision;
+
+import com.ferox.math.AxisAlignedBox;
+import com.ferox.math.Const;
+import com.ferox.math.Matrix4;
+import com.ferox.math.entreri.AxisAlignedBoxProperty;
+import com.ferox.math.entreri.Matrix4Property;
+import com.ferox.math.entreri.Matrix4Property.DefaultMatrix4;
+import com.lhkbob.entreri.ComponentData;
+import com.lhkbob.entreri.Unmanaged;
+import com.lhkbob.entreri.property.DoubleProperty;
+import com.lhkbob.entreri.property.DoubleProperty.DefaultDouble;
+import com.lhkbob.entreri.property.LongProperty;
+import com.lhkbob.entreri.property.LongProperty.DefaultLong;
+import com.lhkbob.entreri.property.ObjectProperty;
+
+/**
+ * <p>
+ * CollisionBody represents an instance of an object in a physics simulation
+ * capable of being collided with. It has both {@link Shape shape} and a
+ * {@link Matrix4 transform} to describe its local geometry and its world
+ * position and orientation. Additionally, it has pseudo-physical parameters for
+ * determining its surface's friction coefficients and collision response (e.g.
+ * elastic or inelastic).
+ * <p>
+ * CollisionBodies also have configurable "groups" which can restrict the sets
+ * of objects that can collide with each other. This is useful for supporting
+ * no-clip like features in multiplyer games for members of the same team. Each
+ * CollisionBody belongs to some number of integer groups; they also have a bit
+ * mask specifying which groups they can collide against. When considering a
+ * pair of CollisionBodies for collision, the pair can collide if any of either
+ * instance's groups are in the opposite's collision mask. A CollisionBody with
+ * no group cannot collide with anything.
+ * 
+ * @author Michael Ludwig
+ */
+public class CollisionBody extends ComponentData<CollisionBody> {
+    /**
+     * Internally CollisionBody stores the groups inside a single long, so group
+     * ids are limited to be between 0 and 63.
+     */
+    public static final int MAX_GROUPS = 64;
+    
+    @DefaultMatrix4(m00=1.0, m01=0.0, m02=0.0, m03=0.0,
+                    m10=0.0, m11=1.0, m12=0.0, m13=0.0,
+                    m20=0.0, m21=0.0, m22=1.0, m23=0.0,
+                    m30=0.0, m31=0.0, m32=0.0, m33=1.0)
+    private Matrix4Property worldTransform;
+    
+    private AxisAlignedBoxProperty worldBounds;
+    private ObjectProperty<Shape> shape;
+    
+    @DefaultDouble(0.0)
+    private DoubleProperty restitution;
+    
+    @DefaultDouble(0.5)
+    private DoubleProperty friction;
+    
+    @DefaultLong(1)
+    private LongProperty collisionGroups;
+    
+    @DefaultLong(1)
+    private LongProperty collisionMask;
+    
+    @Unmanaged private final AxisAlignedBox boundsCache = new AxisAlignedBox();
+    @Unmanaged private final Matrix4 transformCache = new Matrix4();
+
+    private CollisionBody() { }
+
+    /**
+     * Return the friction coefficient for the surface of this CollisionBody. This
+     * is a pseudo-physical value combining both static and kinetic friction. A
+     * value of 0 represents no friction and higher values represent rougher
+     * surfaces.
+     * 
+     * @return The current friction coefficient
+     */
+    public double getFriction() {
+        return friction.get(getIndex());
+    }
+
+    /**
+     * Set the friction coefficient. See {@link #getFriction()} for more
+     * details.
+     * 
+     * @param friction The new friction value
+     * @return This instance
+     * @throws IllegalArgumentException if friction is less than 0
+     */
+    public CollisionBody setFriction(double friction) {
+        if (friction < 0.0)
+            throw new IllegalArgumentException("Friction coefficient must be at least 0, not: " + friction);
+        this.friction.set(friction, getIndex());
+        return this;
+    }
+
+    /**
+     * Get the restitution coefficient of this CollisionBody. Restitution is an
+     * approximation of the elasticity of a surface. A restitution of 0
+     * represents a fully inelastic collision. Higher values represent more
+     * elastic collisions.
+     * 
+     * @return The current restitution coefficient
+     */
+    public double getRestitution() {
+        return restitution.get(getIndex());
+    }
+
+    /**
+     * Set the new restitution coefficient. See {@link #getRestitution()} for
+     * more details.
+     * 
+     * @param restitution The new restitution value
+     * @return This instance
+     * @throws IllegalArgumentException if restitution is less than 0
+     */
+    public CollisionBody setRestitution(double restitution) {
+        if (restitution < 0.0)
+            throw new IllegalArgumentException("Restitution coefficient must be at least 0, not: " + restitution);
+        this.restitution.set(restitution, getIndex());
+        return this;
+    }
+
+    /**
+     * Compares the collision groups and masks of this CollisionBody and
+     * <tt>other</tt> and returns true if the two instances are capable of
+     * colliding. It is always true that
+     * <code>objA.canCollide(objB) == objB.canCollide(objA)</code>.
+     * 
+     * @param other The other CollisionBody to check
+     * @return True if the pair can collide
+     * @throws NullPointerException if other is null
+     */
+    public boolean canCollide(CollisionBody other) {
+        if (other == null)
+            throw new NullPointerException("Collidable cannot be null");
+        
+        return isSet(collisionGroups.get(getIndex()), other.collisionMask.get(other.getIndex())) ||
+               isSet(other.collisionGroups.get(other.getIndex()), collisionMask.get(getIndex()));
+    }
+
+    private static boolean isSet(long groups, long mask) {
+        return (groups & mask) != 0;
+    }
+    
+    private static long set(long groups, long group, boolean set) {
+        if (set)
+            return groups | group;
+        else
+            return groups & ~group;
+    }
+    
+    /**
+     * Check if this CollisionBody is the member of the given integer group. All
+     * CollisionBodies are in group 0 by default.
+     * 
+     * @param group The group, must be at least 0
+     * @return True if the CollisionBody is in the group
+     * @throws IndexOutOfBoundsException if the group is negative or greater
+     *             than MAX_GROUPS
+     */
+    public boolean isMemberOfGroup(int group) {
+        if (group < 0 || group >= MAX_GROUPS)
+            throw new IndexOutOfBoundsException("Group must be in range [0, " + MAX_GROUPS + "), but was " + group);
+        long groups = collisionGroups.get(getIndex());
+        return isSet(groups, 1 << group);
+    }
+
+    /**
+     * Check if this CollisionBody is allowed to collide with specified group.
+     * 
+     * @param group The group to check
+     * @return True if the CollisionBody can collide with the group
+     * @throws IndexOutOfBoundsException if the group is negative or greater
+     *             than MAX_GROUPS
+     */
+    public boolean canCollideWithGroup(int group) {
+        if (group < 0 || group >= MAX_GROUPS)
+            throw new IndexOutOfBoundsException("Group must be in range [0, " + MAX_GROUPS + "), but was " + group);
+        long mask = collisionMask.get(getIndex());
+        return isSet(1 << group, mask);
+    }
+
+    /**
+     * Set whether or not this CollisionBody is the member of the provided
+     * group.
+     * 
+     * @param group The group that this CollisionBody is added to or removed
+     *            from
+     * @param member True if the CollisionBody should be a member
+     * @throws IndexOutOfBoundsException if group is negative or greater than
+     *             MAX_GROUPS
+     */
+    public void setMemberOfGroup(int group, boolean member) {
+        if (group < 0 || group >= MAX_GROUPS)
+            throw new IndexOutOfBoundsException("Group must be in range [0, " + MAX_GROUPS + "), but was " + group);
+        collisionGroups.set(set(collisionGroups.get(getIndex()), 1 << group, member), getIndex());
+    }
+    
+    /**
+     * Set whether or not this CollisionBody can collide with the provided
+     * group.
+     * 
+     * @param group The group that this CollisionBody can collide against
+     * @param member True if the CollisionBody should collide with the group
+     * @throws IndexOutOfBoundsException if group is negative or greater than
+     *             MAX_GROUPS
+     */
+    public void setCollidesWithGroup(int group, boolean collide) {
+        if (group < 0 || group >= MAX_GROUPS)
+            throw new IndexOutOfBoundsException("Group must be in range [0, " + MAX_GROUPS + "), but was " + group);
+        collisionMask.set(set(collisionMask.get(getIndex()), 1 << group, collide), getIndex());
+    }
+
+    /**
+     * Return the matrix instance holding this Collidable's world transform.
+     * 
+     * @return The world transform of the Collidable
+     */
+    public @Const Matrix4 getTransform() {
+        return transformCache;
+    }
+
+    /**
+     * Assign a new Shape to use for this Collidable.
+     * 
+     * @param shape The new Shape
+     * @throws NullPointerException if shape is null
+     */
+    public void setShape(Shape shape) {
+        if (shape == null)
+            throw new NullPointerException("Shape cannot be null");
+        this.shape.set(shape, getIndex());
+        updateBounds();
+    }
+
+    /**
+     * Return the current Shape of this Collidable.
+     * 
+     * @return The shape of this Collidable
+     */
+    public Shape getShape() {
+        return shape.get(getIndex());
+    }
+
+    /**
+     * Copy <tt>t</tt> into this Collidable's transform, updating its location
+     * and orientation. This will also recompute the Collidable's world bounds.
+     * 
+     * @param t The transform to copy
+     * @throws NullPointerException if t is null
+     */
+    public void setTransform(@Const Matrix4 t) {
+        transformCache.set(t);
+        worldTransform.set(t, getIndex());
+        updateBounds();
+    }
+
+    /**
+     * Return the current world bounds of this CollisionBody. This is computed
+     * based off of the Shape's local bounds and the current world transform.
+     * 
+     * @return The world bounds of this CollisionBody
+     */
+    public @Const AxisAlignedBox getWorldBounds() {
+        return boundsCache;
+    }
+    
+    private void updateBounds() {
+        Shape shape = getShape();
+        if (shape != null) {
+            // do the if check just to be nice, if someone calls the 
+            // setTransform() before setShape(), we really don't want to throw
+            // a weird exception
+            boundsCache.transform(shape.getBounds(), getTransform());
+            worldBounds.set(boundsCache, getIndex());
+        }
+    }
+    
+    @Override
+    protected void onSet(int index) {
+        worldTransform.get(index, transformCache);
+        worldBounds.get(index, boundsCache);
+    }
+}

ferox-physics/src/main/java/com/ferox/physics/collision/CollisionCallback.java

-package com.ferox.physics.collision;
-
-
-/**
- * <p>
- * CollisionCallbacks are custom callbacks that are invoked by a
- * CollisionManager on every potentially intersecting object registered with the
- * CollisionManager. The set of potentially colliding objects is a superset of
- * the set of intersecting objects. A CollisionAlgorithmProvider is provided to the
- * callback to distinguish between true intersecting objects and those that
- * aren't.
- * </p>
- * <p>
- * A CollisionManager implementation is allowed to invoke a callback on multiple
- * threads or from a different thread than the one calling
- * {@link CollisionManager#processCollisions(CollisionCallback)}, so it is
- * important for CollisionCallback implementations to be thread-safe.
- * </p>
- * 
- * @author Michael Ludwig
- */
-public interface CollisionCallback {
-    /**
-     * <p>
-     * Process the given pair of Collidables. This is invoked by
-     * CollisionManagers when their processCollisions() method is called with
-     * this Callback. The pair, <tt>objA</tt> and <tt>objB</tt>, may or may not
-     * be actually intersecting. The provided CollisionAlgorithmProvider can be
-     * used to generate {@link ClosestPair ClosestPairs} containing correct
-     * intersecting information.
-     * </p>
-     * <p>
-     * The ordering of the Collidables may not be consistent and is meaningless,
-     * e.g. (<tt>objA</tt>, <tt>objB</tt>) should processed the same as (
-     * <tt>objB</tt>, <tt>objA</tt>).
-     * </p>
-     * 
-     * @param objA The first object in the pair
-     * @param objB The second object in the pair
-     * @param handler The currently configured CollisionAlgorithmProvider of the
-     *            CollisionManager
-     * @throws NullPointerException if any argument is null
-     */
-    public void process(Collidable objA, Collidable objB, CollisionAlgorithmProvider handler);
-}

ferox-physics/src/main/java/com/ferox/physics/collision/CollisionController.java

+package com.ferox.physics.collision;
+
+import com.lhkbob.entreri.SimpleController;
+
+public abstract class CollisionController extends SimpleController {
+// FIXME implement contact manifold logic so that it can easily be 
+    // shared by other collision controllers
+}

ferox-physics/src/main/java/com/ferox/physics/collision/CollisionManager.java

-package com.ferox.physics.collision;
-
-/**
- * <p>
- * CollisionManagers are responsible for providing efficient access to
- * potentially intersecting {@link Collidable Collidables}. Collidables can be
- * added to the manager, at which point they are elligable for collision
- * detection with the other objects in the manager. Potentially intersecting
- * objects should be determined by the world bounds of each Collidable.
- * </p>
- * <p>
- * A {@link CollisionAlgorithmProvider} is responsible for narrowing the set potentially
- * intersecting objects down to those that are actually intersecting.
- * {@link CollisionCallback CollisionCallbacks} are used by the CollisionManager
- * to report each pair of objects that might be intersecting. Implementations
- * are free to exclude pairs that do not have their bounds intersecting.
- * </p>
- * <p>
- * CollisionManagers must be thread-safe. Additionally, they are allowed to
- * execute CollisionCallbacks on multiple and/or different threads when
- * processing the collision.
- * </p>
- * 
- * @author Michael Ludwig
- */
-public interface CollisionManager {
-    /**
-     * Register a Collidable with this CollisionManager. Adding a Collidable
-     * that already is registered should a no-op. In the context of a larger
-     * physics simulation, registering Collidables will most likely be managed
-     * by the simulator and will not need to be done manually.
-     * 
-     * @param collidable The object to add to the CollisionManager
-     * @throws NullPointerException if collidable is null
-     */
-    public void add(Collidable collidable);
-
-    /**
-     * Remove a Collidable from the CollisionManager. If the Collidable is not
-     * registered with this CollisionManager, then this is a no-op.
-     * 
-     * @param collidable The object to remove
-     * @throws NullPointerException if collidable is null
-     */
-    public void remove(Collidable collidable);
-
-    /**
-     * Run the given CollisionCallback on every potentially intersecting pair of
-     * registered Collidables in this CollisionManager. It is the manager's
-     * responsibility to ensure that actually intersecting objects are reported
-     * to the callback, but it is permissible to report non-intersecting objects
-     * as well. The CollisionManager is not required to pay attention to the
-     * collision groups and masks of candidate Collidable pairs.
-     * 
-     * @param callback The callback to invoke on each pair of potentially
-     *            intersecting objects
-     * @throws NullPointerException if callback is null
-     */
-    public void processCollisions(CollisionCallback callback);
-
-    /**
-     * Return the current CollisionAlgorithmProvider used to process potentially
-     * intersecting Collidables. The returned CollisionAlgorithmProvider is provided to
-     * CollisionCallbacks when {@link #processCollisions(CollisionCallback)} is
-     * invoked.
-     * 
-     * @return The CollisionAlgorithmProvider
-     */
-    public CollisionAlgorithmProvider getCollisionHandler();
-
-    /**
-     * Set the CollisionAlgorithmProvider to use for subsequent invocations of
-     * {@link #processCollisions(CollisionCallback)}.
-     * 
-     * @param handler The new CollisionAlgorithmProvider
-     * @throws NullPointerException if handler is null
-     */
-    public void setCollisionHandler(CollisionAlgorithmProvider handler);
-}

ferox-physics/src/main/java/com/ferox/physics/collision/DefaultCollisionAlgorithmProvider.java

  * 
  * @author Michael Ludwig
  */
+// FIXME this does not need to be thread safe anymore, so it can be 
+// greatly simplified. 
 public class DefaultCollisionAlgorithmProvider implements CollisionAlgorithmProvider {
     private final Map<TypePair, CollisionAlgorithm<?, ?>> algorithmCache;
     private final ThreadLocal<TypePair> lookup;

ferox-physics/src/main/java/com/ferox/physics/collision/SpatialHierarchyCollisionManager.java

-package com.ferox.physics.collision;
-
-import java.util.HashMap;
-import java.util.Map;
-import java.util.Map.Entry;
-
-import com.ferox.math.bounds.IntersectionCallback;
-import com.ferox.math.bounds.Octree;
-import com.ferox.math.bounds.Octree.Strategy;
-import com.ferox.math.bounds.SpatialIndex;
-
-/**
- * SpatialHierarchyCollisionManager is a CollisionManager implementation
- * that relies on the {@link SpatialIndex} implementations to determine
- * the set of potentially intersecting objects.
- * @author Michael Ludwig
- *
- */
-public class SpatialHierarchyCollisionManager implements CollisionManager {
-    private final SpatialIndex<Collidable> hierarchy;
-    private final Map<Collidable, Key> hierarchyKeys;
-    
-    private volatile CollisionAlgorithmProvider handler;
-
-    /**
-     * Create a new SpatialHierarchyCollisionManager that uses a static
-     * {@link Octree} with the default initial tree depth. It initially uses a
-     * DefaultCollisionAlgorithmProvider.
-     */
-    public SpatialHierarchyCollisionManager() {
-        this(new Octree<Collidable>(Strategy.STATIC));
-    }
-
-    /**
-     * Create a new SpatialHierarchyCollisionManager that uses the provided
-     * SpatialIndex. It initially uses a DefaultCollisionAlgorithmProvider.
-     * The manager assumes ownership over the hierarchy, and the hierarchy
-     * should not be used by the caller anymore (it should also be empty when
-     * passed to this constructor).
-     * 
-     * @param hierarchy The SpatialIndex instance to use
-     * @throws NullPointerException if hierarchy is null
-     */
-    public SpatialHierarchyCollisionManager(SpatialIndex<Collidable> hierarchy) {
-        this(hierarchy, new DefaultCollisionAlgorithmProvider());
-    }
-
-    /**
-     * Create a new SpatialHierarchyCollisionManager with the provided
-     * SpatialIndex and CollisionAlgorithmProvider. The manager assumes
-     * ownership over the hierarchy, and the hierarchy should not be used by the
-     * caller anymore (it should also be empty when passed to this constructor).
-     * 
-     * @param hierarchy The SpatialIndex instance to use
-     * @param handler The CollisionAlgorithProvider to use
-     * @throws NullPointerException if hierarchy or handler are null
-     */
-    public SpatialHierarchyCollisionManager(SpatialIndex<Collidable> hierarchy, CollisionAlgorithmProvider handler) {
-        if (hierarchy == null)
-            throw new NullPointerException("SpatialIndex cannot be null");
-        this.hierarchy = hierarchy;
-        hierarchyKeys = new HashMap<Collidable, Key>();
-        
-        setCollisionHandler(handler);
-    }
-    
-    @Override
-    public CollisionAlgorithmProvider getCollisionHandler() {
-        return handler;
-    }
-
-    @Override
-    public void setCollisionHandler(CollisionAlgorithmProvider handler) {
-        if (handler == null)
-            throw new NullPointerException("CollisionAlgorithmProvider cannot be null");
-        this.handler = handler;
-    }
-    
-    @Override
-    public void add(Collidable collidable) {
-        synchronized(hierarchy) {
-            if (hierarchyKeys.containsKey(collidable))
-                return; // don't re-add
-            
-            // don't add to the hierarchy yet, that's done during getClosestPairs()
-            hierarchyKeys.put(collidable, new Key());
-        }
-    }
-
-    @Override
-    public void remove(Collidable collidable) {
-        synchronized(hierarchy) {
-            Key key = hierarchyKeys.remove(collidable);
-            if (key != null && key.key != null)
-                hierarchy.remove(collidable, key.key);
-        }
-    }
-    
-    @Override
-    public void processCollisions(CollisionCallback callback) {
-        synchronized(hierarchy) {
-            if (callback == null)
-                throw new NullPointerException("Callback cannot be null");
-            
-            // update every known collidable within the hierarchy
-            Key key;
-            Collidable shape;
-            for (Entry<Collidable, Key> c: hierarchyKeys.entrySet()) {
-                shape = c.getKey();
-                key = c.getValue();
-                
-                // compute world bounds
-                if (key.key == null)
-                    key.key = hierarchy.add(shape, shape.getWorldBounds());
-                else
-                    hierarchy.update(shape, shape.getWorldBounds(), key.key);
-            }
-            
-            hierarchy.query(new CollisionIntersectionCallback(callback));
-        }
-    }
-    
-    private static class Key {
-        // we only have this wrapper so we can track when something
-        // is in the hierarchy or not
-        Object key = null;
-    }
-    
-    private class CollisionIntersectionCallback implements IntersectionCallback<Collidable> {
-        private final CollisionCallback delegate;
-        
-        public CollisionIntersectionCallback(CollisionCallback callback) {
-            delegate = callback;
-        }
-        
-        @Override
-        public void process(Collidable item1, Collidable item2) {
-            delegate.process(item1, item2, handler);
-        }
-    }
-}

ferox-physics/src/main/java/com/ferox/physics/collision/algorithm/MinkowskiDifference.java

 import com.ferox.math.Vector3;
 import com.ferox.math.Vector4;
 import com.ferox.physics.collision.ClosestPair;
-import com.ferox.physics.collision.Collidable;
+import com.ferox.physics.collision.CollisionBody;
 import com.ferox.physics.collision.shape.ConvexShape;
 
 /**
  * in the difference between two shapes, the second shape in the sum is just the
  * negation of one convex shape. This implementation also automatically handles
  * converting the local support functions of {@link ConvexShape ConvexShapes}
- * into world space by applying the world transform of {@link Collidable
+ * into world space by applying the world transform of {@link CollisionBody
  * Collidables} involved.
  * 
  * @author Michael Ludwig

ferox-physics/src/main/java/com/ferox/physics/controller/DiscreteRigidBodyController.java

+package com.ferox.physics.controller;
+
+import java.awt.geom.AffineTransform;
+import java.util.HashSet;
+import java.util.Set;
+
+import com.ferox.math.Const;
+import com.ferox.math.Vector3;
+import com.ferox.physics.collision.ClosestPair;
+import com.ferox.physics.collision.CollisionBody;
+import com.ferox.physics.collision.CollisionAlgorithm;
+import com.ferox.physics.collision.CollisionAlgorithmProvider;
+import com.ferox.physics.collision.CollisionCallback;
+import com.ferox.physics.collision.CollisionManager;
+import com.ferox.physics.dynamics.Integrator;
+import com.ferox.physics.dynamics.RigidBody;
+import com.ferox.physics.dynamics.constraint.Constraint;
+import com.ferox.physics.dynamics.constraint.ConstraintSolver;
+import com.ferox.physics.dynamics.constraint.ContactManifoldCache;
+import com.ferox.util.Bag;
+import com.ferox.util.ChainedCollection;
+
+public class DiscreteRigidBodyController {
+    private final Set<CollisionBody> bodies;
+    private final Bag<RigidBody> rigidBodies;
+
+    private final Vector3 gravity;
+
+    private final Set<Constraint> constraints;
+    
+    private CollisionManager collisionManager;
+    private ConstraintSolver constraintSolver;
+    private Integrator integrator;
+
+    private final ContactManifoldCache contactCache;
+    
+    public DiscreteRigidBodyController(PhysicsWorldConfiguration config) {
+        if (config == null)
+            throw new NullPointerException("Configuration cannot be null");
+        
+        gravity = new Vector3(0, -10, 0);
+        
+        bodies = new HashSet<CollisionBody>();
+        rigidBodies = new Bag<RigidBody>();
+        constraints = new HashSet<Constraint>();
+        
+        contactCache = new ContactManifoldCache();
+        
+        integrator = config.getIntegrator();
+        constraintSolver = config.getConstraintSolver();
+        collisionManager = config.getCollisionManager();
+    }
+
+    public @Const Vector3 getGravity() {
+        return gravity;
+    }
+
+    public void setGravity(@Const Vector3 gravity) {
+        if (gravity == null)
+            throw new NullPointerException("Gravity cannot be null");
+        this.gravity.set(gravity);
+    }
+
+    @Override
+    public void step(float dt) {
+        // FIXME use time step interpolation to maintain a fixed time step rate like in bullet
+        RigidBody b;
+        Vector3f gravForce = new Vector3f();
+        int ct = rigidBodies.size();
+        for (int i = 0; i < ct; i++) {
+            b = rigidBodies.get(i);
+            
+            // add gravity force to each body
+            if (b.getExplicitGravity() != null)
+                b.getExplicitGravity().scale(b.getMass(), gravForce);
+            else
+                gravity.scale(b.getMass(), gravForce);
+
+            b.addForce(gravForce, null); // gravity is a central force, applies no torque
+            b.applyForces(integrator, dt);
+        }
+        
+        collisionManager.processCollisions(new ContactManifoldCallback());
+        contactCache.update();
+        constraintSolver.solve(new ChainedCollection<Constraint>(constraints, contactCache.getContacts()), dt);
+        
+        // recompute next frame position after constraint solving
+        // and store it in the world transform
+        AffineTransform t = new AffineTransform();
+        for (int i = 0; i < ct; i++) {
+            b = rigidBodies.get(i);
+            b.predictMotion(integrator, dt, t);
+            b.setTransform(t);
+        }
+    }
+    
+    @Override
+    public void add(CollisionBody c) {
+        if (c == null)
+            throw new NullPointerException("Body cannot be null");
+        
+        if (!bodies.contains(c)) {
+            bodies.add(c);
+            collisionManager.add(c);
+            if (c instanceof RigidBody)
+                rigidBodies.add((RigidBody) c);
+        }
+    }
+
+    @Override
+    public void remove(CollisionBody c) {
+        if (c == null)
+            throw new NullPointerException("Body cannot be null");
+        
+        if (bodies.remove(c)) {
+            collisionManager.remove(c);
+            contactCache.remove(c);
+            if (c instanceof RigidBody)
+                rigidBodies.remove(c);
+        }
+    }
+
+    @Override
+    public void add(Constraint c) {
+        if (c == null)
+            throw new NullPointerException("Constraint cannot be null");
+        constraints.add(c);
+    }
+
+    @Override
+    public void remove(Constraint c) {
+        if (c == null)
+            throw new NullPointerException("Constraint cannot be null");
+        constraints.remove(c);
+    }
+
+    @Override
+    public Integrator getIntegrator() {
+        return integrator;
+    }
+
+    @Override
+    public void setIntegrator(Integrator integrator) {
+        if (integrator == null)
+            throw new NullPointerException("Integrator cannot be null");
+        this.integrator = integrator;
+    }
+
+    @Override
+    public ConstraintSolver getConstraintSolver() {
+        return constraintSolver;
+    }
+
+    @Override
+    public void setConstraintSolver(ConstraintSolver solver) {
+        if (solver == null)
+            throw new NullPointerException("ConstraintSolver cannot be null");
+        constraintSolver = solver;
+    }
+
+    @Override
+    public CollisionManager getCollisionManager() {
+        return collisionManager;
+    }
+
+    @Override
+    public void setCollisionManager(CollisionManager manager) {
+        if (manager == null)
+            throw new NullPointerException("CollisionManager cannot be null");
+        
+        // remove bodies from old manager and add them to the new one
+        for (CollisionBody c: bodies) {
+            collisionManager.remove(c);
+            manager.add(c);
+        }
+        
+        // update reference
+        collisionManager = manager;
+    }
+    
+    private class ContactManifoldCallback implements CollisionCallback {
+        @Override
+        @SuppressWarnings({ "rawtypes", "unchecked" })
+        public void process(CollisionBody objA, CollisionBody objB, CollisionAlgorithmProvider handler) {
+            if (!(objA instanceof RigidBody) && !(objB instanceof RigidBody))
+                return; // ignore collisions between only static objects
+            
+            CollisionAlgorithm algo = handler.getAlgorithm(objA.getShape().getClass(), objB.getShape().getClass());
+            if (algo != null) {
+                ClosestPair pair = algo.getClosestPair(objA.getShape(), objA.getTransform(), objB.getShape(), objB.getTransform());
+                if (pair != null && pair.isIntersecting())
+                    contactCache.addContact(objA, objB, pair);
+            }
+            
+        }
+    }
+}

ferox-physics/src/main/java/com/ferox/physics/controller/SingleAxisSAPCollisionController.java

+package com.ferox.physics.controller;
+
+import com.ferox.physics.collision.CollisionController;
+
+public class SingleAxisSAPCollisionController extends CollisionController {
+
+}

ferox-physics/src/main/java/com/ferox/physics/controller/SpatialIndexCollisionController.java

+package com.ferox.physics.controller;
+
+import com.ferox.physics.collision.CollisionController;
+
+public class SpatialIndexCollisionController extends CollisionController {
+
+}

ferox-physics/src/main/java/com/ferox/physics/controller/TemporalSAPCollisionController.java

+package com.ferox.physics.controller;
+
+import com.ferox.physics.collision.CollisionController;
+
+public class TemporalSAPCollisionController extends CollisionController {
+
+}

ferox-physics/src/main/java/com/ferox/physics/dynamics/DiscretePhysicsWorld.java

-package com.ferox.physics.dynamics;
-
-import java.util.HashSet;
-import java.util.Set;
-
-import com.ferox.math.ReadOnlyVector3f;
-import com.ferox.math.AffineTransform;
-import com.ferox.math.Vector3f;
-import com.ferox.physics.collision.Collidable;
-import com.ferox.physics.collision.CollisionAlgorithm;
-import com.ferox.physics.collision.CollisionCallback;
-import com.ferox.physics.collision.CollisionAlgorithmProvider;
-import com.ferox.physics.collision.CollisionManager;
-import com.ferox.physics.collision.algorithm.ClosestPair;
-import com.ferox.physics.dynamics.constraint.Constraint;
-import com.ferox.physics.dynamics.constraint.ConstraintSolver;
-import com.ferox.physics.dynamics.constraint.ContactManifoldCache;
-import com.ferox.util.Bag;
-import com.ferox.util.ChainedCollection;
-
-public class DiscretePhysicsWorld implements PhysicsWorld {
-    // FIXME: thread-safety
-    private final Set<Collidable> bodies;
-    private final Bag<RigidBody> rigidBodies;
-
-    private final Vector3f gravity;
-
-    private final Set<Constraint> constraints;
-    
-    private CollisionManager collisionManager;
-    private ConstraintSolver constraintSolver;
-    private Integrator integrator;
-
-    private final ContactManifoldCache contactCache;
-    
-    public DiscretePhysicsWorld(PhysicsWorldConfiguration config) {
-        if (config == null)
-            throw new NullPointerException("Configuration cannot be null");
-        
-        gravity = new Vector3f(0f, -10f, 0f);
-        
-        bodies = new HashSet<Collidable>();
-        rigidBodies = new Bag<RigidBody>();
-        constraints = new HashSet<Constraint>();
-        
-        contactCache = new ContactManifoldCache();
-        
-        integrator = config.getIntegrator();
-        constraintSolver = config.getConstraintSolver();
-        collisionManager = config.getCollisionManager();
-    }
-
-    @Override
-    public ReadOnlyVector3f getGravity() {
-        return gravity;
-    }
-
-    @Override
-    public void setGravity(ReadOnlyVector3f gravity) {
-        if (gravity == null)
-            throw new NullPointerException("Gravity cannot be null");
-        this.gravity.set(gravity);
-    }
-
-    @Override
-    public void step(float dt) {
-        // FIXME use time step interpolation to maintain a fixed time step rate like in bullet
-        RigidBody b;
-        Vector3f gravForce = new Vector3f();
-        int ct = rigidBodies.size();
-        for (int i = 0; i < ct; i++) {
-            b = rigidBodies.get(i);
-            
-            // add gravity force to each body
-            if (b.getExplicitGravity() != null)
-                b.getExplicitGravity().scale(b.getMass(), gravForce);
-            else
-                gravity.scale(b.getMass(), gravForce);
-
-            b.addForce(gravForce, null); // gravity is a central force, applies no torque
-            b.applyForces(integrator, dt);
-        }
-        
-        collisionManager.processCollisions(new ContactManifoldCallback());
-        contactCache.update();
-        constraintSolver.solve(new ChainedCollection<Constraint>(constraints, contactCache.getContacts()), dt);
-        
-        // recompute next frame position after constraint solving
-        // and store it in the world transform
-        AffineTransform t = new AffineTransform();
-        for (int i = 0; i < ct; i++) {
-            b = rigidBodies.get(i);
-            b.predictMotion(integrator, dt, t);
-            b.setTransform(t);
-        }
-    }
-    
-    @Override
-    public void add(Collidable c) {
-        if (c == null)
-            throw new NullPointerException("Body cannot be null");
-        
-        if (!bodies.contains(c)) {
-            bodies.add(c);
-            collisionManager.add(c);
-            if (c instanceof RigidBody)
-                rigidBodies.add((RigidBody) c);
-        }
-    }
-
-    @Override
-    public void remove(Collidable c) {
-        if (c == null)
-            throw new NullPointerException("Body cannot be null");
-        
-        if (bodies.remove(c)) {
-            collisionManager.remove(c);
-            contactCache.remove(c);
-            if (c instanceof RigidBody)
-                rigidBodies.remove(c);
-        }
-    }
-
-    @Override
-    public void add(Constraint c) {
-        if (c == null)
-            throw new NullPointerException("Constraint cannot be null");
-        constraints.add(c);
-    }
-
-    @Override
-    public void remove(Constraint c) {
-        if (c == null)
-            throw new NullPointerException("Constraint cannot be null");
-        constraints.remove(c);
-    }
-
-    @Override
-    public Integrator getIntegrator() {
-        return integrator;
-    }
-
-    @Override
-    public void setIntegrator(Integrator integrator) {
-        if (integrator == null)
-            throw new NullPointerException("Integrator cannot be null");
-        this.integrator = integrator;
-    }
-
-    @Override
-    public ConstraintSolver getConstraintSolver() {
-        return constraintSolver;
-    }
-
-    @Override
-    public void setConstraintSolver(ConstraintSolver solver) {
-        if (solver == null)
-            throw new NullPointerException("ConstraintSolver cannot be null");
-        constraintSolver = solver;
-    }
-
-    @Override
-    public CollisionManager getCollisionManager() {
-        return collisionManager;
-    }
-
-    @Override
-    public void setCollisionManager(CollisionManager manager) {
-        if (manager == null)
-            throw new NullPointerException("CollisionManager cannot be null");
-        
-        // remove bodies from old manager and add them to the new one
-        for (Collidable c: bodies) {
-            collisionManager.remove(c);
-            manager.add(c);
-        }
-        
-        // update reference
-        collisionManager = manager;
-    }
-    
-    private class ContactManifoldCallback implements CollisionCallback {
-        @Override
-        @SuppressWarnings({ "rawtypes", "unchecked" })
-        public void process(Collidable objA, Collidable objB, CollisionAlgorithmProvider handler) {
-            if (!(objA instanceof RigidBody) && !(objB instanceof RigidBody))
-                return; // ignore collisions between only static objects
-            
-            CollisionAlgorithm algo = handler.getAlgorithm(objA.getShape().getClass(), objB.getShape().getClass());
-            if (algo != null) {
-                ClosestPair pair = algo.getClosestPair(objA.getShape(), objA.getTransform(), objB.getShape(), objB.getTransform());
-                if (pair != null && pair.isIntersecting())
-                    contactCache.addContact(objA, objB, pair);
-            }
-            
-        }
-    }
-}

ferox-physics/src/main/java/com/ferox/physics/dynamics/ExplicitEulerIntegrator.java

 package com.ferox.physics.dynamics;
 
-import com.ferox.math.MutableMatrix3f;
-import com.ferox.math.MutableQuat4f;
-import com.ferox.math.MutableVector3f;
-import com.ferox.math.Quat4f;
-import com.ferox.math.ReadOnlyVector3f;
-import com.ferox.math.Vector3f;
+import com.ferox.math.Const;
+import com.ferox.math.Matrix3;
+import com.ferox.math.Quat4;
+import com.ferox.math.Vector3;
 
 public class ExplicitEulerIntegrator implements Integrator {
-    private static final float MAX_ANGULAR_VELOCITY = (float) Math.PI  / 2f;
-    private static final float ANGULAR_MOTION_THRESHOLD = (float) Math.PI / 4f;
-    private static final float ANGULAR_VELOCITY_DAMPING = .5f;
+    private static final double MAX_ANGULAR_VELOCITY = Math.PI  / 2.0;
+    private static final double ANGULAR_MOTION_THRESHOLD = Math.PI / 4.0;
+    private static final double ANGULAR_VELOCITY_DAMPING = .5;
 
-    private final Vector3f tempv = new Vector3f();
-    private final Quat4f tempq1 = new Quat4f();
-    private final Quat4f tempq2 = new Quat4f();
+    private final Vector3 tempv = new Vector3();
+    private final Quat4 tempq1 = new Quat4();
+    private final Quat4 tempq2 = new Quat4();
     
     @Override
-    public void integrateLinearAcceleration(ReadOnlyVector3f a, float dt, MutableVector3f velocity) {
+    public void integrateLinearAcceleration(@Const Vector3 a, double dt, Vector3 velocity) {
         integrateVector(a, dt, velocity);
     }
 
     @Override
-    public void integrateAngularAcceleration(ReadOnlyVector3f a, float dt,
-                                             MutableVector3f angularVelocity) {
+    public void integrateAngularAcceleration(@Const Vector3 a, double dt, Vector3 angularVelocity) {
         integrateVector(a, dt, angularVelocity);
         applyDamping(angularVelocity, dt, ANGULAR_VELOCITY_DAMPING);
     }
     
-    private void applyDamping(MutableVector3f v, float dt, float damping) {
-        v.scale((float) Math.pow(1f - damping, dt));
+    private void applyDamping(Vector3 v, double dt, double damping) {
+        v.scale(Math.pow(1.0 - damping, dt));
     }
 
     @Override
-    public void integrateLinearVelocity(ReadOnlyVector3f v, float dt, MutableVector3f position) {
+    public void integrateLinearVelocity(@Const Vector3 v, double dt, Vector3 position) {
         integrateVector(v, dt, position);
     }
 
     @Override
-    public void integrateAngularVelocity(ReadOnlyVector3f v, float dt, MutableMatrix3f orientation) {
+    public void integrateAngularVelocity(@Const Vector3 v, double dt, Matrix3 orientation) {
         // clamp angular velocity
-        Vector3f axis = tempv;
+        Vector3 axis = tempv;
 
-        float veclength = v.length();
-        float angvel = veclength;
+        double veclength = v.length();
+        double angvel = veclength;
         if (angvel * dt > MAX_ANGULAR_VELOCITY) {
             // set axis to be linear velocity but with magnitude = MAX / dt
+            // FIXME axis is set to garbage at this point, which is probably wrong
             axis.scale(MAX_ANGULAR_VELOCITY / (angvel * dt));
             angvel = MAX_ANGULAR_VELOCITY / dt;
-        } else
+        } else {
             axis.set(v); // don't need to clamp so just set axis to angular velocity
+        }
         
         // angular velocity uses the exponential map method
         // "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
 
         // limit the angular motion - but update the velocity vector
-        float fAngle = angvel;
+        double fAngle = angvel;
         if (angvel * dt > ANGULAR_MOTION_THRESHOLD)
             fAngle = ANGULAR_MOTION_THRESHOLD / dt;
         
-        if (fAngle < .001f) {
+        if (fAngle < .001) {
             // use Taylor's expansions of sync function
-            axis.scale((.5f * dt) - (dt * dt * dt) * (.02083333333333f * fAngle * fAngle));
+            axis.scale((.5 * dt) - (dt * dt * dt) * (.02083333333333 * fAngle * fAngle));
         } else {
             // sync(fAngle) = sin(c * fAngle) / t
-            axis.scale((float) Math.sin(.5f * fAngle * dt) / fAngle);
+            axis.scale(Math.sin(.5 * fAngle * dt) / fAngle);
         }
         
-        MutableQuat4f newRot = tempq1.set(axis.getX(), axis.getY(), axis.getZ(), (float) Math.cos(.5f * fAngle * dt));
-        MutableQuat4f oldRot = tempq2.set(orientation);
+        Quat4 newRot = tempq1.set(axis.x, axis.y, axis.z, Math.cos(.5 * fAngle * dt));
+        Quat4 oldRot = tempq2.set(orientation);
         newRot.mul(oldRot).normalize();
         
         orientation.set(newRot);
     }
     
-    private void integrateVector(ReadOnlyVector3f deriv, float dt, MutableVector3f func) {
+    private void integrateVector(@Const Vector3 deriv, double dt, Vector3 func) {
         if (deriv == null || func == null)
             throw new NullPointerException("Arguments cannot be null");
-        deriv.scaleAdd(dt, func, func);
+        func.add(tempv.scale(deriv, dt));
     }
 }

ferox-physics/src/main/java/com/ferox/physics/dynamics/Integrator.java

 package com.ferox.physics.dynamics;
 
-import com.ferox.math.MutableMatrix3f;
-import com.ferox.math.MutableVector3f;
-import com.ferox.math.ReadOnlyVector3f;
+import com.ferox.math.Const;
+import com.ferox.math.Matrix3;
+import com.ferox.math.Vector3;
 
 public interface Integrator {
-    public void integrateLinearAcceleration(ReadOnlyVector3f a, float dt, MutableVector3f velocity);
+    public void integrateLinearAcceleration(@Const Vector3 a, double dt, Vector3 velocity);
     
-    public void integrateAngularAcceleration(ReadOnlyVector3f a, float dt, MutableVector3f angularVelocity);
+    public void integrateAngularAcceleration(@Const Vector3 a, double dt, Vector3 angularVelocity);
     
-    public void integrateLinearVelocity(ReadOnlyVector3f v, float dt, MutableVector3f position);
+    public void integrateLinearVelocity(@Const Vector3 v, double dt, Vector3 position);
     
-    public void integrateAngularVelocity(ReadOnlyVector3f v, float dt, MutableMatrix3f orientation);
+    public void integrateAngularVelocity(@Const Vector3 v, double dt, Matrix3 orientation);
 }

ferox-physics/src/main/java/com/ferox/physics/dynamics/PhysicsWorld.java

-package com.ferox.physics.dynamics;
-
-import com.ferox.math.ReadOnlyVector3f;
-import com.ferox.physics.collision.Collidable;
-import com.ferox.physics.collision.CollisionManager;
-import com.ferox.physics.dynamics.constraint.Constraint;
-import com.ferox.physics.dynamics.constraint.ConstraintSolver;
-
-// FIXME: remove the physics world configuration and make the components of the physics world
-// final, and to be configured via guice injection
-// must also figure out thread-safety of components -> currently its mixed multithreaded and single-threaded
-public interface PhysicsWorld {
-    public void add(Collidable c);
-    
-    public void remove(Collidable c);
-    
-    public void step(float dt);
-    
-    public void add(Constraint c);
-    
-    public void remove(Constraint c);
-    
-    public Integrator getIntegrator();
-    
-    public void setIntegrator(Integrator integrator);
-    
-    public ConstraintSolver getConstraintSolver();
-    
-    public void setConstraintSolver(ConstraintSolver solver);
-    
-    public CollisionManager getCollisionManager();
-    
-    public void setCollisionManager(CollisionManager manager);
-    
-    public ReadOnlyVector3f getGravity();
-    
-    public void setGravity(ReadOnlyVector3f g);
-}

ferox-physics/src/main/java/com/ferox/physics/dynamics/PhysicsWorldConfiguration.java

-package com.ferox.physics.dynamics;
-
-import com.ferox.physics.collision.CollisionManager;
-import com.ferox.physics.collision.SpatialHierarchyCollisionManager;
-import com.ferox.physics.dynamics.constraint.ConstraintSolver;
-import com.ferox.physics.dynamics.constraint.SequentialImpulseConstraintSolver;
-
-public class PhysicsWorldConfiguration {
-    private Integrator integrator;
-    private ConstraintSolver solver;
-    private CollisionManager manager;
-    
-    public PhysicsWorldConfiguration() {
-        integrator = new ExplicitEulerIntegrator();
-        solver = new SequentialImpulseConstraintSolver();
-        manager = new SpatialHierarchyCollisionManager();
-    }
-    
-    public Integrator getIntegrator() {
-        return integrator;
-    }
-    
-    public PhysicsWorldConfiguration setIntegrator(Integrator integrator) {
-        if (integrator == null)
-            throw new NullPointerException("Integrator cannot be null");
-        this.integrator = integrator;
-        return this;
-    }
-    
-    public ConstraintSolver getConstraintSolver() {
-        return solver;
-    }
-    
-    public PhysicsWorldConfiguration setConstraintSolver(ConstraintSolver solver) {
-        if (solver == null)
-            throw new NullPointerException("ConstraintSolver cannot be null");
-        this.solver = solver;
-        return this;
-    }
-    
-    public CollisionManager getCollisionManager() {
-        return manager;
-    }
-    
-    public PhysicsWorldConfiguration setCollisionManager(CollisionManager manager) {
-        if (manager == null)
-            throw new NullPointerException("CollisionManager cannot be null");
-        this.manager = manager;
-        return this;
-    }
-}

ferox-physics/src/main/java/com/ferox/physics/dynamics/RigidBody.java

 package com.ferox.physics.dynamics;
 
-import com.ferox.math.Matrix3f;
-import com.ferox.math.MutableVector3f;
-import com.ferox.math.ReadOnlyMatrix3f;
-import com.ferox.math.ReadOnlyMatrix4f;
-import com.ferox.math.ReadOnlyVector3f;
-import com.ferox.math.AffineTransform;
-import com.ferox.math.Vector3f;
-import com.ferox.physics.collision.Collidable;
-import com.ferox.physics.collision.Shape;
+import com.ferox.math.Const;
+import com.ferox.math.Matrix3;
+import com.ferox.math.Vector3;
+import com.ferox.math.entreri.Matrix3Property;
+import com.ferox.math.entreri.Vector3Property;
+import com.lhkbob.entreri.ComponentData;
+import com.lhkbob.entreri.Unmanaged;
+import com.lhkbob.entreri.property.DoubleProperty;
 
-public class RigidBody extends Collidable {
-    private float inverseMass; // 1 / mass
-    private final Matrix3f inertiaTensorWorldInverse;
+public class RigidBody extends ComponentData<RigidBody> {
+    private DoubleProperty inverseMass;
+    
+    private Matrix3Property inertiaTensorWorldInverse;
 
-    private ReadOnlyVector3f explicitGravity; // null if gravity is taken from world
-
-    private final Vector3f velocity;
-    private final Vector3f angularVelocity;
-    private final Vector3f totalForce;
-    private final Vector3f totalTorque;
+    private Vector3Property velocity;
+    private Vector3Property angularVelocity;
     
-    public RigidBody(ReadOnlyMatrix4f t, Shape shape, float mass) {
-        super(t, shape);
-
-        velocity = new Vector3f();
-        angularVelocity = new Vector3f();
-        inertiaTensorWorldInverse = new Matrix3f();
-
-        totalForce = new Vector3f();
-        totalTorque = new Vector3f();
-        
-        setMass(mass);
+    private Vector3Property totalForce;
+    private Vector3Property totalTorque;
+    
+    // This got pretty ugly pretty fast
+    @Unmanaged private final Vector3 velocityCache = new Vector3();
+    @Unmanaged private final Vector3 angularVelocityCache = new Vector3();
+    @Unmanaged private final Vector3 forceCache = new Vector3();
+    @Unmanaged private final Vector3 torqueCache = new Vector3();
+    @Unmanaged private final Matrix3 tensorCache = new Matrix3();
+    
+    @Unmanaged private final Vector3 temp = new Vector3();
+    
+    private RigidBody() { }
+    
+    public @Const Matrix3 getInertiaTensorInverse() {
+        return tensorCache;
     }
     
-    /*public void updateFromSolverBody() {
-        velocity.set(velocity.getX() + solverBody.dlX,
-                     velocity.getY() + solverBody.dlY,
-                     velocity.getZ() + solverBody.dlZ);
-        angularVelocity.set(angularVelocity.getX() + solverBody.daX,
-                            angularVelocity.getY() + solverBody.daY,
-                            angularVelocity.getZ() + solverBody.daZ);
-        
-        solverBody.dlX = 0f; solverBody.dlY = 0f; solverBody.dlZ = 0f;
-        solverBody.daX = 0f; solverBody.daY = 0f; solverBody.daZ = 0f;
-    }*/
-    
-    @Override
-    public void setTransform(ReadOnlyMatrix4f t) {
-        super.setTransform(t);
-        updateInertiaTensor();
-    }
-    
+    /* FIXME move these functions into the internals of the physics controller
     private void updateInertiaTensor() {
         // FIXME: what about kinematic objects? they don't get inertia really
         // FIXME: the inertia tensor needs to be inverted (e.g. 1/x, 1/y, 1/z) here
         rotation.mulDiagonal(inertia, inertiaTensorWorldInverse).mulTransposeRight(rotation);
     }
     
-    public ReadOnlyMatrix3f getInertiaTensorInverse() {
-        return inertiaTensorWorldInverse;
-    }
-    
     public void applyForces(Integrator integrator, float dt) {
         if (dt <= 0f)
             throw new IllegalArgumentException("Time delta must be positive, not: " + dt);
         integrator.integrateLinearVelocity(velocity, dt, result.getTranslation());
         integrator.integrateAngularVelocity(angularVelocity, dt, result.getRotation());
     }
+    */
     
-    public void addForce(ReadOnlyVector3f force, ReadOnlyVector3f relPos) {
-        if (force == null)
-            throw new NullPointerException("Force vector cannot be null");
-        totalForce.add(force);
-        
-        if (relPos != null)
-            totalTorque.add(relPos.cross(force, temp3.get()));
-    }
-    
-    public void addImpulse(ReadOnlyVector3f impulse, ReadOnlyVector3f relPos) {
-        if (impulse == null)
-            throw new NullPointerException("Force vector cannot be null");
-        
-        impulse.scaleAdd(inverseMass, velocity, velocity);
+    public void addForce(@Const Vector3 force, @Const Vector3 relPos) {
+        forceCache.add(force);
+        totalForce.set(forceCache, getIndex());
         
         if (relPos != null) {
-            MutableVector3f torque = relPos.cross(impulse, temp3.get());
-            angularVelocity.add(inertiaTensorWorldInverse.mul(torque));
+            torqueCache.add(temp.cross(relPos, force));
+            totalTorque.set(torqueCache, getIndex());
         }
     }
     
-    public void setVelocity(ReadOnlyVector3f vel) {
-        if (vel == null)
-            throw new NullPointerException("Velocity cannot be null");
-        velocity.set(vel);
+    public void addImpulse(@Const Vector3 impulse, @Const Vector3 relPos) {
+        velocityCache.add(temp.scale(impulse, getInverseMass()));
+        velocity.set(velocityCache, getIndex());
+        
+        if (relPos != null) {
+            temp.cross(relPos, impulse);
+            temp.mul(tensorCache, temp);
+            angularVelocityCache.add(temp);
+            
+            angularVelocity.set(angularVelocityCache, getIndex());
+        }
     }
     
-    public void setAngularVelocity(ReadOnlyVector3f angVel) {
-        if (angVel == null)
-            throw new NullPointerException("Angular velocity cannot be null");
-        angularVelocity.set(angVel);
+    public void setVelocity(@Const Vector3 vel) {
+        velocityCache.set(vel);
+        velocity.set(vel, getIndex());
+    }
+    
+    public void setAngularVelocity(@Const Vector3 angVel) {
+        angularVelocityCache.set(angVel);
+        angularVelocity.set(angVel, getIndex());
     }
     
     public void clearForces() {
-        totalForce.set(0f, 0f, 0f);
-        totalTorque.set(0f, 0f, 0f);
+        forceCache.set(0.0, 0.0, 0.0);
+        torqueCache.set(0.0, 0.0, 0.0);
+        
+        totalForce.set(forceCache, getIndex());
+        totalTorque.set(torqueCache, getIndex());
     }
 
-    public ReadOnlyVector3f getExplicitGravity() {
-        return explicitGravity;
+    public void setMass(double mass) {
+        if (mass <= 0.0)
+            throw new IllegalArgumentException("Mass must be positive");
+        inverseMass.set(1.0 / mass, getIndex());
     }
 
-    public void setExplicitGravity(ReadOnlyVector3f gravity) {
-        explicitGravity = gravity;
+    public double getMass() {
+        return 1.0 / inverseMass.get(getIndex());
     }
 
-    public void setMass(float mass) {
-        if (mass <= 0f)
-            throw new IllegalArgumentException("Mass must be positive");
-        inverseMass = 1f / mass;
-        updateInertiaTensor();
+    public double getInverseMass() {
+        return inverseMass.get(getIndex());
     }
 
-    public float getMass() {
-        return 1f / inverseMass;
+    public @Const Vector3 getVelocity() {
+        return velocityCache;
     }
 
-    public float getInverseMass() {
-        return inverseMass;
-    }
-
-    public ReadOnlyVector3f getVelocity() {
-        return velocity;
-    }
-
-    public ReadOnlyVector3f getAngularVelocity() {
-        return angularVelocity;
+    public @Const Vector3 getAngularVelocity() {
+        return angularVelocityCache;
     }
     
-    /* 
-     * ThreadLocals for computations. 
-     */
-    
-    private static final ThreadLocal<Vector3f> temp3 = new ThreadLocal<Vector3f>() {
-        @Override