[FEAT] build with maven project

.checkstyle

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<fileset-config file-format-version="1.2.0" simple-config="true" sync-formatter="false">
+  <fileset name="all" enabled="true" check-config-name="Ewol" local="false">
+    <file-match-pattern match-pattern="." include-pattern="true"/>
+  </fileset>
+</fileset-config>

.classpath

@@ -1,29 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<classpath>
-	<classpathentry excluding="**/*.java__|**/__*.java" kind="src" path="src">
-		<attributes>
-			<attribute name="optional" value="true"/>
-		</attributes>
-	</classpathentry>
-	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-14">
-		<attributes>
-			<attribute name="module" value="true"/>
-		</attributes>
-	</classpathentry>
-	<classpathentry kind="con" path="org.eclipse.jdt.junit.JUNIT_CONTAINER/5">
-		<attributes>
-			<attribute name="test" value="true"/>
-		</attributes>
-	</classpathentry>
-	<classpathentry combineaccessrules="false" kind="src" path="/atriasoft-etk">
-		<attributes>
-			<attribute name="module" value="true"/>
-		</attributes>
-	</classpathentry>
-	<classpathentry combineaccessrules="false" kind="src" path="/scenarium-logger">
-		<attributes>
-			<attribute name="module" value="true"/>
-		</attributes>
-	</classpathentry>
-	<classpathentry kind="output" path="out/eclipse/classes"/>
-</classpath>

.gitignore

@@ -1,3 +1,5 @@
+
+/__pycache__/
 /examples/target/
 /examples/framework.log.*
 /target/

.project

@@ -1,18 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<projectDescription>
-	<name>atriasoft-ephysics</name>
-	<comment></comment>
-	<projects>
-		<project>atriasoft-ephysics</project>
-	</projects>
-	<buildSpec>
-		<buildCommand>
-			<name>org.eclipse.jdt.core.javabuilder</name>
-			<arguments>
-			</arguments>
-		</buildCommand>
-	</buildSpec>
-	<natures>
-		<nature>org.eclipse.jdt.core.javanature</nature>
-	</natures>
-</projectDescription>

examples/src/net/jreactphysics3d/examples/collisionshapes/AbstractGameObjectShape.java

@@ -0,0 +1,81 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisionshapes;
+
+import net.frameworkgl.gameobjects.GameObject;
+import net.frameworkgl.math.Vector3f;
+import org.atriasoft.ephysics.body.RigidBody;
+import org.atriasoft.ephysics.collision.shapes.CollisionShape;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+import org.atriasoft.ephysics.mathematics.Matrix3x3;
+import org.atriasoft.etk.math.Quaternion;
+import org.atriasoft.ephysics.mathematics.Transform;
+import org.atriasoft.ephysics.mathematics.Vector3;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public abstract class AbstractGameObjectShape extends GameObject {
+
+    private CollisionShape collisionShape;
+    private RigidBody rigidBody;
+
+    protected void createRigidBody(CollisionShape collisionShape, Vector3f position, float mass, DynamicsWorld dynamicsWorld) {
+
+        this.collisionShape = collisionShape;
+
+        Matrix3x3 inertiaTensor = collisionShape.computeLocalInertiaTensor(mass);
+
+        Quaternion initOrientation = new Quaternion().identity();
+        Vector3 initPosition = new Vector3(position.getX(), position.getY(), position.getZ());
+        Transform transform = new Transform(initPosition, initOrientation);
+
+        rigidBody = dynamicsWorld.createRigidBody(transform, mass, inertiaTensor, collisionShape);
+    }
+
+    public CollisionShape getCollisionShape() {
+        return collisionShape;
+    }
+
+    public RigidBody getRigidBody() {
+        return rigidBody;
+    }
+
+    public void updateTransform() {
+
+        // Get the interpolated transform of the rigid body
+        Transform transform = rigidBody.getInterpolatedTransform(new Transform());
+
+        // Compute the transform used for rendering the box
+        float[] glMatrix = new float[16];
+        transform.getOpenGLMatrix(glMatrix);
+
+        // Apply the scaling matrix to have the correct box dimensions
+        getWorldTransform().fromOpenGLArray(glMatrix);
+        getWorldTransform().multiply(getScalingTransform());
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisionshapes/Box.java

@@ -0,0 +1,73 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisionshapes;
+
+import java.io.IOException;
+import net.frameworkgl.Fw;
+import net.frameworkgl.math.Transform4f;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.mesh.Mesh;
+import net.frameworkgl.opengl.renderable.AbstractRenderable;
+import org.atriasoft.ephysics.collision.shapes.BoxShape;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+import org.atriasoft.ephysics.mathematics.Vector3;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Box extends AbstractGameObjectShape {
+
+    public Box(Vector3f position, Vector3f size, float mass, DynamicsWorld dynamicsWorld) {
+
+        // Scaling
+        Vector3f halfSize = new Vector3f(size).multiply(0.5f);
+        Transform4f scaling = new Transform4f();
+        scaling.getRotation().setDiagonal(halfSize);
+        setScalingTransform(scaling); // Attach to game object
+
+        Vector3 extent = new Vector3(size.getX(), size.getY(), size.getZ()).multiply(0.5f);
+        BoxShape collisionShape = new BoxShape(extent, 0.02f);
+        createRigidBody(collisionShape, position, mass, dynamicsWorld);
+
+        try {
+            // Mesh
+            Mesh mesh = GL.meshFactory.createMesh();
+            setMesh(mesh); // Attach mesh to game object
+            Fw.graphics.loadMesh("primitives/cube.obj", mesh);
+            mesh.setAllColors(0.3f, 0.3f, 0.3f, 1.0f);
+            mesh.updateHasBooleansFromSegment();
+
+            // Renderable
+            AbstractRenderable renderable = Fw.graphics.createInterleavedRenderable();
+            renderable.create(mesh);
+            setRenderable(renderable); // Attach to game object
+        } catch (IOException ex) {
+            throw new RuntimeException(ex);
+        }
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisionshapes/Capsule.java

@@ -0,0 +1,71 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisionshapes;
+
+import java.io.IOException;
+import net.frameworkgl.Fw;
+import net.frameworkgl.math.Transform4f;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.mesh.Mesh;
+import net.frameworkgl.opengl.renderable.AbstractRenderable;
+import org.atriasoft.ephysics.collision.shapes.CapsuleShape;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Capsule extends AbstractGameObjectShape {
+
+    public Capsule(Vector3f position, float height, float radius, float mass, DynamicsWorld dynamicsWorld) {
+
+        // Scaling
+        Vector3f size = new Vector3f(radius, (height + 2.0f * radius) / 3.0f, radius);
+        Transform4f scaling = new Transform4f();
+        scaling.getRotation().setDiagonal(size);
+        setScalingTransform(scaling); // Attach to game object
+
+        CapsuleShape collisionShape = new CapsuleShape(radius, height, 0.02f);
+        createRigidBody(collisionShape, position, mass, dynamicsWorld);
+
+        try {
+            // Mesh
+            Mesh mesh = GL.meshFactory.createMesh();
+            setMesh(mesh); // Attach mesh to game object
+            Fw.graphics.loadMesh("primitives/capsule.obj", mesh);
+            mesh.setAllColors(0.8f, 0.8f, 0.8f, 1.0f);
+            mesh.updateHasBooleansFromSegment();
+
+            // Renderable
+            AbstractRenderable renderable = Fw.graphics.createInterleavedRenderable();
+            renderable.create(mesh);
+            setRenderable(renderable); // Attach to game object
+        } catch (IOException ex) {
+            throw new RuntimeException(ex);
+        }
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisionshapes/CollisionShapes.java

@@ -0,0 +1,408 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisionshapes;
+
+import java.nio.FloatBuffer;
+import java.util.ArrayList;
+import java.util.List;
+import net.frameworkgl.Fw;
+import net.frameworkgl.gameobjects.GameObject;
+import net.frameworkgl.Screen;
+import net.frameworkgl.helpers.Keyboard;
+import net.frameworkgl.math.MathHelper;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.camera.LegacyCamera;
+import net.frameworkgl.opengl.camera.LegacyCameraController;
+import net.frameworkgl.opengl.constants.GetString;
+import net.frameworkgl.opengl.constants.Light;
+import net.frameworkgl.utils.FpsTimer;
+import net.frameworkgl.utils.MemoryUsage;
+import org.atriasoft.ephysics.body.RigidBody;
+import org.atriasoft.ephysics.constraint.ContactPoint;
+import org.atriasoft.ephysics.engine.ContactManifold;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+import org.atriasoft.ephysics.engine.Material;
+import org.atriasoft.ephysics.mathematics.Vector3;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class CollisionShapes extends Screen {
+
+    private final static Logger log = LoggerFactory.getLogger(CollisionShapes.class);
+    private static final float BOX_MASS = 1.0f;
+    private static final float CAPSULE_HEIGHT = 1.0f;
+    private static final float CAPSULE_MASS = 1.0f;
+    private static final float CAPSULE_RADIUS = 1.0f;
+    private static final float CONE_HEIGHT = 3.0f;
+    private static final float CONE_MASS = 1.0f;
+    private static final float CONE_RADIUS = 2.0f;
+    private static final float CYLINDER_HEIGHT = 5.0f;
+    private static final float CYLINDER_MASS = 1.0f;
+    private static final float CYLINDER_RADIUS = 1.0f;
+    private static final float FLOOR_MASS = 100.0f;
+    private static final float SPHERE_MASS = 1.5f;
+    private static final float SPHERE_RADIUS = 1.0f;
+    private static final int MAX_BOXES = 5;
+    private static final int MAX_CAPSULES = 5;
+    private static final int MAX_CONES = 5;
+    private static final int MAX_CYLINDERS = 5;
+    private static final int MAX_SPHERES = 5;
+    private static final Vector3f BOX_SIZE = new Vector3f(2.0f, 2.0f, 2.0f);
+    private static final Vector3f FLOOR_SIZE = new Vector3f(50.0f, 0.5f, 50.0f);
+
+    private DynamicsWorld dynamicsWorld;
+    private FloatBuffer lightFloatBuffer;
+    private FpsTimer fpsTimer;
+    private LegacyCamera camera;
+    private LegacyCameraController cameraController;
+    private final List<AbstractGameObjectShape> gameObjectShapes;
+    private final List<VisualContactPoint> visualContactPoints;
+    private MemoryUsage memoryUsage;
+
+    public CollisionShapes(String[] args) {
+        gameObjectShapes = new ArrayList<>();
+        visualContactPoints = new ArrayList<>();
+    }
+
+    private void createBoxes() {
+
+        // Create all the boxes of the scene
+        for (int i = 0; i < MAX_BOXES; i++) {
+
+            // Position
+            float angle = i * 30.0f;
+            float radius = 3.0f;
+            Vector3f position = new Vector3f(
+                    radius * MathHelper.Cos(angle),
+                    60.0f + i * (BOX_SIZE.getY() + 0.8f),
+                    radius * MathHelper.Sin(angle));
+
+            // Create a box and a corresponding rigid body in the dynamics world
+            Box box = new Box(position, BOX_SIZE, BOX_MASS, dynamicsWorld);
+
+            // The box is a moving rigid body
+            RigidBody rigidBody = (RigidBody) box.getRigidBody();
+            rigidBody.setIsMotionEnabled(true);
+
+            // Change the material properties of the rigid body
+            Material material = rigidBody.getMaterial();
+            material.setBounciness(0.2f);
+
+            // Add the box to the list of game objects in the scene
+            gameObjectShapes.add(box);
+        }
+    }
+
+    private void createCapsule() {
+
+        // Create all the capsules of the scene
+        for (int i = 0; i < MAX_CAPSULES; i++) {
+
+            // Position
+            float angle = i * 45.0f;
+            float radius = 3.0f;
+            Vector3f position = new Vector3f(
+                    radius * MathHelper.Cos(angle),
+                    15.0f + i * (CAPSULE_HEIGHT + 0.5f),
+                    radius * MathHelper.Sin(angle));
+
+            // Create a capsule and a corresponding rigid body in the dynamics world
+            Capsule capsule = new Capsule(position, CAPSULE_HEIGHT, CAPSULE_RADIUS, CAPSULE_MASS, dynamicsWorld);
+
+            // The cylinder is a moving rigid body
+            RigidBody rigidBody = (RigidBody) capsule.getRigidBody();
+            rigidBody.setIsMotionEnabled(true);
+
+            // Change the material properties of the rigid body
+            Material material = rigidBody.getMaterial();
+            material.setBounciness(0.2f);
+
+            // Add the capsule to the list of game objects in the scene
+            gameObjectShapes.add(capsule);
+        }
+    }
+
+    private void createCones() {
+
+        // Create all the cones of the scene
+        for (int i = 0; i < MAX_CONES; i++) {
+
+            // Position
+            float angle = i * 50.0f;
+            float radius = 3.0f;
+            Vector3f position = new Vector3f(
+                    radius * MathHelper.Cos(angle),
+                    35.0f + i * (CONE_HEIGHT + 0.3f),
+                    radius * MathHelper.Sin(angle));
+
+            // Create a cone and a corresponding rigid body in the dynamics world
+            Cone cone = new Cone(position, CONE_HEIGHT, CONE_RADIUS, CONE_MASS, dynamicsWorld);
+
+            // The cone is a moving rigid body
+            RigidBody rigidBody = (RigidBody) cone.getRigidBody();
+            rigidBody.setIsMotionEnabled(true);
+
+            // Change the material properties of the rigid body
+            Material material = rigidBody.getMaterial();
+            material.setBounciness(0.2f);
+
+            // Add the cone to the list of game objects in the scene
+            gameObjectShapes.add(cone);
+        }
+    }
+
+    private void createCylinder() {
+
+        // Create all the cylinders of the scene
+        for (int i = 0; i < MAX_CYLINDERS; i++) {
+
+            // Position
+            float angle = i * 35.0f;
+            float radius = 3.0f;
+            Vector3f position = new Vector3f(
+                    radius * MathHelper.Cos(angle),
+                    25.0f + i * (CYLINDER_HEIGHT + 0.3f),
+                    radius * MathHelper.Sin(angle));
+
+            // Create a cylinder and a corresponding rigid body in the dynamics world
+            Cylinder cylinder = new Cylinder(position, CYLINDER_HEIGHT, CYLINDER_RADIUS, CYLINDER_MASS, dynamicsWorld);
+
+            // The cylinder is a moving rigid body
+            RigidBody rigidBody = (RigidBody) cylinder.getRigidBody();
+            rigidBody.setIsMotionEnabled(true);
+
+            // Change the material properties of the rigid body
+            Material material = rigidBody.getMaterial();
+            material.setBounciness(0.2f);
+
+            // Add the cylinder to the list of game objects in the scene
+            gameObjectShapes.add(cylinder);
+        }
+    }
+
+    private void createFloor() {
+
+        // Create the floor
+        Vector3f floorPosition = new Vector3f();
+        Box floor = new Box(floorPosition, FLOOR_SIZE, FLOOR_MASS, dynamicsWorld);
+
+        // The floor must be a non-moving rigid body
+        RigidBody rigidBody = (RigidBody) floor.getRigidBody();
+        rigidBody.setIsMotionEnabled(false);
+
+        // Change the material properties of the rigid body
+        Material material = rigidBody.getMaterial();
+        material.setBounciness(0.2f);
+
+        // Add the floor to the list of game objects in the scene
+        gameObjectShapes.add(floor);
+    }
+
+    private void createSpheres() {
+
+        // Create all the spheres of the scene
+        for (int i = 0; i < MAX_SPHERES; i++) {
+
+            // Position
+            float angle = i * 35.0f;
+            float radius = 3.0f;
+            Vector3f position = new Vector3f(
+                    radius * MathHelper.Cos(angle),
+                    50.0f + i * (SPHERE_RADIUS + 0.8f),
+                    radius * MathHelper.Sin(angle));
+
+            // Create a sphere and a corresponding rigid body in the dynamics world
+            Sphere sphere = new Sphere(position, SPHERE_RADIUS, SPHERE_MASS, dynamicsWorld);
+
+            // The sphere is a moving rigid body
+            RigidBody rigidBody = (RigidBody) sphere.getRigidBody();
+            rigidBody.setIsMotionEnabled(true);
+
+            // Change the material properties of the rigid body
+            Material material = rigidBody.getMaterial();
+            material.setBounciness(0.2f);
+
+            // Add the sphere to the list of game objects in the scene
+            gameObjectShapes.add(sphere);
+        }
+    }
+
+    private void generateContactPoints() {
+        visualContactPoints.clear();
+        List<ContactManifold> contactManifolds = dynamicsWorld.getContactManifolds();
+        for (ContactManifold contactManifold : contactManifolds) {
+            for (int i = 0; i < contactManifold.getNumContactPoints(); i++) {
+                ContactPoint point = contactManifold.getContactPoint(i);
+                Vector3 pos = point.getWorldPointOnBody1();
+                Vector3f position = new Vector3f(pos.getX(), pos.getY(), pos.getZ());
+                VisualContactPoint visualPoint = new VisualContactPoint(position);
+                visualContactPoints.add(visualPoint);
+            }
+        }
+    }
+
+    private void handleInput() {
+        if (Fw.input.isKeyDown(Keyboard.ESCAPE) == true) {
+            Fw.app.stopRunning();
+        }
+        cameraController.update();
+    }
+
+    @Override
+    public void destroy() {
+        for (GameObject gameObjectShape : gameObjectShapes) {
+            gameObjectShape.destroy();
+        }
+        VisualContactPoint.Destroy();
+        Fw.input.removeInputProcessor(cameraController);
+        Fw.input.releaseMouseCursor();
+    }
+
+    @Override
+    public void init() {
+
+        // Create timer
+        fpsTimer = new FpsTimer();
+
+        // Setup camera and controller
+        camera = GL.cameraFactory.createLegacyCamera();
+        camera.setPosition(0.0f, 6.0f, 25.0f);
+        cameraController = GL.cameraFactory.createLegacyCameraController();
+        cameraController.setCamera(camera);
+
+        // Time step and gravity for the physics simulation
+        float timeStep = 1.0f / 60.0f;
+        Vector3 gravity = new Vector3(0, -9.81f, 0);
+
+        // Create dynamics world
+        dynamicsWorld = new DynamicsWorld(gravity, timeStep);
+        dynamicsWorld.start();
+
+        // Float buffer for light and matrices
+        lightFloatBuffer = GL.bufferHelper.createFloatBuffer(4);
+
+        // Memory usage
+        memoryUsage = new MemoryUsage();
+
+        // Create game objects
+        createBoxes();
+        createCapsule();
+        createCones();
+        createCylinder();
+        createFloor();
+        createSpheres();
+
+        GL.o1.enableCulling();
+        GL.o1.cullBackFaces();
+        GL.o1.enableDepthTest();
+        GL.o1.setDepthFuncLess();
+        GL.o1.enableDepthMask();
+        GL.o1.setClearDepth(1.0f);
+        GL.o1.enableColorMaterial();
+        GL.o1.enableLight0();
+        GL.o1.enableLighting();
+        GL.o1.setSmoothLighting(true);
+        GL.o1.enableNormalize();
+        GL.o1.clear();
+
+        GL.o1.setProjectionPerspective(
+                70.0f,
+                (float) Fw.config.getCurrentWidth() / (float) Fw.config.getCurrentHeight(),
+                0.05f, 512.0f);
+        GL.o1.setModelViewIdentity();
+
+        // Light position
+        lightFloatBuffer.put(0.0f);
+        lightFloatBuffer.put(15.0f);
+        lightFloatBuffer.put(0.0f);
+        lightFloatBuffer.put(1.0f);
+        lightFloatBuffer.flip();
+
+        log.info("OpenGL version: " + GL.o1.getString(GetString.VERSION));
+
+        Fw.input.addInputProcessor(cameraController);
+        Fw.input.grabMouseCursor();
+    }
+
+    @Override
+    public void pause() {
+    }
+
+    @Override
+    public void render() {
+        fpsTimer.update();
+        memoryUsage.update();
+
+        if (Fw.timer.isGameTick()) {
+            // Do nothing
+        }
+
+        if (Fw.timer.isRenderTick()) {
+            handleInput();
+
+            // Take a simulation step
+            dynamicsWorld.update();
+
+            // Generate the new visual contact points
+            generateContactPoints();
+
+            // Clear screen and reset modelview matrix
+            GL.o1.clear();
+            GL.o1.setModelViewIdentity();
+
+            camera.updateOpenGL();
+
+            GL.o1.light(Light.LIGHT0, Light.POSITION, lightFloatBuffer);
+
+            // Render directly
+            for (AbstractGameObjectShape gameObjectShape : gameObjectShapes) {
+                gameObjectShape.updateTransform();
+                Fw.graphics.render(gameObjectShape);
+            }
+
+            // Render contact points
+            GL.o1.disableDepthTest();
+            for (GameObject visualContactPoint : visualContactPoints) {
+                Fw.graphics.render(visualContactPoint);
+            }
+            GL.o1.enableDepthTest();
+        }
+    }
+
+    @Override
+    public void resize(int width, int height) {
+        GL.o1.setViewport(0, 0, width, height);
+    }
+
+    @Override
+    public void resume() {
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisionshapes/Cone.java

@@ -0,0 +1,71 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisionshapes;
+
+import java.io.IOException;
+import net.frameworkgl.Fw;
+import net.frameworkgl.math.Transform4f;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.mesh.Mesh;
+import net.frameworkgl.opengl.renderable.AbstractRenderable;
+import org.atriasoft.ephysics.collision.shapes.ConeShape;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Cone extends AbstractGameObjectShape {
+
+    public Cone(Vector3f position, float height, float radius, float mass, DynamicsWorld dynamicsWorld) {
+
+        // Scaling
+        Vector3f size = new Vector3f(radius, height, radius);
+        Transform4f scaling = new Transform4f();
+        scaling.getRotation().setDiagonal(size);
+        setScalingTransform(scaling); // Attach to game object
+
+        ConeShape collisionShape = new ConeShape(radius, height, 0.02f);
+        createRigidBody(collisionShape, position, mass, dynamicsWorld);
+
+        try {
+            // Mesh
+            Mesh mesh = GL.meshFactory.createMesh();
+            setMesh(mesh); // Attach mesh to game object
+            Fw.graphics.loadMesh("primitives/cone.obj", mesh);
+            mesh.setAllColors(0.8f, 0.8f, 0.8f, 1.0f);
+            mesh.updateHasBooleansFromSegment();
+
+            // Renderable
+            AbstractRenderable renderable = Fw.graphics.createInterleavedRenderable();
+            renderable.create(mesh);
+            setRenderable(renderable); // Attach to game object
+        } catch (IOException ex) {
+            throw new RuntimeException(ex);
+        }
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisionshapes/Configuration.java

@@ -0,0 +1,39 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisionshapes;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Configuration extends net.frameworkgl.Configuration {
+
+    public Configuration(String[] args) {
+        super(args);
+        withOpenGL33ProfileCompatibility();
+        setWindowTitle("Collision Shapes");
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisionshapes/Cylinder.java

@@ -0,0 +1,71 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisionshapes;
+
+import java.io.IOException;
+import net.frameworkgl.Fw;
+import net.frameworkgl.math.Transform4f;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.mesh.Mesh;
+import net.frameworkgl.opengl.renderable.AbstractRenderable;
+import org.atriasoft.ephysics.collision.shapes.CylinderShape;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Cylinder extends AbstractGameObjectShape {
+
+    public Cylinder(Vector3f position, float height, float radius, float mass, DynamicsWorld dynamicsWorld) {
+
+        // Scaling
+        Vector3f size = new Vector3f(radius, height, radius);
+        Transform4f scaling = new Transform4f();
+        scaling.getRotation().setDiagonal(size);
+        setScalingTransform(scaling); // Attach to game object
+
+        CylinderShape collisionShape = new CylinderShape(radius, height, 0.02f);
+        createRigidBody(collisionShape, position, mass, dynamicsWorld);
+
+        try {
+            // Mesh
+            Mesh mesh = GL.meshFactory.createMesh();
+            setMesh(mesh); // Attach mesh to game object
+            Fw.graphics.loadMesh("primitives/cylinder.obj", mesh);
+            mesh.setAllColors(0.8f, 0.8f, 0.8f, 1.0f);
+            mesh.updateHasBooleansFromSegment();
+
+            // Renderable
+            AbstractRenderable renderable = Fw.graphics.createInterleavedRenderable();
+            renderable.create(mesh);
+            setRenderable(renderable); // Attach to game object
+        } catch (IOException ex) {
+            throw new RuntimeException(ex);
+        }
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisionshapes/Demo.java

@@ -0,0 +1,43 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisionshapes;
+
+import net.frameworkgl.Bootstrap;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Demo {
+
+    /**
+     * @param args the command line arguments
+     */
+    public static void main(String[] args) {
+        Bootstrap boot = new Bootstrap();
+        boot.start(Configuration.class, CollisionShapes.class, args);
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisionshapes/Sphere.java

@@ -0,0 +1,71 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisionshapes;
+
+import java.io.IOException;
+import net.frameworkgl.Fw;
+import net.frameworkgl.math.Transform4f;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.mesh.Mesh;
+import net.frameworkgl.opengl.renderable.AbstractRenderable;
+import org.atriasoft.ephysics.collision.shapes.SphereShape;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Sphere extends AbstractGameObjectShape {
+
+    public Sphere(Vector3f position, float radius, float mass, DynamicsWorld dynamicsWorld) {
+
+        // Scaling
+        Vector3f size = new Vector3f(radius, radius, radius);
+        Transform4f scaling = new Transform4f();
+        scaling.getRotation().setDiagonal(size);
+        setScalingTransform(scaling); // Attach to game object
+
+        SphereShape collisionShape = new SphereShape(radius, 0.02f);
+        createRigidBody(collisionShape, position, mass, dynamicsWorld);
+
+        try {
+            // Mesh
+            Mesh mesh = GL.meshFactory.createMesh();
+            setMesh(mesh); // Attach mesh to game object
+            Fw.graphics.loadMesh("primitives/uvsphere.obj", mesh);
+            mesh.setAllColors(0.8f, 0.8f, 0.8f, 1.0f);
+            mesh.updateHasBooleansFromSegment();
+
+            // Renderable
+            AbstractRenderable renderable = Fw.graphics.createInterleavedRenderable();
+            renderable.create(mesh);
+            setRenderable(renderable); // Attach to game object
+        } catch (IOException ex) {
+            throw new RuntimeException(ex);
+        }
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisionshapes/VisualContactPoint.java

@@ -0,0 +1,96 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisionshapes;
+
+import java.io.IOException;
+import net.frameworkgl.Fw;
+import net.frameworkgl.gameobjects.GameObject;
+import net.frameworkgl.math.Transform4f;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.mesh.Mesh;
+import net.frameworkgl.opengl.renderable.AbstractRenderable;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class VisualContactPoint extends GameObject {
+
+    private static final float VISUAL_CONTACT_POINT_RADIUS = 0.1f;
+
+    private static boolean initialized;
+    private static Mesh staticMesh;
+    private static AbstractRenderable staticRenderable;
+
+    public VisualContactPoint(Vector3f position) {
+
+        // Scaling
+        Vector3f size = new Vector3f(VISUAL_CONTACT_POINT_RADIUS, VISUAL_CONTACT_POINT_RADIUS, VISUAL_CONTACT_POINT_RADIUS);
+        Transform4f scaling = new Transform4f();
+        scaling.getRotation().setDiagonal(size);
+        setScalingTransform(scaling); // Attach to game object
+
+        getWorldTransform().getPosition().set(position);
+        getWorldTransform().multiply(getScalingTransform());
+
+        CreateStaticData();
+        setMesh(staticMesh); // Attach mesh to game object
+        setRenderable(staticRenderable); // Attach to game object
+    }
+
+    public static void CreateStaticData() {
+        if (initialized) {
+            return;
+        }
+
+        try {
+            // Mesh
+            staticMesh = GL.meshFactory.createMesh();
+            Fw.graphics.loadMesh("primitives/cone.obj", staticMesh);
+            staticMesh.setAllColors(1.0f, 1.0f, 0.0f, 1.0f);
+            staticMesh.updateHasBooleansFromSegment();
+
+            // Renderable
+            staticRenderable = Fw.graphics.createInterleavedRenderable();
+            staticRenderable.create(staticMesh);
+        } catch (IOException ex) {
+            throw new RuntimeException(ex);
+        }
+
+        // Only do this once
+        initialized = true;
+    }
+
+    public static void Destroy() {
+        if (!initialized) {
+            return;
+        }
+        staticRenderable.destroy();
+        staticRenderable = null;
+        initialized = false;
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisiontest/AbstractGameObjectShape.java

@@ -0,0 +1,82 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisiontest;
+
+import net.frameworkgl.gameobjects.GameObject;
+import net.frameworkgl.math.Vector3f;
+import org.atriasoft.ephysics.body.RigidBody;
+import org.atriasoft.ephysics.collision.shapes.CollisionShape;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+import org.atriasoft.ephysics.mathematics.Matrix3x3;
+import org.atriasoft.etk.math.Quaternion;
+import org.atriasoft.ephysics.mathematics.Transform;
+import org.atriasoft.ephysics.mathematics.Vector3;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public abstract class AbstractGameObjectShape extends GameObject {
+
+    private CollisionShape collisionShape;
+    private RigidBody rigidBody;
+
+    protected void createRigidBody(
+            CollisionShape collisionShape, Vector3f position, float mass, DynamicsWorld dynamicsWorld) {
+
+        this.collisionShape = collisionShape;
+
+        Matrix3x3 inertiaTensor = collisionShape.computeLocalInertiaTensor(mass);
+
+        Quaternion initOrientation = new Quaternion().identity();
+        Vector3 initPosition = new Vector3(position.getX(), position.getY(), position.getZ());
+        Transform transform = new Transform(initPosition, initOrientation);
+
+        rigidBody = dynamicsWorld.createRigidBody(transform, mass, inertiaTensor, collisionShape);
+    }
+
+    public CollisionShape getCollisionShape() {
+        return collisionShape;
+    }
+
+    public RigidBody getRigidBody() {
+        return rigidBody;
+    }
+
+    public void updateTransform() {
+
+        // Get the interpolated transform of the rigid body
+        Transform transform = rigidBody.getInterpolatedTransform(new Transform());
+
+        // Compute the transform used for rendering the box
+        float[] glMatrix = new float[16];
+        transform.getOpenGLMatrix(glMatrix);
+
+        // Apply the scaling matrix to have the correct box dimensions
+        getWorldTransform().fromOpenGLArray(glMatrix);
+        getWorldTransform().multiply(getScalingTransform());
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisiontest/Box.java

@@ -0,0 +1,73 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisiontest;
+
+import java.io.IOException;
+import net.frameworkgl.Fw;
+import net.frameworkgl.math.Transform4f;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.mesh.Mesh;
+import net.frameworkgl.opengl.renderable.AbstractRenderable;
+import org.atriasoft.ephysics.collision.shapes.BoxShape;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+import org.atriasoft.ephysics.mathematics.Vector3;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Box extends AbstractGameObjectShape {
+
+    public Box(Vector3f position, Vector3f size, float mass, DynamicsWorld dynamicsWorld) {
+
+        // Scaling
+        Vector3f halfSize = new Vector3f(size).multiply(0.5f);
+        Transform4f scaling = new Transform4f();
+        scaling.getRotation().setDiagonal(halfSize);
+        setScalingTransform(scaling); // Attach to game object
+
+        Vector3 extent = new Vector3(size.getX(), size.getY(), size.getZ()).multiply(0.5f);
+        BoxShape collisionShape = new BoxShape(extent, 0.02f);
+        createRigidBody(collisionShape, position, mass, dynamicsWorld);
+
+        try {
+            // Mesh
+            Mesh mesh = GL.meshFactory.createMesh();
+            setMesh(mesh); // Attach mesh to game object
+            Fw.graphics.loadMesh("primitives/cube.obj", mesh);
+            mesh.setAllColors(0.3f, 0.3f, 0.3f, 1.0f);
+            mesh.updateHasBooleansFromSegment();
+
+            // Renderable
+            AbstractRenderable renderable = Fw.graphics.createInterleavedRenderable();
+            renderable.create(mesh);
+            setRenderable(renderable); // Attach to game object
+        } catch (IOException ex) {
+            throw new RuntimeException(ex);
+        }
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisiontest/Capsule.java

@@ -0,0 +1,71 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisiontest;
+
+import java.io.IOException;
+import net.frameworkgl.Fw;
+import net.frameworkgl.math.Transform4f;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.mesh.Mesh;
+import net.frameworkgl.opengl.renderable.AbstractRenderable;
+import org.atriasoft.ephysics.collision.shapes.CapsuleShape;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Capsule extends AbstractGameObjectShape {
+
+    public Capsule(Vector3f position, float height, float radius, float mass, DynamicsWorld dynamicsWorld) {
+
+        // Scaling
+        Vector3f size = new Vector3f(radius, (height + 2.0f * radius) / 3.0f, radius);
+        Transform4f scaling = new Transform4f();
+        scaling.getRotation().setDiagonal(size);
+        setScalingTransform(scaling); // Attach to game object
+
+        CapsuleShape collisionShape = new CapsuleShape(radius, height, 0.02f);
+        createRigidBody(collisionShape, position, mass, dynamicsWorld);
+
+        try {
+            // Mesh
+            Mesh mesh = GL.meshFactory.createMesh();
+            setMesh(mesh); // Attach mesh to game object
+            Fw.graphics.loadMesh("primitives/capsule.obj", mesh);
+            mesh.setAllColors(0.8f, 0.8f, 0.8f, 1.0f);
+            mesh.updateHasBooleansFromSegment();
+
+            // Renderable
+            AbstractRenderable renderable = Fw.graphics.createInterleavedRenderable();
+            renderable.create(mesh);
+            setRenderable(renderable); // Attach to game object
+        } catch (IOException ex) {
+            throw new RuntimeException(ex);
+        }
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisiontest/CollisionTest.java

@@ -0,0 +1,280 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisiontest;
+
+import java.nio.FloatBuffer;
+import java.util.ArrayList;
+import java.util.List;
+import net.frameworkgl.Fw;
+import net.frameworkgl.gameobjects.GameObject;
+import net.frameworkgl.Screen;
+import net.frameworkgl.helpers.Keyboard;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.camera.LegacyCamera;
+import net.frameworkgl.opengl.camera.LegacyCameraController;
+import net.frameworkgl.opengl.constants.GetString;
+import net.frameworkgl.opengl.constants.Light;
+import net.frameworkgl.utils.FpsTimer;
+import net.frameworkgl.utils.MemoryUsage;
+import org.atriasoft.ephysics.body.RigidBody;
+import org.atriasoft.ephysics.constraint.ContactPoint;
+import org.atriasoft.ephysics.engine.ContactManifold;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+import org.atriasoft.ephysics.engine.Material;
+import org.atriasoft.ephysics.mathematics.Vector3;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class CollisionTest extends Screen {
+
+    private final static Logger log = LoggerFactory.getLogger(CollisionTest.class);
+    private static final float BOX_MASS = 1.0f;
+    private static final float CAPSULE_HEIGHT = 1.0f;
+    private static final float CAPSULE_MASS = 1.0f;
+    private static final float CAPSULE_RADIUS = 1.0f;
+    private static final float CONE_HEIGHT = 3.0f;
+    private static final float CONE_MASS = 1.0f;
+    private static final float CONE_RADIUS = 2.0f;
+    private static final float CYLINDER_HEIGHT = 5.0f;
+    private static final float CYLINDER_MASS = 1.0f;
+    private static final float CYLINDER_RADIUS = 1.0f;
+    private static final float FLOOR_MASS = 100.0f;
+    private static final float SPHERE_MASS = 1.5f;
+    private static final float SPHERE_RADIUS = 1.0f;
+    private static final Vector3f BOX_SIZE = new Vector3f(2.0f, 2.0f, 2.0f);
+
+    private DynamicsWorld dynamicsWorld;
+    private FloatBuffer lightFloatBuffer;
+    private FpsTimer fpsTimer;
+    private LegacyCamera camera;
+    private LegacyCameraController cameraController;
+    private final List<AbstractGameObjectShape> gameObjectShapes;
+    private final List<VisualContactPoint> visualContactPoints;
+    private MemoryUsage memoryUsage;
+
+    public CollisionTest(String[] args) {
+        gameObjectShapes = new ArrayList<>();
+        visualContactPoints = new ArrayList<>();
+    }
+
+    private void generateContactPoints() {
+        visualContactPoints.clear();
+        List<ContactManifold> contactManifolds = dynamicsWorld.getContactManifolds();
+        for (ContactManifold contactManifold : contactManifolds) {
+            for (int i = 0; i < contactManifold.getNumContactPoints(); i++) {
+                ContactPoint point = contactManifold.getContactPoint(i);
+                Vector3 pos = point.getWorldPointOnBody1();
+                Vector3f position = new Vector3f(pos.getX(), pos.getY(), pos.getZ());
+                VisualContactPoint visualPoint = new VisualContactPoint(position);
+                visualContactPoints.add(visualPoint);
+            }
+        }
+    }
+
+    private void handleInput() {
+        if (Fw.input.isKeyDown(Keyboard.ESCAPE) == true) {
+            Fw.app.stopRunning();
+        }
+        Vector3f direction = new Vector3f();
+        if (Fw.input.isKeyDown(Keyboard.NUM0) == true) {
+            direction.setY(1.0f);
+        }
+        if (Fw.input.isKeyDown(Keyboard.NUM2) == true) {
+            direction.setY(-1.0f);
+        }
+        if (Fw.input.isKeyDown(Keyboard.NUM4) == true) {
+            direction.setX(-1.0f);
+        }
+        if (Fw.input.isKeyDown(Keyboard.NUM6) == true) {
+            direction.setX(1.0f);
+        }
+        if (Fw.input.isKeyDown(Keyboard.NUM7) == true) {
+            direction.setZ(-1.0f);
+        }
+        if (Fw.input.isKeyDown(Keyboard.NUM9) == true) {
+            direction.setZ(1.0f);
+        }
+        RigidBody rigidBody = (RigidBody) cylinderMoveable.getRigidBody();
+        if (Fw.input.isKeyDown(Keyboard.NUM1) == true) {
+            rigidBody.applyTorque(new Vector3(0.5f, 0.0f, 0.0f));
+        }
+        if (Fw.input.isKeyDown(Keyboard.NUM3) == true) {
+            rigidBody.applyTorque(new Vector3(-0.5f, 0.0f, 0.0f));
+        }
+        if (direction.magnitudeSquared() > 0) {
+            direction.normalize();
+            rigidBody.applyForceToCenter(new Vector3(direction.getX(), direction.getY(), direction.getZ()));
+        }
+        cameraController.update();
+    }
+
+    @Override
+    public void destroy() {
+        for (GameObject gameObjectShape : gameObjectShapes) {
+            gameObjectShape.destroy();
+        }
+        VisualContactPoint.Destroy();
+        Fw.input.removeInputProcessor(cameraController);
+        Fw.input.releaseMouseCursor();
+    }
+
+    private Cylinder cylinderMoveable;
+
+    @Override
+    public void init() {
+
+        // Create timer
+        fpsTimer = new FpsTimer();
+
+        // Setup camera and controller
+        camera = GL.cameraFactory.createLegacyCamera();
+        camera.setPosition(0.0f, 1.0f, 5.0f);
+        cameraController = GL.cameraFactory.createLegacyCameraController();
+        cameraController.setCamera(camera);
+
+        // Time step and gravity for the physics simulation
+        float timeStep = 1.0f / 60.0f;
+        Vector3 gravity = new Vector3(0, -9.81f, 0);
+
+        // Create dynamics world
+        dynamicsWorld = new DynamicsWorld(gravity, timeStep);
+        dynamicsWorld.setIsGratityEnabled(false);
+        dynamicsWorld.start();
+
+        // Float buffer for light and matrices
+        lightFloatBuffer = GL.bufferHelper.createFloatBuffer(4);
+
+        // Memory usage
+        memoryUsage = new MemoryUsage();
+
+        // Add spheres
+        Material material;
+        RigidBody rigidBody;
+        Box floor;
+        floor = new Box(new Vector3f(), new Vector3f(40.0f, 0.5f, 40.0f), FLOOR_MASS, dynamicsWorld);
+        rigidBody = (RigidBody) floor.getRigidBody();
+        rigidBody.setIsMotionEnabled(false);
+        material = rigidBody.getMaterial();
+        material.setBounciness(0.2f);
+        gameObjectShapes.add(floor);
+        cylinderMoveable = new Cylinder(new Vector3f(0.0f, 3.0f, 0.0f), CYLINDER_HEIGHT, CYLINDER_RADIUS, CYLINDER_MASS, dynamicsWorld);
+        rigidBody = (RigidBody) cylinderMoveable.getRigidBody();
+        rigidBody.setIsMotionEnabled(true);
+        material = rigidBody.getMaterial();
+        material.setBounciness(0.2f);
+        gameObjectShapes.add(cylinderMoveable);
+
+        GL.o1.enableCulling();
+        GL.o1.cullBackFaces();
+        GL.o1.enableDepthTest();
+        GL.o1.setDepthFuncLess();
+        GL.o1.enableDepthMask();
+        GL.o1.setClearDepth(1.0f);
+        GL.o1.enableColorMaterial();
+        GL.o1.enableLight0();
+        GL.o1.enableLighting();
+        GL.o1.setSmoothLighting(true);
+        GL.o1.enableNormalize();
+        GL.o1.clear();
+
+        GL.o1.setProjectionPerspective(
+                70.0f,
+                (float) Fw.config.getCurrentWidth() / (float) Fw.config.getCurrentHeight(),
+                0.05f, 512.0f);
+        GL.o1.setModelViewIdentity();
+
+        // Light position
+        lightFloatBuffer.put(0.0f);
+        lightFloatBuffer.put(15.0f);
+        lightFloatBuffer.put(0.0f);
+        lightFloatBuffer.put(1.0f);
+        lightFloatBuffer.flip();
+
+        log.info("OpenGL version: " + GL.o1.getString(GetString.VERSION));
+
+        Fw.input.addInputProcessor(cameraController);
+        Fw.input.grabMouseCursor();
+    }
+
+    @Override
+    public void pause() {
+    }
+
+    @Override
+    public void render() {
+        fpsTimer.update();
+        memoryUsage.update();
+
+        if (Fw.timer.isGameTick()) {
+            // Do nothing
+        }
+
+        if (Fw.timer.isRenderTick()) {
+            handleInput();
+
+            // Take a simulation step
+            dynamicsWorld.update();
+
+            // Generate the new visual contact points
+            generateContactPoints();
+
+            // Clear screen and reset modelview matrix
+            GL.o1.clear();
+            GL.o1.setModelViewIdentity();
+
+            camera.updateOpenGL();
+
+            GL.o1.light(Light.LIGHT0, Light.POSITION, lightFloatBuffer);
+
+            // Render directly
+            for (AbstractGameObjectShape gameObjectShape : gameObjectShapes) {
+                gameObjectShape.updateTransform();
+                Fw.graphics.render(gameObjectShape);
+            }
+
+            // Render contact points
+            GL.o1.disableDepthTest();
+            for (GameObject visualContactPoint : visualContactPoints) {
+                Fw.graphics.render(visualContactPoint);
+            }
+            GL.o1.enableDepthTest();
+        }
+    }
+
+    @Override
+    public void resize(int width, int height) {
+        GL.o1.setViewport(0, 0, width, height);
+    }
+
+    @Override
+    public void resume() {
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisiontest/Cone.java

@@ -0,0 +1,71 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisiontest;
+
+import java.io.IOException;
+import net.frameworkgl.Fw;
+import net.frameworkgl.math.Transform4f;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.mesh.Mesh;
+import net.frameworkgl.opengl.renderable.AbstractRenderable;
+import org.atriasoft.ephysics.collision.shapes.ConeShape;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Cone extends AbstractGameObjectShape {
+
+    public Cone(Vector3f position, float height, float radius, float mass, DynamicsWorld dynamicsWorld) {
+
+        // Scaling
+        Vector3f size = new Vector3f(radius, height, radius);
+        Transform4f scaling = new Transform4f();
+        scaling.getRotation().setDiagonal(size);
+        setScalingTransform(scaling); // Attach to game object
+
+        ConeShape collisionShape = new ConeShape(radius, height, 0.02f);
+        createRigidBody(collisionShape, position, mass, dynamicsWorld);
+
+        try {
+            // Mesh
+            Mesh mesh = GL.meshFactory.createMesh();
+            setMesh(mesh); // Attach mesh to game object
+            Fw.graphics.loadMesh("primitives/cone.obj", mesh);
+            mesh.setAllColors(0.8f, 0.8f, 0.8f, 1.0f);
+            mesh.updateHasBooleansFromSegment();
+
+            // Renderable
+            AbstractRenderable renderable = Fw.graphics.createInterleavedRenderable();
+            renderable.create(mesh);
+            setRenderable(renderable); // Attach to game object
+        } catch (IOException ex) {
+            throw new RuntimeException(ex);
+        }
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisiontest/Configuration.java

@@ -0,0 +1,39 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisiontest;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Configuration extends net.frameworkgl.Configuration {
+
+    public Configuration(String[] args) {
+        super(args);
+        withOpenGL33ProfileCompatibility();
+        setWindowTitle("Collision Test");
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisiontest/Cylinder.java

@@ -0,0 +1,71 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisiontest;
+
+import java.io.IOException;
+import net.frameworkgl.Fw;
+import net.frameworkgl.math.Transform4f;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.mesh.Mesh;
+import net.frameworkgl.opengl.renderable.AbstractRenderable;
+import org.atriasoft.ephysics.collision.shapes.CylinderShape;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Cylinder extends AbstractGameObjectShape {
+
+    public Cylinder(Vector3f position, float height, float radius, float mass, DynamicsWorld dynamicsWorld) {
+
+        // Scaling
+        Vector3f size = new Vector3f(radius, height, radius);
+        Transform4f scaling = new Transform4f();
+        scaling.getRotation().setDiagonal(size);
+        setScalingTransform(scaling); // Attach to game object
+
+        CylinderShape collisionShape = new CylinderShape(radius, height, 0.02f);
+        createRigidBody(collisionShape, position, mass, dynamicsWorld);
+
+        try {
+            // Mesh
+            Mesh mesh = GL.meshFactory.createMesh();
+            setMesh(mesh); // Attach mesh to game object
+            Fw.graphics.loadMesh("primitives/cylinder.obj", mesh);
+            mesh.setAllColors(0.8f, 0.8f, 0.8f, 1.0f);
+            mesh.updateHasBooleansFromSegment();
+
+            // Renderable
+            AbstractRenderable renderable = Fw.graphics.createInterleavedRenderable();
+            renderable.create(mesh);
+            setRenderable(renderable); // Attach to game object
+        } catch (IOException ex) {
+            throw new RuntimeException(ex);
+        }
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisiontest/Demo.java

@@ -0,0 +1,43 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisiontest;
+
+import net.frameworkgl.Bootstrap;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Demo {
+
+    /**
+     * @param args the command line arguments
+     */
+    public static void main(String[] args) {
+        Bootstrap boot = new Bootstrap();
+        boot.start(Configuration.class, CollisionTest.class, args);
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisiontest/Sphere.java

@@ -0,0 +1,71 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisiontest;
+
+import java.io.IOException;
+import net.frameworkgl.Fw;
+import net.frameworkgl.math.Transform4f;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.mesh.Mesh;
+import net.frameworkgl.opengl.renderable.AbstractRenderable;
+import org.atriasoft.ephysics.collision.shapes.SphereShape;
+import org.atriasoft.ephysics.engine.DynamicsWorld;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class Sphere extends AbstractGameObjectShape {
+
+    public Sphere(Vector3f position, float radius, float mass, DynamicsWorld dynamicsWorld) {
+
+        // Scaling
+        Vector3f size = new Vector3f(radius, radius, radius);
+        Transform4f scaling = new Transform4f();
+        scaling.getRotation().setDiagonal(size);
+        setScalingTransform(scaling); // Attach to game object
+
+        SphereShape collisionShape = new SphereShape(radius, 0.02f);
+        createRigidBody(collisionShape, position, mass, dynamicsWorld);
+
+        try {
+            // Mesh
+            Mesh mesh = GL.meshFactory.createMesh();
+            setMesh(mesh); // Attach mesh to game object
+            Fw.graphics.loadMesh("primitives/uvsphere.obj", mesh);
+            mesh.setAllColors(0.8f, 0.8f, 0.8f, 1.0f);
+            mesh.updateHasBooleansFromSegment();
+
+            // Renderable
+            AbstractRenderable renderable = Fw.graphics.createInterleavedRenderable();
+            renderable.create(mesh);
+            setRenderable(renderable); // Attach to game object
+        } catch (IOException ex) {
+            throw new RuntimeException(ex);
+        }
+    }
+
+}

examples/src/net/jreactphysics3d/examples/collisiontest/VisualContactPoint.java

@@ -0,0 +1,96 @@
+/*
+ * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/
+ * Copyright (c) 2010-2013 Daniel Chappuis
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from the
+ * use of this software.
+ *
+ * Permission is granted to anyone to use this software for any purpose,
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ *
+ * 1. The origin of this software must not be misrepresented; you must not claim
+ *    that you wrote the original software. If you use this software in a
+ *    product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ *
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ *
+ * 3. This notice may not be removed or altered from any source distribution.
+ *
+ * This file has been modified during the port to Java and differ from the source versions.
+ */
+package org.atriasoft.ephysics.examples.collisiontest;
+
+import java.io.IOException;
+import net.frameworkgl.Fw;
+import net.frameworkgl.gameobjects.GameObject;
+import net.frameworkgl.math.Transform4f;
+import net.frameworkgl.math.Vector3f;
+import net.frameworkgl.opengl.GL;
+import net.frameworkgl.opengl.mesh.Mesh;
+import net.frameworkgl.opengl.renderable.AbstractRenderable;
+
+/**
+ *
+ * @author Jason Sorensen <sorensenj@smert.net>
+ */
+public class VisualContactPoint extends GameObject {
+
+    private static final float VISUAL_CONTACT_POINT_RADIUS = 0.1f;
+
+    private static boolean initialized;
+    private static Mesh staticMesh;
+    private static AbstractRenderable staticRenderable;
+
+    public VisualContactPoint(Vector3f position) {
+
+        // Scaling
+        Vector3f size = new Vector3f(VISUAL_CONTACT_POINT_RADIUS, VISUAL_CONTACT_POINT_RADIUS, VISUAL_CONTACT_POINT_RADIUS);
+        Transform4f scaling = new Transform4f();
+        scaling.getRotation().setDiagonal(size);
+        setScalingTransform(scaling); // Attach to game object
+
+        getWorldTransform().getPosition().set(position);
+        getWorldTransform().multiply(getScalingTransform());
+
+        CreateStaticData();
+        setMesh(staticMesh); // Attach mesh to game object
+        setRenderable(staticRenderable); // Attach to game object
+    }
+
+    public static void CreateStaticData() {
+        if (initialized) {
+            return;
+        }
+
+        try {
+            // Mesh
+            staticMesh = GL.meshFactory.createMesh();
+            Fw.graphics.loadMesh("primitives/cone.obj", staticMesh);
+            staticMesh.setAllColors(1.0f, 1.0f, 0.0f, 1.0f);
+            staticMesh.updateHasBooleansFromSegment();
+
+            // Renderable
+            staticRenderable = Fw.graphics.createInterleavedRenderable();
+            staticRenderable.create(staticMesh);
+        } catch (IOException ex) {
+            throw new RuntimeException(ex);
+        }
+
+        // Only do this once
+        initialized = true;
+    }
+
+    public static void Destroy() {
+        if (!initialized) {
+            return;
+        }
+        staticRenderable.destroy();
+        staticRenderable = null;
+        initialized = false;
+    }
+
+}

lutin_org-atriasoft-ephysics.py

@@ -0,0 +1,145 @@
+#!/usr/bin/python
+import realog.debug as debug
+import lutin.tools as tools
+import realog.debug as debug
+import lutin.image as image
+import os
+import lutin.multiprocess as lutinMultiprocess
+
+
+def get_type():
+	return "LIBRARY_DYNAMIC"
+
+def get_desc():
+	return "Ewol Tool Kit"
+
+def get_licence():
+	return "MPL-2"
+
+def get_compagny_type():
+	return "org"
+
+def get_compagny_name():
+	return "atria-soft"
+
+#def get_maintainer():
+#	return "authors.txt"
+
+#def get_version():
+#	return "version.txt"
+
+def configure(target, my_module):
+
+	my_module.add_src_file([
+	    'src/module-info.java',
+	    'src/org/atriasoft/ephysics/body/Body.java',
+	    'src/org/atriasoft/ephysics/body/CollisionBody.java',
+	    'src/org/atriasoft/ephysics/body/RigidBody.java',
+	    'src/org/atriasoft/ephysics/body/BodyType.java',
+	    'src/org/atriasoft/ephysics/RaycastCallback.java',
+	    'src/org/atriasoft/ephysics/mathematics/SetMultiple.java',
+	    'src/org/atriasoft/ephysics/mathematics/Ray.java',
+	    'src/org/atriasoft/ephysics/mathematics/SetInteger.java',
+	    'src/org/atriasoft/ephysics/mathematics/Matrix2f.java',
+	    'src/org/atriasoft/ephysics/mathematics/PairInt.java',
+	    'src/org/atriasoft/ephysics/mathematics/PairIntVector3f.java',
+	    'src/org/atriasoft/ephysics/mathematics/Set.java',
+	    'src/org/atriasoft/ephysics/mathematics/Mathematics.java',
+	    'src/org/atriasoft/ephysics/configuration/Defaults.java',
+	    'src/org/atriasoft/ephysics/internal/LOGGER.java',
+	    'src/org/atriasoft/ephysics/engine/DynamicsWorld.java',
+	    'src/org/atriasoft/ephysics/engine/CollisionWorld.java',
+	    'src/org/atriasoft/ephysics/engine/ContactManifoldSolver.java',
+	    'src/org/atriasoft/ephysics/engine/Material.java',
+	    'src/org/atriasoft/ephysics/engine/ConstraintSolver.java',
+	    'src/org/atriasoft/ephysics/engine/ConstraintSolverData.java',
+	    'src/org/atriasoft/ephysics/engine/Impulse.java',
+	    'src/org/atriasoft/ephysics/engine/ContactPointSolver.java',
+	    'src/org/atriasoft/ephysics/engine/OverlappingPair.java',
+	    'src/org/atriasoft/ephysics/engine/ContactSolver.java',
+	    'src/org/atriasoft/ephysics/engine/EventListener.java',
+	    'src/org/atriasoft/ephysics/engine/CollisionCallback.java',
+	    'src/org/atriasoft/ephysics/engine/Island.java',
+	    'src/org/atriasoft/ephysics/collision/TriangleVertexArray.java',
+	    'src/org/atriasoft/ephysics/collision/ContactManifoldSet.java',
+	    'src/org/atriasoft/ephysics/collision/TriangleMesh.java',
+	    'src/org/atriasoft/ephysics/collision/ProxyShape.java',
+	    'src/org/atriasoft/ephysics/collision/ContactManifold.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/ConeShape.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/ConcaveMeshShape.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/CylinderShape.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/AABB.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/Bounds.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/ConvexShape.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/ConvexMeshShape.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/SphereShape.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/CacheData.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/CapsuleShape.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/BoxShape.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/ConcaveShape.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/CollisionShape.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/HeightFieldShape.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/TriangleShape.java',
+	    'src/org/atriasoft/ephysics/collision/shapes/CollisionShapeType.java',
+	    'src/org/atriasoft/ephysics/collision/CollisionShapeInfo.java',
+	    'src/org/atriasoft/ephysics/collision/TestCollisionBetweenShapesCallback.java',
+	    'src/org/atriasoft/ephysics/collision/narrowphase/CollisionDispatch.java',
+	    'src/org/atriasoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.java',
+	    'src/org/atriasoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.java',
+	    'src/org/atriasoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java',
+	    'src/org/atriasoft/ephysics/collision/narrowphase/GJK/Simplex.java',
+	    'src/org/atriasoft/ephysics/collision/narrowphase/NarrowPhaseCallback.java',
+	    'src/org/atriasoft/ephysics/collision/narrowphase/EPA/TriangleEPA.java',
+	    'src/org/atriasoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java',
+	    'src/org/atriasoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java',
+	    'src/org/atriasoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java',
+	    'src/org/atriasoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.java',
+	    'src/org/atriasoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.java',
+	    'src/org/atriasoft/ephysics/collision/broadphase/DTree.java',
+	    'src/org/atriasoft/ephysics/collision/broadphase/DynamicAABBTree.java',
+	    'src/org/atriasoft/ephysics/collision/broadphase/DTreeLeafData.java',
+	    'src/org/atriasoft/ephysics/collision/broadphase/CallbackRaycast.java',
+	    'src/org/atriasoft/ephysics/collision/broadphase/DTreeNode.java',
+	    'src/org/atriasoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java',
+	    'src/org/atriasoft/ephysics/collision/broadphase/DTreeLeafInt.java',
+	    'src/org/atriasoft/ephysics/collision/broadphase/CallbackOverlapping.java',
+	    'src/org/atriasoft/ephysics/collision/broadphase/PairDTree.java',
+	    'src/org/atriasoft/ephysics/collision/Triangle.java',
+	    'src/org/atriasoft/ephysics/collision/ContactManifoldListElement.java',
+	    'src/org/atriasoft/ephysics/collision/CollisionDetection.java',
+	    'src/org/atriasoft/ephysics/RaycastInfo.java',
+	    'src/org/atriasoft/ephysics/RaycastTest.java',
+	    'src/org/atriasoft/ephysics/constraint/HingeJointInfo.java',
+	    'src/org/atriasoft/ephysics/constraint/BallAndSocketJoint.java',
+	    'src/org/atriasoft/ephysics/constraint/HingeJoint.java',
+	    'src/org/atriasoft/ephysics/constraint/SliderJointInfo.java',
+	    'src/org/atriasoft/ephysics/constraint/FixedJointInfo.java',
+	    'src/org/atriasoft/ephysics/constraint/JointInfo.java',
+	    'src/org/atriasoft/ephysics/constraint/JointType.java',
+	    'src/org/atriasoft/ephysics/constraint/Joint.java',
+	    'src/org/atriasoft/ephysics/constraint/ContactPoint.java',
+	    'src/org/atriasoft/ephysics/constraint/SliderJoint.java',
+	    'src/org/atriasoft/ephysics/constraint/ContactPointInfo.java',
+	    'src/org/atriasoft/ephysics/constraint/ContactsPositionCorrectionTechnique.java',
+	    'src/org/atriasoft/ephysics/constraint/JointsPositionCorrectionTechnique.java',
+	    'src/org/atriasoft/ephysics/constraint/BallAndSocketJointInfo.java',
+	    'src/org/atriasoft/ephysics/constraint/FixedJoint.java',
+	    'src/org/atriasoft/ephysics/constraint/JointListElement.java',
+	    'src/org/atriasoft/ephysics/Configuration.java',
+	    ])
+	my_module.add_path('src/', type='java')
+	
+	my_module.add_depend([
+	    'org-atriasoft-etk'
+	    ])
+	
+	#my_module.add_path([
+	#    'lib/spotbugs-annotations-4.2.2.jar'
+	#    ],
+	#    type='java',
+	#    export=True
+	#);
+	my_module.add_flag('java', "RELEASE_15_PREVIEW");
+	
+	return True
+

pom.xml

@@ -0,0 +1,122 @@
+<project xmlns="http://maven.apache.org/POM/4.0.0" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 https://maven.apache.org/xsd/maven-4.0.0.xsd">
+  <modelVersion>4.0.0</modelVersion>
+  <groupId>org.atriasoft</groupId>
+  <artifactId>ephysics</artifactId>
+  <version>0.1.0</version>
+	<properties>
+		<maven.compiler.version>3.13.0</maven.compiler.version>
+		<maven.compiler.source>21</maven.compiler.source>
+		<maven.compiler.target>21</maven.compiler.target>
+		<maven.dependency.version>3.1.1</maven.dependency.version>
+	</properties>
+
+	<repositories>
+	  <repository>
+	    <id>gitea</id>
+	    <url>https://gitea.atria-soft.org/api/packages/org.atriasoft/maven</url>
+	  </repository>
+	</repositories>
+	<distributionManagement>
+	  <repository>
+	    <id>gitea</id>
+	    <url>https://gitea.atria-soft.org/api/packages/org.atriasoft/maven</url>
+	  </repository>
+	  <snapshotRepository>
+	    <id>gitea</id>
+	    <url>https://gitea.atria-soft.org/api/packages/org.atriasoft/maven</url>
+	  </snapshotRepository>
+	</distributionManagement>
+
+	<dependencies>
+		<dependency>
+			<groupId>org.atriasoft</groupId>
+			<artifactId>etk</artifactId>
+			<version>0.1.0</version>
+		</dependency>
+		<dependency>
+		    <groupId>org.junit.jupiter</groupId>
+		    <artifactId>junit-jupiter-api</artifactId>
+		    <version>5.11.0-M2</version>
+		    <scope>test</scope>
+		</dependency>
+		<dependency>
+			<groupId>org.slf4j</groupId>
+			<artifactId>slf4j-simple</artifactId>
+			<version>2.1.0-alpha1</version>
+			<scope>test</scope>
+		</dependency>
+    </dependencies>
+
+	<build>
+		<sourceDirectory>src</sourceDirectory>
+		<testSourceDirectory>test/src</testSourceDirectory>
+		<plugins>
+			<plugin>
+				<groupId>org.apache.maven.plugins</groupId>
+				<artifactId>maven-compiler-plugin</artifactId>
+				<version>${maven.compiler.version}</version>
+				<configuration>
+					<source>${maven.compiler.source}</source>
+					<target>${maven.compiler.target}</target>
+					<!--<encoding>${project.build.sourceEncoding}</encoding>-->
+				</configuration>
+			</plugin>
+			<!-- Create the source bundle -->
+			<plugin>
+				<groupId>org.apache.maven.plugins</groupId>
+				<artifactId>maven-source-plugin</artifactId>
+				<executions>
+					<execution>
+						<id>attach-sources</id>
+						<goals>
+							<goal>jar</goal>
+						</goals>
+					</execution>
+				</executions>
+			</plugin>
+			<!-- junit results -->
+			<plugin>
+				<groupId>org.apache.maven.plugins</groupId>
+				<artifactId>maven-surefire-plugin</artifactId>
+				<version>3.0.0-M5</version>
+			</plugin>
+		    <plugin>
+				<artifactId>maven-assembly-plugin</artifactId>
+				<configuration>
+					<archive>
+						<manifest>
+							<mainClass>fully.qualified.MainClass</mainClass>
+						</manifest>
+					</archive>
+					<descriptorRefs>
+						<descriptorRef>jar-with-dependencies</descriptorRef>
+					</descriptorRefs>
+				</configuration>
+		    </plugin>
+			<!-- Java-doc generation for stand-alone site -->
+			<plugin>
+				<groupId>org.apache.maven.plugins</groupId>
+				<artifactId>maven-javadoc-plugin</artifactId>
+				<version>3.2.0</version>
+				<configuration>
+					<show>private</show>
+					<nohelp>true</nohelp>
+				</configuration>
+			</plugin>
+		</plugins>
+	</build>
+	<!-- Generate Java-docs As Part Of Project Reports -->
+	<reporting>
+		<plugins>
+			<plugin>
+				<groupId>org.apache.maven.plugins</groupId>
+				<artifactId>maven-javadoc-plugin</artifactId>
+				<version>3.2.0</version>
+				<configuration>
+					<show>public</show>
+				</configuration>
+			</plugin>
+		</plugins>
+	</reporting>
+
+</project>

src/module-info.java

@@ -17,5 +17,4 @@ open module org.atriasoft.ephysics {
 	exports org.atriasoft.ephysics;
 	
 	requires transitive org.atriasoft.etk;
-	requires javafx.base;
 }

src/org/atriasoft/ephysics/Configuration.java

@@ -3,6 +3,30 @@ package org.atriasoft.ephysics;
 import org.atriasoft.etk.math.Constant;
 
 public class Configuration {
+	/// Default bounciness factor for a rigid body
+	public static final float DEFAULT_BOUNCINESS = 0.5f;
+	/// Default friction coefficient for a rigid body
+	public static final float DEFAULT_FRICTION_COEFFICIENT = 0.3f;
+	
+	/// Number of iterations when solving the position raints of the Sequential Impulse technique
+	public static final int DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5;
+	
+	/// Default rolling resistance
+	public static final float DEFAULT_ROLLING_RESISTANCE = 0.0f;
+	
+	/// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
+	/// might enter sleeping mode
+	public static final float DEFAULT_SLEEP_ANGULAR_VELOCITY = 5.0f * (Constant.PI / 180.0f); //(old default : 0.3)
+	
+	/// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
+	/// might enter sleeping mode.
+	public static final float DEFAULT_SLEEP_LINEAR_VELOCITY = 0.3f; //(old default : 0.01)
+	
+	/// Time (in seconds) that a body must stay still to be idered sleeping
+	public static final float DEFAULT_TIME_BEFORE_SLEEP = 0.3f; //(old default : 3.0)
+	
+	/// Number of iterations when solving the velocity raints of the Sequential Impulse technique
+	public static final int DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10;
 	
 	/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
 	/// inflated with a ant gap to allow the collision shape to move a little bit
@@ -14,41 +38,17 @@ public class Configuration {
 	/// followin ant with the linear velocity and the elapsed time between two frames.
 	public static final float DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = 1.7f;
 	
-	/// Distance threshold for two contact points for a valid persistent contact (in meters)
-	public static final float PERSISTENT_CONTACT_DIST_THRESHOLD = 0.03f;
-	
-	/// Default friction coefficient for a rigid body
-	public static final float DEFAULT_FRICTION_COEFFICIENT = 0.3f;
-	
-	/// Default bounciness factor for a rigid body
-	public static final float DEFAULT_BOUNCINESS = 0.5f;
-	
-	/// Default rolling resistance
-	public static final float DEFAULT_ROLLING_RESISTANCE = 0.0f;
-	
-	/// True if the spleeping technique is enabled
-	public static final boolean SPLEEPING_ENABLED = true;
-	
 	/// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
 	public static final float OBJECT_MARGIN = 0.04f;
 	
+	/// Distance threshold for two contact points for a valid persistent contact (in meters)
+	public static final float PERSISTENT_CONTACT_DIST_THRESHOLD = 0.03f;
+	
 	/// Velocity threshold for contact velocity restitution
 	public static final float RESTITUTION_VELOCITY_THRESHOLD = 1.0f;
 	
-	/// Number of iterations when solving the velocity raints of the Sequential Impulse technique
-	public static final int DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10;
+	/// True if the spleeping technique is enabled
+	public static final boolean SPLEEPING_ENABLED = true;
 	
-	/// Number of iterations when solving the position raints of the Sequential Impulse technique
-	public static final int DEFAULT_POSITION_SOLVER_NB_ITERATIONS = 5;
-	
-	/// Time (in seconds) that a body must stay still to be idered sleeping
-	public static final float DEFAULT_TIME_BEFORE_SLEEP = 0.3f; //(old default : 3.0)
-	
-	/// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
-	/// might enter sleeping mode.
-	public static final float DEFAULT_SLEEP_LINEAR_VELOCITY = 0.3f; //(old default : 0.01)
-	
-	/// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
-	/// might enter sleeping mode
-	public static final float DEFAULT_SLEEP_ANGULAR_VELOCITY = 5.0f * (Constant.PI / 180.0f); //(old default : 0.3)
+	private Configuration() {}
 }

src/org/atriasoft/ephysics/RaycastCallback.java

@@ -2,7 +2,7 @@ package org.atriasoft.ephysics;
 
 public interface RaycastCallback {
 	/**
-	 * @brief This method will be called for each ProxyShape that is hit by the
+	 * This method will be called for each ProxyShape that is hit by the
 	 * ray. You cannot make any assumptions about the order of the
 	 * calls. You should use the return value to control the continuation
 	 * of the ray. The returned value is the next maxFraction value to use.
@@ -13,8 +13,8 @@ public interface RaycastCallback {
 	 * value in the RaycastInfo object), the current ray will be clipped
 	 * to this fraction in the next queries. If you return -1.0, it will
 	 * ignore this ProxyShape and continue the ray cast.
-	 * @param[in] _raycastInfo Information about the raycast hit
+	 * @param raycastInfo Information about the raycast hit
 	 * @return Value that controls the continuation of the ray after a hit
 	 */
-	float notifyRaycastHit(RaycastInfo _raycastInfo);
+	float notifyRaycastHit(RaycastInfo raycastInfo);
 }

src/org/atriasoft/ephysics/RaycastInfo.java

@@ -1,13 +1,12 @@
 package org.atriasoft.ephysics;
 
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.body.CollisionBody;
 import org.atriasoft.ephysics.collision.ProxyShape;
+import org.atriasoft.etk.math.Vector3f;
 
 public class RaycastInfo {
-	public Vector3f worldPoint = new Vector3f(); //!< Hit point in world-space coordinates
-	public Vector3f worldNormal = new Vector3f(); //!< Surface normal at hit point in world-space coordinates
+	public Vector3f worldPoint = Vector3f.ZERO; //!< Hit point in world-space coordinates
+	public Vector3f worldNormal = Vector3f.ZERO; //!< Surface normal at hit point in world-space coordinates
 	public float hitFraction = 0.0f; //!< Fraction distance of the hit point between point1 and point2 of the ray. The hit point "p" is such that p = point1 + hitFraction * (point2 - point1)
 	public long meshSubpart = -1; //!< Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise)
 	public long triangleIndex = -1; //!< Hit triangle index (only used for triangles mesh and -1 otherwise)

src/org/atriasoft/ephysics/RaycastTest.java

@@ -8,21 +8,21 @@ public class RaycastTest {
 	public RaycastCallback userCallback; //!< User callback class
 	/// Constructor
 	
-	public RaycastTest(final RaycastCallback _callback) {
-		this.userCallback = _callback;
+	public RaycastTest(final RaycastCallback callback) {
+		this.userCallback = callback;
 	}
 	
 	/// Ray cast test against a proxy shape
-	public float raycastAgainstShape(final ProxyShape _shape, final Ray _ray) {
+	public float raycastAgainstShape(final ProxyShape shape, final Ray ray) {
 		// Ray casting test against the collision shape
 		final RaycastInfo raycastInfo = new RaycastInfo();
-		final boolean isHit = _shape.raycast(_ray, raycastInfo);
+		final boolean isHit = shape.raycast(ray, raycastInfo);
 		// If the ray hit the collision shape
 		if (isHit) {
 			// Report the hit to the user and return the
 			// user hit fraction value
 			return this.userCallback.notifyRaycastHit(raycastInfo);
 		}
-		return _ray.maxFraction;
+		return ray.maxFraction;
 	}
 }

src/org/atriasoft/ephysics/body/Body.java

@@ -27,15 +27,10 @@ package org.atriasoft.ephysics.body;
 /**
  * This class is an abstract class to represent a body of the physics engine.
  *
- * @author Jason Sorensen <sorensenj@smert.net>
  */
 public abstract class Body {
 	// Unique ID of the body
 	protected int id; // TODO check where it came from ...
-	// True if the body has already been added in an island (for sleeping technique)
-	public boolean isAlreadyInIsland = false;
-	// True if the body is allowed to go to sleep for better efficiency
-	protected boolean isAllowedToSleep = true;
 	/**
 	 * True if the body is active.
 	 * 
@@ -45,6 +40,10 @@ public abstract class Body {
 	 * A joint connected to an inactive body will also be inactive.
 	 */
 	protected boolean isActive = true;
+	// True if the body is allowed to go to sleep for better efficiency
+	protected boolean isAllowedToSleep = true;
+	// True if the body has already been added in an island (for sleeping technique)
+	public boolean isAlreadyInIsland = false;
 	// True if the body is sleeping (for sleeping technique)
 	protected boolean isSleeping = false;
 	// Elapsed time since the body velocity was bellow the sleep velocity
@@ -53,8 +52,8 @@ public abstract class Body {
 	protected Object userData = null;
 	
 	/**
-	 * @brief Constructor
-	 * @param[in] _id ID of the new body
+	 * Constructor
+	 * @param _id ID of the new body
 	 */
 	public Body(final int bodyID) {
 		this.id = bodyID;
@@ -70,7 +69,7 @@ public abstract class Body {
 	}
 	
 	/**
-	 * @brief Return a pointer to the user data attached to this body
+	 * Return a pointer to the user data attached to this body
 	 * @return A pointer to the user data you have attached to the body
 	 */
 	Object getUserData() {
@@ -78,7 +77,7 @@ public abstract class Body {
 	}
 	
 	/**
-	 * @brief Return the id of the body
+	 * Return the id of the body
 	 * @return The ID of the body
 	 */
 	public boolean isActive() {
@@ -86,8 +85,8 @@ public abstract class Body {
 	}
 	
 	/**
-	 * @brief Set whether or not the body is allowed to go to sleep
-	 * @param[in] _isAllowedToSleep True if the body is allowed to sleep
+	 * Set whether or not the body is allowed to go to sleep
+	 * @param _isAllowedToSleep True if the body is allowed to sleep
 	 */
 	public boolean isAllowedToSleep() {
 		return this.isAllowedToSleep;
@@ -98,7 +97,7 @@ public abstract class Body {
 	}
 	
 	/**
-	 * @brief Return whether or not the body is sleeping
+	 * Return whether or not the body is sleeping
 	 * @return True if the body is currently sleeping and false otherwise
 	 */
 	public boolean isSleeping() {
@@ -106,8 +105,8 @@ public abstract class Body {
 	}
 	
 	/**
-	 * @brief Set whether or not the body is active
-	 * @param[in] _isActive True if you want to activate the body
+	 * Set whether or not the body is active
+	 * @param _isActive True if you want to activate the body
 	 */
 	void setIsActive(final boolean isActive) {
 		this.isActive = isActive;
@@ -127,8 +126,8 @@ public abstract class Body {
 	}
 	
 	/**
-	 * @brief Set the variable to know whether or not the body is sleeping
-	 * @param[in] _isSleeping Set the new status
+	 * Set the variable to know whether or not the body is sleeping
+	 * @param _isSleeping Set the new status
 	 */
 	public void setIsSleeping(final boolean isSleeping) {
 		if (isSleeping) {
@@ -144,8 +143,8 @@ public abstract class Body {
 	}
 	
 	/**
-	 * @brief Attach user data to this body
-	 * @param[in] _userData A pointer to the user data you want to attach to the body
+	 * Attach user data to this body
+	 * @param _userData A pointer to the user data you want to attach to the body
 	 */
 	public void setUserData(final Object userData) {
 		this.userData = userData;

src/org/atriasoft/ephysics/body/CollisionBody.java

@@ -30,67 +30,70 @@ import org.atriasoft.ephysics.collision.ProxyShape;
 import org.atriasoft.ephysics.collision.shapes.AABB;
 import org.atriasoft.ephysics.collision.shapes.CollisionShape;
 import org.atriasoft.ephysics.engine.CollisionWorld;
-import org.atriasoft.ephysics.internal.Log;
 import org.atriasoft.ephysics.mathematics.Ray;
 import org.atriasoft.etk.math.Transform3D;
 import org.atriasoft.etk.math.Vector3f;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 /**
  * This class represents a body that is able to collide with others bodies. This class inherits from the Body class.
  *
- * @author Jason Sorensen <sorensenj@smert.net>
  */
 public class CollisionBody extends Body {
-	// Type of body (static, kinematic or dynamic)
-	protected BodyType type;
-	// Position and orientation of the body
-	protected Transform3D transform;
-	// First element of the linked list of proxy collision shapes of this body
-	protected ProxyShape proxyCollisionShapes;
-	// Number of collision shapes
-	protected long numberCollisionShapes;
+	static final Logger LOGGER = LoggerFactory.getLogger(CollisionBody.class);
 	// First element of the linked list of contact manifolds involving this body
 	public ContactManifoldListElement contactManifoldsList;
+	// Number of collision shapes
+	protected long numberCollisionShapes;
+	// First element of the linked list of proxy collision shapes of this body
+	protected ProxyShape proxyCollisionShapes;
+	// Position and orientation of the body
+	protected Transform3D transform;
+	// Type of body (static, kinematic or dynamic)
+	protected BodyType type;
 	// Reference to the world the body belongs to
 	protected CollisionWorld world;
-	
+
 	/**
-	 * @brief Constructor
-	 * @param[in] _transform The transform of the body
-	 * @param[in] _world The physics world where the body is created
-	 * @param[in] _id ID of the body
+	 * Constructor
+	 * @param transform The transform of the body
+	 * @param world The physics world where the body is created
+	 * @param id ID of the body
 	 */
-	public CollisionBody(final Transform3D _transform, final CollisionWorld _world, final int _id) {
-		super(_id);
+	public CollisionBody(final Transform3D transform, final CollisionWorld world, final int id) {
+		super(id);
 		this.type = BodyType.DYNAMIC;
-		this.transform = _transform;
+		this.transform = transform;
 		this.proxyCollisionShapes = null;
 		this.numberCollisionShapes = 0;
 		this.contactManifoldsList = null;
-		this.world = _world;
-		
-		//Log.debug("         set transform: " + _transform);
-		if (Float.isNaN(_transform.getPosition().x)) {
-			Log.critical("         set transform: " + _transform);
+		this.world = world;
+
+		//LOGGER.debug("         set transform: " + transform);
+		if (Float.isNaN(transform.getPosition().x())) {
+			LOGGER.error("[CRITICAL]         set transform: " + transform);
+			System.exit(-1);
 		}
-		if (Float.isInfinite(_transform.getOrientation().z)) {
-			Log.critical("         set transform: " + _transform);
+		if (Float.isInfinite(transform.getOrientation().z())) {
+			LOGGER.error("[CRITICAL]         set transform: " + transform);
+			System.exit(-1);
 		}
 	}
-	
+
 	/**
-	 * @brief Add a collision shape to the body. Note that you can share a collision shape between several bodies using the same collision shape instance to
+	 * Add a collision shape to the body. Note that you can share a collision shape between several bodies using the same collision shape instance to
 	 * when you add the shape to the different bodies. Do not forget to delete the collision shape you have created at the end of your program.
-	 * 
+	 *
 	 * This method will return a pointer to a new proxy shape. A proxy shape is an object that links a collision shape and a given body. You can use the
 	 * returned proxy shape to get and set information about the corresponding collision shape for that body.
-	 * @param[in] collisionShape A pointer to the collision shape you want to add to the body
-	 * @param[in] transform The transformation of the collision shape that transforms the local-space of the collision shape int32_to the local-space of the body
+	 * @param collisionShape A pointer to the collision shape you want to add to the body
+	 * @param transform The transformation of the collision shape that transforms the local-space of the collision shape int32to the local-space of the body
 	 * @return A pointer to the proxy shape that has been created to link the body to the new collision shape you have added.
 	 */
-	public ProxyShape addCollisionShape(final CollisionShape _collisionShape, final Transform3D _transform) {
+	public ProxyShape addCollisionShape(final CollisionShape collisionShape, final Transform3D transform) {
 		// Create a proxy collision shape to attach the collision shape to the body
-		final ProxyShape proxyShape = new ProxyShape(this, _collisionShape, _transform, 1.0f);
+		final ProxyShape proxyShape = new ProxyShape(this, collisionShape, transform, 1.0f);
 		// Add it to the list of proxy collision shapes of the body
 		if (this.proxyCollisionShapes == null) {
 			this.proxyCollisionShapes = proxyShape;
@@ -99,23 +102,23 @@ public class CollisionBody extends Body {
 			this.proxyCollisionShapes = proxyShape;
 		}
 		final AABB aabb = new AABB();
-		_collisionShape.computeAABB(aabb, this.transform.multiplyNew(_transform));
+		collisionShape.computeAABB(aabb, this.transform.multiply(transform));
 		this.world.collisionDetection.addProxyCollisionShape(proxyShape, aabb);
 		this.numberCollisionShapes++;
 		return proxyShape;
 	}
-	
+
 	/**
-	 * @brief Ask the broad-phase to test again the collision shapes of the body for collision (as if the body has moved).
+	 * Ask the broad-phase to test again the collision shapes of the body for collision (as if the body has moved).
 	 */
 	protected void askForBroadPhaseCollisionCheck() {
 		for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.getNext()) {
 			this.world.getCollisionDetection().askForBroadPhaseCollisionCheck(shape);
 		}
 	}
-	
+
 	/**
-	 * @brief Compute and return the AABB of the body by merging all proxy shapes AABBs
+	 * Compute and return the AABB of the body by merging all proxy shapes AABBs
 	 * @return The axis-aligned bounding box (AABB) of the body in world-space coordinates
 	 */
 	public AABB getAABB() {
@@ -123,119 +126,120 @@ public class CollisionBody extends Body {
 		if (this.proxyCollisionShapes == null) {
 			return bodyAABB;
 		}
-		//Log.info("tmp this.transform : " + this.transform);
-		//Log.info("tmp this.proxyCollisionShapes.getLocalToBodyTransform() : " + this.proxyCollisionShapes.getLocalToBodyTransform());
-		//Log.info("tmp this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()) : " + this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()));
-		this.proxyCollisionShapes.getCollisionShape().computeAABB(bodyAABB, this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()));
+		//LOGGER.info("tmp this.transform : " + this.transform);
+		//LOGGER.info("tmp this.proxyCollisionShapes.getLocalToBodyTransform() : " + this.proxyCollisionShapes.getLocalToBodyTransform());
+		//LOGGER.info("tmp this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()) : " + this.transform.multiplyNew(this.proxyCollisionShapes.getLocalToBodyTransform()));
+		this.proxyCollisionShapes.getCollisionShape().computeAABB(bodyAABB,
+				this.transform.multiply(this.proxyCollisionShapes.getLocalToBodyTransform()));
 		for (ProxyShape shape = this.proxyCollisionShapes.next; shape != null; shape = shape.next) {
 			final AABB aabb = new AABB();
-			shape.getCollisionShape().computeAABB(aabb, this.transform.multiplyNew(shape.getLocalToBodyTransform()));
+			shape.getCollisionShape().computeAABB(aabb, this.transform.multiply(shape.getLocalToBodyTransform()));
 			bodyAABB.mergeWithAABB(aabb);
 		}
-		//Log.info("tmp aabb : " + bodyAABB);
-		//Log.info("tmp aabb : " + bodyAABB.getMax().lessNew(bodyAABB.getMin()));
+		//LOGGER.info("tmp aabb : " + bodyAABB);
+		//LOGGER.info("tmp aabb : " + bodyAABB.getMax().lessNew(bodyAABB.getMin()));
 		return bodyAABB;
 	}
-	
+
 	/**
-	 * @brief Get the first element of the linked list of contact manifolds involving this body
+	 * Get the first element of the linked list of contact manifolds involving this body
 	 * @return A pointer to the first element of the linked-list with the contact manifolds of this body
 	 */
 	public ContactManifoldListElement getContactManifoldsList() {
 		return this.contactManifoldsList;
 	}
-	
+
 	/**
-	 * @brief Get the body local-space coordinates of a point given in the world-space coordinates
-	 * @param[in] _worldPoint A point in world-space coordinates
+	 * Get the body local-space coordinates of a point given in the world-space coordinates
+	 * @param worldPoint A point in world-space coordinates
 	 * @return The point in the local-space coordinates of the body
 	 */
-	public Vector3f getLocalPoint(final Vector3f _worldPoint) {
-		return this.transform.inverseNew().multiply(_worldPoint);
+	public Vector3f getLocalPoint(final Vector3f worldPoint) {
+		return this.transform.inverseNew().multiply(worldPoint);
 	}
-	
+
 	/**
-	 * @brief Get the body local-space coordinates of a vector given in the world-space coordinates
-	 * @param[in] _worldVector A vector in world-space coordinates
+	 * Get the body local-space coordinates of a vector given in the world-space coordinates
+	 * @param worldVector A vector in world-space coordinates
 	 * @return The vector in the local-space coordinates of the body
 	 */
-	public Vector3f getLocalVector(final Vector3f _worldVector) {
-		return this.transform.getOrientation().inverseNew().multiply(_worldVector);
+	public Vector3f getLocalVector(final Vector3f worldVector) {
+		return this.transform.getOrientation().inverse().multiply(worldVector);
 	}
-	
+
 	/**
-	 * @brief Get the linked list of proxy shapes of that body
+	 * Get the linked list of proxy shapes of that body
 	 * @return The pointer of the first proxy shape of the linked-list of all the
 	 *		 proxy shapes of the body
 	 */
 	public ProxyShape getProxyShapesList() {
 		return this.proxyCollisionShapes;
 	}
-	
+
 	/**
-	 * @brief Return the current position and orientation
-	 * @return The current transformation of the body that transforms the local-space of the body int32_to world-space
+	 * Return the current position and orientation
+	 * @return The current transformation of the body that transforms the local-space of the body int32to world-space
 	 */
 	public Transform3D getTransform() {
 		return this.transform;
 	}
-	
+
 	/**
-	 * @brief Return the type of the body
+	 * Return the type of the body
 	 * @return the type of the body (STATIC, KINEMATIC, DYNAMIC)
 	 */
 	public BodyType getType() {
 		return this.type;
 	}
-	
+
 	public CollisionWorld getWorld() {
 		return this.world;
 	}
-	
+
 	/**
-	 * @brief Get the world-space coordinates of a point given the local-space coordinates of the body
-	 * @param[in] _localPoint A point in the local-space coordinates of the body
+	 * Get the world-space coordinates of a point given the local-space coordinates of the body
+	 * @param localPoint A point in the local-space coordinates of the body
 	 * @return The point in world-space coordinates
 	 */
-	public Vector3f getWorldPoint(final Vector3f _localPoint) {
-		return this.transform.multiplyNew(_localPoint);
+	public Vector3f getWorldPoint(final Vector3f localPoint) {
+		return this.transform.multiply(localPoint);
 	}
-	
+
 	/**
-	 * @brief Get the world-space vector of a vector given in local-space coordinates of the body
-	 * @param[in] _localVector A vector in the local-space coordinates of the body
+	 * Get the world-space vector of a vector given in local-space coordinates of the body
+	 * @param localVector A vector in the local-space coordinates of the body
 	 * @return The vector in world-space coordinates
 	 */
-	public Vector3f getWorldVector(final Vector3f _localVector) {
-		return this.transform.getOrientation().multiply(_localVector);
+	public Vector3f getWorldVector(final Vector3f localVector) {
+		return this.transform.getOrientation().multiply(localVector);
 	}
-	
+
 	/**
-	 * @brief Raycast method with feedback information
+	 * Raycast method with feedback information
 	 * The method returns the closest hit among all the collision shapes of the body
-	 * @param[in] _ray The ray used to raycast agains the body
-	 * @param[out] _raycastInfo Structure that contains the result of the raycasting (valid only if the method returned true)
+	 * @param ray The ray used to raycast agains the body
+	 * @param raycastInfo Structure that contains the result of the raycasting (valid only if the method returned true)
 	 * @return True if the ray hit the body and false otherwise
 	 */
-	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo) {
-		if (this.isActive == false) {
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo) {
+		if (!this.isActive) {
 			return false;
 		}
 		boolean isHit = false;
-		
-		final Ray rayTemp = new Ray(_ray);
+
+		final Ray rayTemp = new Ray(ray);
 		for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
 			// Test if the ray hits the collision shape
-			if (shape.raycast(rayTemp, _raycastInfo)) {
-				rayTemp.maxFraction = _raycastInfo.hitFraction;
+			if (shape.raycast(rayTemp, raycastInfo)) {
+				rayTemp.maxFraction = raycastInfo.hitFraction;
 				isHit = true;
 			}
 		}
 		return isHit;
 	}
-	
+
 	/**
-	 * @brief Remove all the collision shapes
+	 * Remove all the collision shapes
 	 */
 	public void removeAllCollisionShapes() {
 		ProxyShape current = this.proxyCollisionShapes;
@@ -251,16 +255,16 @@ public class CollisionBody extends Body {
 		}
 		this.proxyCollisionShapes = null;
 	}
-	
+
 	/**
-	 * @brief Remove a collision shape from the body
+	 * Remove a collision shape from the body
 	 * To remove a collision shape, you need to specify the pointer to the proxy shape that has been returned when you have added the collision shape to the body
-	 * @param[in] _proxyShape The pointer of the proxy shape you want to remove
+	 * @param proxyShape The pointer of the proxy shape you want to remove
 	 */
-	public void removeCollisionShape(final ProxyShape _proxyShape) {
+	public void removeCollisionShape(final ProxyShape proxyShape) {
 		ProxyShape current = this.proxyCollisionShapes;
 		// If the the first proxy shape is the one to remove
-		if (current == _proxyShape) {
+		if (current == proxyShape) {
 			this.proxyCollisionShapes = current.next;
 			if (this.isActive) {
 				this.world.collisionDetection.removeProxyCollisionShape(current);
@@ -272,7 +276,7 @@ public class CollisionBody extends Body {
 		// Look for the proxy shape that contains the collision shape in parameter
 		while (current.next != null) {
 			// If we have found the collision shape to remove
-			if (current.next == _proxyShape) {
+			if (current.next == proxyShape) {
 				// Remove the proxy collision shape
 				ProxyShape elementToRemove = current.next;
 				current.next = elementToRemove.next;
@@ -287,17 +291,17 @@ public class CollisionBody extends Body {
 			current = current.next;
 		}
 	}
-	
+
 	/**
-	 * @brief Reset the contact manifold lists
+	 * Reset the contact manifold lists
 	 */
 	public void resetContactManifoldsList() {
 		// Delete the linked list of contact manifolds of that body
 		this.contactManifoldsList = null;
 	}
-	
+
 	/**
-	 * @brief Reset the this.isAlreadyInIsland variable of the body and contact manifolds.
+	 * Reset the this.isAlreadyInIsland variable of the body and contact manifolds.
 	 * This method also returns the number of contact manifolds of the body.
 	 */
 	public int resetIsAlreadyInIslandAndCountManifolds() {
@@ -312,23 +316,23 @@ public class CollisionBody extends Body {
 		}
 		return nbManifolds;
 	}
-	
+
 	/**
-	 * @brief Set whether or not the body is active
-	 * @param[in] _isActive True if you want to activate the body
+	 * Set whether or not the body is active
+	 * @param isActive True if you want to activate the body
 	 */
 	@Override
-	public void setIsActive(final boolean _isActive) {
+	public void setIsActive(final boolean isActive) {
 		// If the state does not change
-		if (this.isActive == _isActive) {
+		if (this.isActive == isActive) {
 			return;
 		}
-		super.setIsActive(_isActive);
+		super.setIsActive(isActive);
 		// If we have to activate the body
-		if (_isActive == true) {
+		if (isActive) {
 			for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
 				final AABB aabb = new AABB();
-				shape.getCollisionShape().computeAABB(aabb, this.transform.multiplyNew(shape.localToBodyTransform));
+				shape.getCollisionShape().computeAABB(aabb, this.transform.multiply(shape.localToBodyTransform));
 				this.world.collisionDetection.addProxyCollisionShape(shape, aabb);
 			}
 		} else {
@@ -338,52 +342,54 @@ public class CollisionBody extends Body {
 			resetContactManifoldsList();
 		}
 	}
-	
+
 	/**
-	 * @brief Set the current position and orientation
-	 * @param transform The transformation of the body that transforms the local-space of the body int32_to world-space
+	 * Set the current position and orientation
+	 * @param transform The transformation of the body that transforms the local-space of the body int32to world-space
 	 */
-	public void setTransform(final Transform3D _transform) {
-		//Log.info("         set transform: " + this.transform + " ==> " + _transform);
-		if (Float.isNaN(_transform.getPosition().x)) {
-			Log.critical("         set transform: " + this.transform + " ==> " + _transform);
+	public void setTransform(final Transform3D transform) {
+		//LOGGER.info("         set transform: " + this.transform + " ==> " + transform);
+		if (Float.isNaN(transform.getPosition().x())) {
+			LOGGER.error("[CRITICAL]         set transform: " + this.transform + " ==> " + transform);
+			System.exit(-1);
 		}
-		if (Float.isInfinite(_transform.getOrientation().z)) {
-			Log.critical("         set transform: " + this.transform + " ==> " + _transform);
+		if (Float.isInfinite(transform.getOrientation().z())) {
+			LOGGER.error("[CRITICAL]         set transform: " + this.transform + " ==> " + transform);
+			System.exit(-1);
 		}
-		this.transform = _transform;
+		this.transform = transform;
 		updateBroadPhaseState();
 	}
-	
+
 	/**
-	 * @brief Set the type of the body
-	 * @param[in] type The type of the body (STATIC, KINEMATIC, DYNAMIC)
+	 * Set the type of the body
+	 * @param type The type of the body (STATIC, KINEMATIC, DYNAMIC)
 	 */
-	public void setType(final BodyType _type) {
-		this.type = _type;
+	public void setType(final BodyType type) {
+		this.type = type;
 		if (this.type == BodyType.STATIC) {
 			// Update the broad-phase state of the body
 			updateBroadPhaseState();
 		}
 	}
-	
+
 	/**
-	 * @brief Return true if a point is inside the collision body
+	 * Return true if a point is inside the collision body
 	 * This method returns true if a point is inside any collision shape of the body
-	 * @param[in] _worldPoint The point to test (in world-space coordinates)
+	 * @param worldPoint The point to test (in world-space coordinates)
 	 * @return True if the point is inside the body
 	 */
-	public boolean testPointInside(final Vector3f _worldPoint) {
+	public boolean testPointInside(final Vector3f worldPoint) {
 		for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
-			if (shape.testPointInside(_worldPoint)) {
+			if (shape.testPointInside(worldPoint)) {
 				return true;
 			}
 		}
 		return false;
 	}
-	
+
 	/**
-	 * @brief Update the broad-phase state for this body (because it has moved for instance)
+	 * Update the broad-phase state for this body (because it has moved for instance)
 	 */
 	protected void updateBroadPhaseState() {
 		// For all the proxy collision shapes of the body
@@ -392,13 +398,14 @@ public class CollisionBody extends Body {
 			updateProxyShapeInBroadPhase(shape, false);
 		}
 	}
-	
+
 	/**
-	 * @brief Update the broad-phase state of a proxy collision shape of the body
+	 * Update the broad-phase state of a proxy collision shape of the body
 	 */
-	public void updateProxyShapeInBroadPhase(final ProxyShape _proxyShape, final boolean _forceReinsert /* false */) {
+	public void updateProxyShapeInBroadPhase(final ProxyShape proxyShape, final boolean forceReinsert /* false */) {
 		final AABB aabb = new AABB();
-		_proxyShape.getCollisionShape().computeAABB(aabb, this.transform.multiplyNew(_proxyShape.getLocalToBodyTransform()));
-		this.world.getCollisionDetection().updateProxyCollisionShape(_proxyShape, aabb, new Vector3f(0, 0, 0), _forceReinsert);
+		proxyShape.getCollisionShape().computeAABB(aabb, this.transform.multiply(proxyShape.getLocalToBodyTransform()));
+		this.world.getCollisionDetection().updateProxyCollisionShape(proxyShape, aabb, new Vector3f(0, 0, 0),
+				forceReinsert);
 	}
 }

src/org/atriasoft/ephysics/body/RigidBody.java

@@ -32,10 +32,11 @@ import org.atriasoft.ephysics.constraint.JointListElement;
 import org.atriasoft.ephysics.engine.CollisionWorld;
 import org.atriasoft.ephysics.engine.DynamicsWorld;
 import org.atriasoft.ephysics.engine.Material;
-import org.atriasoft.ephysics.internal.Log;
 import org.atriasoft.etk.math.Matrix3f;
 import org.atriasoft.etk.math.Transform3D;
 import org.atriasoft.etk.math.Vector3f;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 /**
  *  This class represents a rigid body of the physics
@@ -44,24 +45,25 @@ import org.atriasoft.etk.math.Vector3f;
  * CollisionBody class.
  */
 public class RigidBody extends CollisionBody {
-	
-	protected float initMass = 1.0f; //!< Intial mass of the body
-	protected Vector3f centerOfMassLocal = new Vector3f(0, 0, 0); //!< Center of mass of the body in local-space coordinates. The center of mass can therefore be different from the body origin
-	public Vector3f centerOfMassWorld = new Vector3f(); //!< Center of mass of the body in world-space coordinates
-	public Vector3f linearVelocity = new Vector3f(); //!< Linear velocity of the body
-	public Vector3f angularVelocity = new Vector3f(); //!< Angular velocity of the body
-	public Vector3f externalForce = new Vector3f(); //!< Current external force on the body
-	public Vector3f externalTorque = new Vector3f(); //!< Current external torque on the body
-	public Matrix3f inertiaTensorLocal = new Matrix3f(); //!< Local inertia tensor of the body (in local-space) with respect to the center of mass of the body
-	public Matrix3f inertiaTensorLocalInverse = new Matrix3f(); //!< Inverse of the inertia tensor of the body
-	public float massInverse; //!< Inverse of the mass of the body
-	protected boolean isGravityEnabled = true; //!< True if the gravity needs to be applied to this rigid body
-	protected Material material = new Material(); //!< Material properties of the rigid body
-	protected float linearDamping = 0.0f; //!< Linear velocity damping factor
+	static final Logger LOGGER = LoggerFactory.getLogger(RigidBody.class);
+
 	protected float angularDamping = 0.0f; //!< Angular velocity damping factor
-	public JointListElement jointsList = null; //!< First element of the linked list of joints involving this body
 	protected boolean angularReactionEnable = true;
-	
+	public Vector3f angularVelocity = Vector3f.ZERO; //!< Angular velocity of the body
+	protected Vector3f centerOfMassLocal = new Vector3f(0, 0, 0); //!< Center of mass of the body in local-space coordinates. The center of mass can therefore be different from the body origin
+	public Vector3f centerOfMassWorld = Vector3f.ZERO; //!< Center of mass of the body in world-space coordinates
+	public Vector3f externalForce = Vector3f.ZERO; //!< Current external force on the body
+	public Vector3f externalTorque = Vector3f.ZERO; //!< Current external torque on the body
+	public Matrix3f inertiaTensorLocal = Matrix3f.IDENTITY; //!< Local inertia tensor of the body (in local-space) with respect to the center of mass of the body
+	public Matrix3f inertiaTensorLocalInverse = Matrix3f.IDENTITY; //!< Inverse of the inertia tensor of the body
+	protected float initMass = 1.0f; //!< Intial mass of the body
+	protected boolean isGravityEnabled = true; //!< True if the gravity needs to be applied to this rigid body
+	public JointListElement jointsList = null; //!< First element of the linked list of joints involving this body
+	protected float linearDamping = 0.0f; //!< Linear velocity damping factor
+	public Vector3f linearVelocity = Vector3f.ZERO; //!< Linear velocity of the body
+	public float massInverse; //!< Inverse of the mass of the body
+	protected Material material = new Material(); //!< Material properties of the rigid body
+
 	/**
 	 *  Constructor
 	 * @param transform The transformation of the body
@@ -70,25 +72,28 @@ public class RigidBody extends CollisionBody {
 	 */
 	public RigidBody(final Transform3D transform, final CollisionWorld world, final int id) {
 		super(transform, world, id);
-		this.centerOfMassWorld = new Vector3f(transform.getPosition());
+		this.centerOfMassWorld = transform.getPosition();
 		this.massInverse = 1.0f / this.initMass;
 	}
-	
+
 	/**
 	 *  Add a collision shape to the body.
 	 * When you add a collision shape to the body, an intternal copy of this collision shape will be created internally.
 	 * Therefore, you can delete it right after calling this method or use it later to add it to another body.
 	 * This method will return a pointer to a new proxy shape. A proxy shape is an object that links a collision shape and a given body.
 	 * You can use the returned proxy shape to get and set information about the corresponding collision shape for that body.
-	 * @param _collisionShape The collision shape you want to add to the body
-	 * @param _transform The transformation of the collision shape that transforms the local-space of the collision shape into the local-space of the body
-	 * @param _mass Mass (in kilograms) of the collision shape you want to add
+	 * @param collisionShape The collision shape you want to add to the body
+	 * @param transform The transformation of the collision shape that transforms the local-space of the collision shape into the local-space of the body
+	 * @param mass Mass (in kilograms) of the collision shape you want to add
 	 * @return A pointer to the proxy shape that has been created to link the body to the new collision shape you have added.
 	 */
-	public ProxyShape addCollisionShape(final CollisionShape _collisionShape, final Transform3D _transform, final float _mass) {
-		assert (_mass > 0.0f);
+	public ProxyShape addCollisionShape(
+			final CollisionShape collisionShape,
+			final Transform3D transform,
+			final float mass) {
+		assert (mass > 0.0f);
 		// Create a new proxy collision shape to attach the collision shape to the body
-		final ProxyShape proxyShape = new ProxyShape(this, _collisionShape, _transform, _mass);
+		final ProxyShape proxyShape = new ProxyShape(this, collisionShape, transform, mass);
 		// Add it to the list of proxy collision shapes of the body
 		if (this.proxyCollisionShapes == null) {
 			this.proxyCollisionShapes = proxyShape;
@@ -98,14 +103,14 @@ public class RigidBody extends CollisionBody {
 		}
 		// Compute the world-space AABB of the new collision shape
 		final AABB aabb = new AABB();
-		_collisionShape.computeAABB(aabb, this.transform.multiplyNew(_transform));
+		collisionShape.computeAABB(aabb, this.transform.multiply(transform));
 		// Notify the collision detection about this new collision shape
 		this.world.collisionDetection.addProxyCollisionShape(proxyShape, aabb);
 		this.numberCollisionShapes++;
 		recomputeMassInformation();
 		return proxyShape;
 	}
-	
+
 	/**
 	 *  Apply an external force to the body at a given point (in world-space coordinates).
 	 * If the point is not at the center of mass of the body, it will also
@@ -114,64 +119,64 @@ public class RigidBody extends CollisionBody {
 	 * force will we added to the sum of the applied forces and that this sum will be
 	 * reset to zero at the end of each call of the DynamicsWorld::update() method.
 	 * You can only apply a force to a dynamic body otherwise, this method will do nothing.
-	 * @param _force The force to apply on the body
-	 * @param _point The point where the force is applied (in world-space coordinates)
+	 * @param force The force to apply on the body
+	 * @param point The point where the force is applied (in world-space coordinates)
 	 */
-	public void applyForce(final Vector3f _force, final Vector3f _point) {
+	public void applyForce(final Vector3f force, final Vector3f point) {
 		if (this.type != BodyType.DYNAMIC) {
 			return;
 		}
 		if (this.isSleeping) {
 			setIsSleeping(false);
 		}
-		this.externalForce.add(_force);
-		this.externalTorque.add(_point.lessNew(this.centerOfMassWorld).cross(_force));
+		this.externalForce = this.externalForce.add(force);
+		this.externalTorque = this.externalTorque.add(point.less(this.centerOfMassWorld).cross(force));
 	}
-	
+
 	/**
 	 *  Apply an external force to the body at its center of mass.
 	 * If the body is sleeping, calling this method will wake it up. Note that the
 	 * force will we added to the sum of the applied forces and that this sum will be
 	 * reset to zero at the end of each call of the DynamicsWorld::update() method.
 	 * You can only apply a force to a dynamic body otherwise, this method will do nothing.
-	 * @param _force The external force to apply on the center of mass of the body
+	 * @param force The external force to apply on the center of mass of the body
 	 */
-	public void applyForceToCenterOfMass(final Vector3f _force) {
+	public void applyForceToCenterOfMass(final Vector3f force) {
 		if (this.type != BodyType.DYNAMIC) {
 			return;
 		}
 		if (this.isSleeping) {
 			setIsSleeping(false);
 		}
-		this.externalForce.add(_force);
+		this.externalForce = this.externalForce.add(force);
 	}
-	
+
 	/**
 	 *  Apply an external torque to the body.
 	 * If the body is sleeping, calling this method will wake it up. Note that the
 	 * force will we added to the sum of the applied torques and that this sum will be
 	 * reset to zero at the end of each call of the DynamicsWorld::update() method.
 	 * You can only apply a force to a dynamic body otherwise, this method will do nothing.
-	 * @param _torque The external torque to apply on the body
+	 * @param torque The external torque to apply on the body
 	 */
-	public void applyTorque(final Vector3f _torque) {
+	public void applyTorque(final Vector3f torque) {
 		if (this.type != BodyType.DYNAMIC) {
 			return;
 		}
 		if (this.isSleeping) {
 			setIsSleeping(false);
 		}
-		this.externalTorque.add(_torque);
+		this.externalTorque = this.externalTorque.add(torque);
 	}
-	
+
 	/**
 	 *  Set the variable to know if the gravity is applied to this rigid body
-	 * @param _isEnabled True if you want the gravity to be applied to this body
+	 * @param isEnabled True if you want the gravity to be applied to this body
 	 */
-	public void enableGravity(final boolean _isEnabled) {
-		this.isGravityEnabled = _isEnabled;
+	public void enableGravity(final boolean isEnabled) {
+		this.isGravityEnabled = isEnabled;
 	}
-	
+
 	/**
 	 *  Get the angular velocity damping factor
 	 * @return The angular damping factor of this body
@@ -179,7 +184,7 @@ public class RigidBody extends CollisionBody {
 	public float getAngularDamping() {
 		return this.angularDamping;
 	}
-	
+
 	/**
 		 *  Get the angular velocity of the body
 		 * @return The angular velocity vector of the body
@@ -187,12 +192,12 @@ public class RigidBody extends CollisionBody {
 	public Vector3f getAngularVelocity() {
 		return this.angularVelocity;
 	}
-	
+
 	/**
 	 *  Get the inverse of the inertia tensor in world coordinates.
-	 * The inertia tensor I_w in world coordinates is computed with the
-	 * local inverse inertia tensor I_b^-1 in body coordinates
-	 * by I_w = R * I_b^-1 * R^T
+	 * The inertia tensor Iw in world coordinates is computed with the
+	 * local inverse inertia tensor Ib^-1 in body coordinates
+	 * by Iw = R * Ib^-1 * R^T
 	 * where R is the rotation matrix (and R^T its transpose) of the
 	 * current orientation quaternion of the body
 	 * @return The 3x3 inverse inertia tensor matrix of the body in world-space coordinates
@@ -201,15 +206,16 @@ public class RigidBody extends CollisionBody {
 		// TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE
 		//		INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES
 		// Compute and return the inertia tensor in world coordinates
-		//Log.error("}}}    this.transform=" + this.transform);
-		//Log.error("}}}    this.inertiaTensorLocalInverse=" + this.inertiaTensorLocalInverse);
-		//Log.error("}}}    this.transform.getOrientation().getMatrix().transposeNew()=" + this.transform.getOrientation().getMatrix().transposeNew());
-		//Log.error("}}}    this.transform.getOrientation().getMatrix()=" + this.transform.getOrientation().getMatrix());
-		final Matrix3f tmp = this.transform.getOrientation().getMatrix().multiplyNew(this.inertiaTensorLocalInverse).multiply(this.transform.getOrientation().getMatrix().transposeNew());
-		//Log.error("}}}    tmp=" + tmp);
+		//LOGGER.error("}}}    this.transform=" + this.transform);
+		//LOGGER.error("}}}    this.inertiaTensorLocalInverse=" + this.inertiaTensorLocalInverse);
+		//LOGGER.error("}}}    this.transform.getOrientation().getMatrix().transposeNew()=" + this.transform.getOrientation().getMatrix().transposeNew());
+		//LOGGER.error("}}}    this.transform.getOrientation().getMatrix()=" + this.transform.getOrientation().getMatrix());
+		final Matrix3f tmp = this.transform.getOrientation().getMatrix().multiply(this.inertiaTensorLocalInverse)
+				.multiply(this.transform.getOrientation().getMatrix().transpose());
+		//LOGGER.error("}}}    tmp=" + tmp);
 		return tmp;
 	}
-	
+
 	/**
 	 *  Get the local inertia tensor of the body (in local-space coordinates)
 	 * @return The 3x3 inertia tensor matrix of the body
@@ -217,21 +223,22 @@ public class RigidBody extends CollisionBody {
 	public Matrix3f getInertiaTensorLocal() {
 		return this.inertiaTensorLocal;
 	}
-	
+
 	/**
 	 *  Get the inertia tensor in world coordinates.
-	 * The inertia tensor I_w in world coordinates is computed
-	 * with the local inertia tensor I_b in body coordinates
-	 * by I_w = R * I_b * R^T
+	 * The inertia tensor Iw in world coordinates is computed
+	 * with the local inertia tensor Ib in body coordinates
+	 * by Iw = R * Ib * R^T
 	 * where R is the rotation matrix (and R^T its transpose) of
 	 * the current orientation quaternion of the body
 	 * @return The 3x3 inertia tensor matrix of the body in world-space coordinates
 	 */
 	public Matrix3f getInertiaTensorWorld() {
 		// Compute and return the inertia tensor in world coordinates
-		return this.transform.getOrientation().getMatrix().multiplyNew(this.inertiaTensorLocal).multiply(this.transform.getOrientation().getMatrix().transposeNew());
+		return this.transform.getOrientation().getMatrix().multiply(this.inertiaTensorLocal)
+				.multiply(this.transform.getOrientation().getMatrix().transpose());
 	}
-	
+
 	/**
 	 *  Get the first element of the linked list of joints involving this body
 	 * @return The first element of the linked-list of all the joints involving this body
@@ -239,7 +246,7 @@ public class RigidBody extends CollisionBody {
 	public JointListElement getJointsList() {
 		return this.jointsList;
 	}
-	
+
 	/**
 	 *  Get the linear velocity damping factor
 	 * @return The linear damping factor of this body
@@ -247,7 +254,7 @@ public class RigidBody extends CollisionBody {
 	public float getLinearDamping() {
 		return this.linearDamping;
 	}
-	
+
 	/**
 	 *  Get the linear velocity
 	 * @return The linear velocity vector of the body
@@ -255,7 +262,7 @@ public class RigidBody extends CollisionBody {
 	public Vector3f getLinearVelocity() {
 		return this.linearVelocity;
 	}
-	
+
 	/**
 	 *  Get the mass of the body
 	 * @return The mass (in kilograms) of the body
@@ -263,7 +270,7 @@ public class RigidBody extends CollisionBody {
 	public float getMass() {
 		return this.initMass;
 	}
-	
+
 	/**
 	 *  get a reference to the material properties of the rigid body
 	 * @return A reference to the material of the body
@@ -271,11 +278,11 @@ public class RigidBody extends CollisionBody {
 	public Material getMaterial() {
 		return this.material;
 	}
-	
+
 	public boolean isAngularReactionEnable() {
 		return this.angularReactionEnable;
 	}
-	
+
 	/**
 	 *  get the need of gravity appling to this rigid body
 	 * @return True if the gravity is applied to the body
@@ -283,16 +290,16 @@ public class RigidBody extends CollisionBody {
 	public boolean isGravityEnabled() {
 		return this.isGravityEnabled;
 	}
-	
+
 	/**
 	 *  Recompute the center of mass, total mass and inertia tensor of the body using all the collision shapes attached to the body.
 	 */
 	public void recomputeMassInformation() {
 		this.initMass = 0.0f;
 		this.massInverse = 0.0f;
-		this.inertiaTensorLocal.setZero();
-		this.inertiaTensorLocalInverse.setZero();
-		this.centerOfMassLocal.setZero();
+		this.inertiaTensorLocal = Matrix3f.ZERO;
+		this.inertiaTensorLocalInverse = Matrix3f.ZERO;
+		this.centerOfMassLocal = Vector3f.ZERO;
 		// If it is STATIC or KINEMATIC body
 		if (this.type == BodyType.STATIC || this.type == BodyType.KINEMATIC) {
 			this.centerOfMassWorld = this.transform.getPosition();
@@ -302,7 +309,8 @@ public class RigidBody extends CollisionBody {
 		// Compute the total mass of the body
 		for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
 			this.initMass += shape.getMass();
-			this.centerOfMassLocal.add(shape.getLocalToBodyTransform().getPosition().multiplyNew(shape.getMass()));
+			this.centerOfMassLocal = this.centerOfMassLocal
+					.add(shape.getLocalToBodyTransform().getPosition().multiply(shape.getMass()));
 		}
 		if (this.initMass > 0.0f) {
 			this.massInverse = 1.0f / this.initMass;
@@ -312,40 +320,41 @@ public class RigidBody extends CollisionBody {
 		}
 		// Compute the center of mass
 		final Vector3f oldCenterOfMass = this.centerOfMassWorld;
-		this.centerOfMassLocal.multiply(this.massInverse);
-		this.centerOfMassWorld = this.transform.multiplyNew(this.centerOfMassLocal);
+		this.centerOfMassLocal = this.centerOfMassLocal.multiply(this.massInverse);
+		this.centerOfMassWorld = this.transform.multiply(this.centerOfMassLocal);
 		// Compute the total mass and inertia tensor using all the collision shapes
 		for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
 			// Get the inertia tensor of the collision shape in its local-space
-			Matrix3f inertiaTensor = new Matrix3f();
-			shape.getCollisionShape().computeLocalInertiaTensor(inertiaTensor, shape.getMass());
+			Matrix3f inertiaTensor = shape.getCollisionShape().computeLocalInertiaTensor(shape.getMass());
 			// Convert the collision shape inertia tensor into the local-space of the body
 			final Transform3D shapeTransform = shape.getLocalToBodyTransform();
 			final Matrix3f rotationMatrix = shapeTransform.getOrientation().getMatrix();
-			inertiaTensor = rotationMatrix.multiplyNew(inertiaTensor).multiply(rotationMatrix.transposeNew());
+			inertiaTensor = rotationMatrix.multiply(inertiaTensor).multiply(rotationMatrix.transpose());
 			// Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
 			// center into a inertia tensor w.r.t to the body origin.
-			final Vector3f offset = shapeTransform.getPosition().lessNew(this.centerOfMassLocal);
+			final Vector3f offset = shapeTransform.getPosition().less(this.centerOfMassLocal);
 			final float offsetSquare = offset.length2();
-			final Vector3f off1 = offset.multiplyNew(-offset.x);
-			final Vector3f off2 = offset.multiplyNew(-offset.y);
-			final Vector3f off3 = offset.multiplyNew(-offset.z);
-			final Matrix3f offsetMatrix = new Matrix3f(off1.x + offsetSquare, off1.y, off1.z, off2.x, off2.y + offsetSquare, off2.z, off3.x, off3.y, off3.z + offsetSquare);
-			offsetMatrix.multiply(shape.getMass());
-			this.inertiaTensorLocal.add(inertiaTensor.addNew(offsetMatrix));
+			final Vector3f off1 = offset.multiply(-offset.x());
+			final Vector3f off2 = offset.multiply(-offset.y());
+			final Vector3f off3 = offset.multiply(-offset.z());
+			Matrix3f offsetMatrix = new Matrix3f(off1.x() + offsetSquare, off1.y(), off1.z(), off2.x(),
+					off2.y() + offsetSquare, off2.z(), off3.x(), off3.y(), off3.z() + offsetSquare);
+			offsetMatrix = offsetMatrix.multiply(shape.getMass());
+			this.inertiaTensorLocal = this.inertiaTensorLocal.add(inertiaTensor.add(offsetMatrix));
 		}
 		// Compute the local inverse inertia tensor
-		this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverseNew();
+		this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverse();
 		// Update the linear velocity of the center of mass
-		this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.lessNew(oldCenterOfMass)));
+		this.linearVelocity = this.linearVelocity
+				.add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass)));
 	}
-	
+
 	@Override
-	public void removeCollisionShape(final ProxyShape _proxyShape) {
-		super.removeCollisionShape(_proxyShape);
+	public void removeCollisionShape(final ProxyShape proxyShape) {
+		super.removeCollisionShape(proxyShape);
 		recomputeMassInformation();
 	}
-	
+
 	/**
 	 *  Remove a joint from the joints list
 	 */
@@ -370,37 +379,37 @@ public class RigidBody extends CollisionBody {
 			}
 		}
 	}
-	
+
 	/**
 	 *  Set the angular damping factor. This is the ratio of the angular velocity that the body will lose at every seconds of simulation.
-	 * @param _angularDamping The angular damping factor of this body
+	 * @param angularDamping The angular damping factor of this body
 	 */
-	public void setAngularDamping(final float _angularDamping) {
-		assert (_angularDamping >= 0.0f);
-		this.angularDamping = _angularDamping;
+	public void setAngularDamping(final float angularDamping) {
+		assert (angularDamping >= 0.0f);
+		this.angularDamping = angularDamping;
 	}
-	
+
 	public void setAngularReactionEnable(final boolean angularReactionEnable) {
 		this.angularReactionEnable = angularReactionEnable;
 	}
-	
+
 	/**
 	*  Set the angular velocity.
-	* @param _angularVelocity The angular velocity vector of the body
+	* @param angularVelocity The angular velocity vector of the body
 	*/
-	public void setAngularVelocity(final Vector3f _angularVelocity) {
+	public void setAngularVelocity(final Vector3f angularVelocity) {
 		if (this.type == BodyType.STATIC) {
 			return;
 		}
-		this.angularVelocity = _angularVelocity;
+		this.angularVelocity = angularVelocity;
 		if (this.angularVelocity.length2() > 0.0f) {
 			setIsSleeping(false);
 		}
 	}
-	
+
 	/**
 	 *  Set the local center of mass of the body (in local-space coordinates)
-	 * @param _centerOfMassLocal The center of mass of the body in local-space coordinates
+	 * @param centerOfMassLocal The center of mass of the body in local-space coordinates
 	 */
 	public void setCenterOfMassLocal(final Vector3f centerOfMassLocal) {
 		if (this.type != BodyType.DYNAMIC) {
@@ -408,69 +417,70 @@ public class RigidBody extends CollisionBody {
 		}
 		final Vector3f oldCenterOfMass = this.centerOfMassWorld;
 		this.centerOfMassLocal = centerOfMassLocal;
-		this.centerOfMassWorld = this.transform.multiplyNew(this.centerOfMassLocal);
-		this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.lessNew(oldCenterOfMass)));
+		this.centerOfMassWorld = this.transform.multiply(this.centerOfMassLocal);
+		this.linearVelocity = this.linearVelocity
+				.add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass)));
 	}
-	
+
 	/**
 	 *  Set the local inertia tensor of the body (in local-space coordinates)
-	 * @param _inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space coordinates
+	 * @param inertiaTensorLocal The 3x3 inertia tensor matrix of the body in local-space coordinates
 	 */
 	public void setInertiaTensorLocal(final Matrix3f inertiaTensorLocal) {
 		if (this.type != BodyType.DYNAMIC) {
 			return;
 		}
 		this.inertiaTensorLocal = inertiaTensorLocal;
-		this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverseNew();
+		this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverse();
 	}
-	
+
 	/**
 	 *  Set the variable to know whether or not the body is sleeping
-	 * @param _isSleeping New sleeping state of the body
+	 * @param isSleeping New sleeping state of the body
 	 */
 	@Override
-	public void setIsSleeping(final boolean _isSleeping) {
-		if (_isSleeping) {
-			this.linearVelocity.setZero();
-			this.angularVelocity.setZero();
-			this.externalForce.setZero();
-			this.externalTorque.setZero();
+	public void setIsSleeping(final boolean isSleeping) {
+		if (isSleeping) {
+			this.linearVelocity = Vector3f.ZERO;
+			this.angularVelocity = Vector3f.ZERO;
+			this.externalForce = Vector3f.ZERO;
+			this.externalTorque = Vector3f.ZERO;
 		}
-		super.setIsSleeping(_isSleeping);
+		super.setIsSleeping(isSleeping);
 	}
-	
+
 	/**
 	 *  Set the linear damping factor. This is the ratio of the linear velocity that the body will lose every at seconds of simulation.
-	 * @param _linearDamping The linear damping factor of this body
+	 * @param linearDamping The linear damping factor of this body
 	 */
-	public void setLinearDamping(final float _linearDamping) {
-		assert (_linearDamping >= 0.0f);
-		this.linearDamping = _linearDamping;
+	public void setLinearDamping(final float linearDamping) {
+		assert (linearDamping >= 0.0f);
+		this.linearDamping = linearDamping;
 	}
-	
+
 	/**
 	 *  Set the linear velocity of the rigid body.
-	 * @param _linearVelocity Linear velocity vector of the body
+	 * @param linearVelocity Linear velocity vector of the body
 	 */
-	public void setLinearVelocity(final Vector3f _linearVelocity) {
+	public void setLinearVelocity(final Vector3f linearVelocity) {
 		if (this.type == BodyType.STATIC) {
 			return;
 		}
-		this.linearVelocity = _linearVelocity;
+		this.linearVelocity = linearVelocity;
 		if (this.linearVelocity.length2() > 0.0f) {
 			setIsSleeping(false);
 		}
 	}
-	
+
 	/**
 	 *  Set the mass of the rigid body
-	 * @param _mass The mass (in kilograms) of the body
+	 * @param mass The mass (in kilograms) of the body
 	 */
-	public void setMass(final float _mass) {
+	public void setMass(final float mass) {
 		if (this.type != BodyType.DYNAMIC) {
 			return;
 		}
-		this.initMass = _mass;
+		this.initMass = mass;
 		if (this.initMass > 0.0f) {
 			this.massInverse = 1.0f / this.initMass;
 		} else {
@@ -478,97 +488,104 @@ public class RigidBody extends CollisionBody {
 			this.massInverse = 1.0f;
 		}
 	}
-	
+
 	/**
 	 *  Set a new material for this rigid body
-	 * @param _material The material you want to set to the body
+	 * @param material The material you want to set to the body
 	 */
-	public void setMaterial(final Material _material) {
-		this.material = _material;
+	public void setMaterial(final Material material) {
+		this.material = material;
 	}
-	
+
 	/**
 	 *  Set the current position and orientation
-	 * @param _transform The transformation of the body that transforms the local-space of the body into world-space
+	 * @param transform The transformation of the body that transforms the local-space of the body into world-space
 	 */
 	@Override
-	public void setTransform(final Transform3D _transform) {
-		//Log.info("         set transform: " + this.transform + " ==> " + _transform);
-		if (_transform.getPosition().x == Float.NaN) {
-			Log.critical("         set transform: " + this.transform + " ==> " + _transform);
+	public void setTransform(final Transform3D transform) {
+		//LOGGER.info("         set transform: " + this.transform + " ==> " + transform);
+		if (transform.getPosition().x() == Float.NaN) {
+			LOGGER.error("[CRITICAL]          set transform: " + this.transform + " ==> " + transform);
+			System.exit(-1);
 		}
-		if (Float.isInfinite(_transform.getOrientation().z)) {
-			Log.critical("         set transform: " + this.transform + " ==> " + _transform);
+		if (Float.isInfinite(transform.getOrientation().z())) {
+			LOGGER.error("[CRITICAL]          set transform: " + this.transform + " ==> " + transform);
+			System.exit(-1);
 		}
-		this.transform = _transform;
+		this.transform = transform;
 		final Vector3f oldCenterOfMass = this.centerOfMassWorld;
 		// Compute the new center of mass in world-space coordinates
-		this.centerOfMassWorld = this.transform.multiplyNew(this.centerOfMassLocal);
+		this.centerOfMassWorld = this.transform.multiply(this.centerOfMassLocal);
 		// Update the linear velocity of the center of mass
-		this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.lessNew(oldCenterOfMass)));
+		this.linearVelocity = this.linearVelocity
+				.add(this.angularVelocity.cross(this.centerOfMassWorld.less(oldCenterOfMass)));
 		updateBroadPhaseState();
 	}
-	
+
 	@Override
-	public void setType(final BodyType _type) {
-		if (this.type == _type) {
+	public void setType(final BodyType type) {
+		if (this.type == type) {
 			return;
 		}
-		super.setType(_type);
+		super.setType(type);
 		recomputeMassInformation();
 		if (this.type == BodyType.STATIC) {
 			// Reset the velocity to zero
-			this.linearVelocity.setZero();
-			this.angularVelocity.setZero();
+			this.linearVelocity = Vector3f.ZERO;
+			this.angularVelocity = Vector3f.ZERO;
 		}
 		if (this.type == BodyType.STATIC || this.type == BodyType.KINEMATIC) {
 			// Reset the inverse mass and inverse inertia tensor to zero
 			this.massInverse = 0.0f;
-			this.inertiaTensorLocal.setZero();
-			this.inertiaTensorLocalInverse.setZero();
+			this.inertiaTensorLocal = Matrix3f.ZERO;
+			this.inertiaTensorLocalInverse = Matrix3f.ZERO;
 		} else {
 			this.massInverse = 1.0f / this.initMass;
-			this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverseNew();
+			this.inertiaTensorLocalInverse = this.inertiaTensorLocal.inverse();
 		}
 		setIsSleeping(false);
 		resetContactManifoldsList();
 		// Ask the broad-phase to test again the collision shapes of the body for collision detection (as if the body has moved)
 		askForBroadPhaseCollisionCheck();
-		this.externalForce.setZero();
-		this.externalTorque.setZero();
+		this.externalForce = Vector3f.ZERO;
+		this.externalTorque = Vector3f.ZERO;
 	}
-	
+
 	@Override
 	public void updateBroadPhaseState() {
 		final DynamicsWorld world = (DynamicsWorld) this.world;
-		final Vector3f displacement = this.linearVelocity.multiplyNew(world.timeStep);
-		
+		final Vector3f displacement = this.linearVelocity.multiply(world.timeStep);
+
 		// For all the proxy collision shapes of the body
 		for (ProxyShape shape = this.proxyCollisionShapes; shape != null; shape = shape.next) {
 			// Recompute the world-space AABB of the collision shape
 			final AABB aabb = new AABB();
-			//Log.info("         : " + aabb.getMin() + " " + aabb.getMax());
-			//Log.info("         this.transform: " + this.transform);
-			shape.getCollisionShape().computeAABB(aabb, this.transform.multiplyNew(shape.getLocalToBodyTransform()));
-			//Log.info("         : " + aabb.getMin() + " " + aabb.getMax());
+			//LOGGER.info("         : " + aabb.getMin() + " " + aabb.getMax());
+			//LOGGER.info("         this.transform: " + this.transform);
+			shape.getCollisionShape().computeAABB(aabb, this.transform.multiply(shape.getLocalToBodyTransform()));
+			//LOGGER.info("         : " + aabb.getMin() + " " + aabb.getMax());
 			// Update the broad-phase state for the proxy collision shape
-			//Log.warning("            ==> updateProxyCollisionShape");
+			//LOGGER.warn("            ==> updateProxyCollisionShape");
 			world.collisionDetection.updateProxyCollisionShape(shape, aabb, displacement);
 		}
 	}
-	
+
 	/**
 	 *  Update the transform of the body after a change of the center of mass
 	 */
 	public void updateTransformWithCenterOfMass() {
 		// Translate the body according to the translation of the center of mass position
-		this.transform.setPosition(this.centerOfMassWorld.lessNew(this.transform.getOrientation().multiply(this.centerOfMassLocal)));
-		if (Float.isNaN(this.transform.getPosition().x) == true) {
-			Log.critical("updateTransformWithCenterOfMass: " + this.transform);
+		this.transform = new Transform3D(
+				this.centerOfMassWorld.less(this.transform.getOrientation().multiply(this.centerOfMassLocal)),
+				this.transform.getOrientation());
+		if (Float.isNaN(this.transform.getPosition().x())) {
+			LOGGER.error("[CRITICAL] updateTransformWithCenterOfMass: " + this.transform);
+			System.exit(-1);
 		}
-		if (Float.isInfinite(this.transform.getOrientation().z) == true) {
-			Log.critical("         set transform: " + this.transform);
+		if (Float.isInfinite(this.transform.getOrientation().z())) {
+			LOGGER.error("[CRITICAL]          set transform: " + this.transform);
+			System.exit(-1);
 		}
 	}
-	
+
 }

src/org/atriasoft/ephysics/collision/CollisionDetection.java

@@ -2,6 +2,7 @@ package org.atriasoft.ephysics.collision;
 
 import java.util.Iterator;
 import java.util.Map;
+import java.util.Map.Entry;
 import java.util.TreeMap;
 
 import org.atriasoft.ephysics.RaycastCallback;
@@ -25,39 +26,41 @@ import org.atriasoft.ephysics.engine.CollisionCallback;
 import org.atriasoft.ephysics.engine.CollisionWorld;
 import org.atriasoft.ephysics.engine.EventListener;
 import org.atriasoft.ephysics.engine.OverlappingPair;
-import org.atriasoft.ephysics.internal.Log;
 import org.atriasoft.ephysics.mathematics.PairInt;
 import org.atriasoft.ephysics.mathematics.Ray;
 import org.atriasoft.ephysics.mathematics.Set;
 import org.atriasoft.ephysics.mathematics.SetMultiple;
 import org.atriasoft.etk.math.Vector3f;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 /*
- * @brief It computes the collision detection algorithms. We first
+ * It computes the collision detection algorithms. We first
  * perform a broad-phase algorithm to know which pairs of bodies can
  * collide and then we run a narrow-phase algorithm to compute the
  * collision contacts between bodies.
  */
 public class CollisionDetection implements NarrowPhaseCallback {
-	/// Reference on the physics world
-	private final CollisionWorld world;
+	static final Logger LOGGER = LoggerFactory.getLogger(CollisionDetection.class);
 	/// Broad-phase algorithm
 	private final BroadPhaseAlgorithm broadPhaseAlgorithm;
 	/// Collision Detection Dispatch configuration
 	private CollisionDispatch collisionDispatch;
-	private final DefaultCollisionDispatch defaultCollisionDispatch = new DefaultCollisionDispatch();
 	/// Collision detection matrix (algorithms to use)
 	private final NarrowPhaseAlgorithm[][] collisionMatrix = new NarrowPhaseAlgorithm[CollisionShapeType.NB_COLLISION_SHAPE_TYPES][CollisionShapeType.NB_COLLISION_SHAPE_TYPES];
-	public Map<PairDTree, OverlappingPair> overlappingPairs = new TreeMap<>(Set.getPairDTreeCoparator()); //!< Broad-phase overlapping pairs
 	private final Map<PairDTree, OverlappingPair> contactOverlappingPair = new TreeMap<>(Set.getPairDTreeCoparator()); //!< Overlapping pairs in contact (during the current Narrow-phase collision detection)
+	private final DefaultCollisionDispatch defaultCollisionDispatch = new DefaultCollisionDispatch();
+	private boolean isCollisionShapesAdded; //!< True if some collision shapes have been added previously
 	// TODO : Delete this
 	private final GJKAlgorithm narrowPhaseGJKAlgorithm; //!< Narrow-phase GJK algorithm
-	private final SetMultiple<PairInt> noCollisionPairs = new SetMultiple<PairInt>(Set.getPairIntCoparator()); //!< Set of pair of bodies that cannot collide between each other
-	private boolean isCollisionShapesAdded; //!< True if some collision shapes have been added previously
-	
+	private final SetMultiple<PairInt> noCollisionPairs = new SetMultiple<>(Set.getPairIntCoparator()); //!< Set of pair of bodies that cannot collide between each other
+	public Map<PairDTree, OverlappingPair> overlappingPairs = new TreeMap<>(Set.getPairDTreeCoparator()); //!< Broad-phase overlapping pairs
+	/// Reference on the physics world
+	private final CollisionWorld world;
+
 	/// Constructor
-	public CollisionDetection(final CollisionWorld _world) {
-		this.world = _world;
+	public CollisionDetection(final CollisionWorld world) {
+		this.world = world;
 		this.broadPhaseAlgorithm = new BroadPhaseAlgorithm(this);
 		this.isCollisionShapesAdded = false;
 		this.narrowPhaseGJKAlgorithm = new GJKAlgorithm(this);
@@ -66,24 +69,24 @@ public class CollisionDetection implements NarrowPhaseCallback {
 		// Fill-in the collision detection matrix with algorithms
 		fillInCollisionMatrix();
 	}
-	
+
 	/// Add all the contact manifold of colliding pairs to their bodies
 	private void addAllContactManifoldsToBodies() {
 		// For each overlapping pairs in contact during the narrow-phase
 		for (final OverlappingPair it : this.contactOverlappingPair.values()) {
-			// Add all the contact manifolds of the pair int32_to the list of contact manifolds
+			// Add all the contact manifolds of the pair int32to the list of contact manifolds
 			// of the two bodies involved in the contact
 			addContactManifoldToBody(it);
 		}
 	}
-	
+
 	/// Add a contact manifold to the linked list of contact manifolds of the two bodies
 	/// involed in the corresponding contact.
-	private void addContactManifoldToBody(final OverlappingPair _pair) {
-		assert (_pair != null);
-		final CollisionBody body1 = _pair.getShape1().getBody();
-		final CollisionBody body2 = _pair.getShape2().getBody();
-		final ContactManifoldSet manifoldSet = _pair.getContactManifoldSet();
+	private void addContactManifoldToBody(final OverlappingPair pair) {
+		assert (pair != null);
+		final CollisionBody body1 = pair.getShape1().getBody();
+		final CollisionBody body2 = pair.getShape2().getBody();
+		final ContactManifoldSet manifoldSet = pair.getContactManifoldSet();
 		// For each contact manifold in the set of manifolds in the pair
 		for (int i = 0; i < manifoldSet.getNbContactManifolds(); i++) {
 			final ContactManifold contactManifold = manifoldSet.getContactManifold(i);
@@ -91,70 +94,72 @@ public class CollisionDetection implements NarrowPhaseCallback {
 			// Add the contact manifold at the beginning of the linked
 			// list of contact manifolds of the first body
 			body1.contactManifoldsList = new ContactManifoldListElement(contactManifold, body1.contactManifoldsList);
-			
+
 			// Add the contact manifold at the beginning of the linked
 			// list of the contact manifolds of the second body
 			body2.contactManifoldsList = new ContactManifoldListElement(contactManifold, body2.contactManifoldsList);
-			
+
 		}
 	}
-	
+
 	/// Add a pair of bodies that cannot collide with each other
-	public void addNoCollisionPair(final CollisionBody _body1, final CollisionBody _body2) {
-		this.noCollisionPairs.add(OverlappingPair.computeBodiesIndexPair(_body1, _body2));
+	public void addNoCollisionPair(final CollisionBody body1, final CollisionBody body2) {
+		this.noCollisionPairs.add(OverlappingPair.computeBodiesIndexPair(body1, body2));
 	}
-	
+
 	/// Add a proxy collision shape to the collision detection
-	public void addProxyCollisionShape(final ProxyShape _proxyShape, final AABB _aabb) {
+	public void addProxyCollisionShape(final ProxyShape proxyShape, final AABB aabb) {
 		// Add the body to the broad-phase
-		this.broadPhaseAlgorithm.addProxyCollisionShape(_proxyShape, _aabb);
+		this.broadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb);
 		this.isCollisionShapesAdded = true;
 	}
-	
+
 	// Ask for a collision shape to be tested again during broad-phase.
 	/// We simply put the shape in the list of collision shape that have moved in the
 	/// previous frame so that it is tested for collision again in the broad-phase.
-	public void askForBroadPhaseCollisionCheck(final ProxyShape _shape) {
-		this.broadPhaseAlgorithm.addMovedCollisionShape(_shape.broadPhaseID);
+	public void askForBroadPhaseCollisionCheck(final ProxyShape shape) {
+		this.broadPhaseAlgorithm.addMovedCollisionShape(shape.broadPhaseID);
 	}
-	
+
 	/// Allow the broadphase to notify the collision detection about an overlapping pair.
 	/// This method is called by the broad-phase collision detection algorithm
-	public void broadPhaseNotifyOverlappingPair(final ProxyShape _shape1, final ProxyShape _shape2) {
-		assert (_shape1.broadPhaseID != _shape2.broadPhaseID);
+	public void broadPhaseNotifyOverlappingPair(final ProxyShape shape1, final ProxyShape shape2) {
+		assert (shape1.broadPhaseID != shape2.broadPhaseID);
 		// If the two proxy collision shapes are from the same body, skip it
-		if (_shape1.getBody().getID() == _shape2.getBody().getID()) {
+		if (shape1.getBody().getID() == shape2.getBody().getID()) {
 			return;
 		}
-		//Log.info(" check collision is allowed: " + _shape1.getBody().getID() + "  " + _shape2.getBody().getID());
+		//LOGGER.info(" check collision is allowed: " + shape1.getBody().getID() + "  " + shape2.getBody().getID());
 		// Check if the collision filtering allows collision between the two shapes
-		if ((_shape1.getCollideWithMaskBits() & _shape2.getCollisionCategoryBits()) == 0 || (_shape1.getCollisionCategoryBits() & _shape2.getCollideWithMaskBits()) == 0) {
-			Log.info("    ==> not permited ...");
+		if ((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0
+				|| (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0) {
+			LOGGER.info("    ==> not permited ...");
 			return;
 		}
 		// Compute the overlapping pair ID
-		final PairDTree pairID = OverlappingPair.computeID(_shape1, _shape2);
+		final PairDTree pairID = OverlappingPair.computeID(shape1, shape2);
 		// Check if the overlapping pair already exists
 		if (this.overlappingPairs.get(pairID) != null) {
 			return;
 		}
 		// Compute the maximum number of contact manifolds for this pair
-		final int nbMaxManifolds = CollisionShape.computeNbMaxContactManifolds(_shape1.getCollisionShape().getType(), _shape2.getCollisionShape().getType());
-		// Create the overlapping pair and add it int32_to the set of overlapping pairs
-		final OverlappingPair newPair = new OverlappingPair(_shape1, _shape2, nbMaxManifolds);
+		final int nbMaxManifolds = CollisionShape.computeNbMaxContactManifolds(shape1.getCollisionShape().getType(),
+				shape2.getCollisionShape().getType());
+		// Create the overlapping pair and add it int32to the set of overlapping pairs
+		final OverlappingPair newPair = new OverlappingPair(shape1, shape2, nbMaxManifolds);
 		assert (newPair != null);
 		this.overlappingPairs.putIfAbsent(pairID, newPair);
 		// Wake up the two bodies
-		_shape1.getBody().setIsSleeping(false);
-		_shape2.getBody().setIsSleeping(false);
+		shape1.getBody().setIsSleeping(false);
+		shape2.getBody().setIsSleeping(false);
 	}
-	
+
 	/// Delete all the contact points in the currently overlapping pairs
 	private void clearContactPoints() {
 		// For each overlapping pair
 		this.overlappingPairs.forEach((key, value) -> value.clearContactPoints());
 	}
-	
+
 	/// Compute the broad-phase collision detection
 	private void computeBroadPhase() {
 		// If new collision shapes have been added to bodies
@@ -165,33 +170,29 @@ public class CollisionDetection implements NarrowPhaseCallback {
 			this.broadPhaseAlgorithm.computeOverlappingPairs();
 		}
 	}
-	
+
 	/// Compute the collision detection
 	public void computeCollisionDetection() {
-		//Log.info("computeBroadPhase();");
+		//LOGGER.info("computeBroadPhase();");
 		// Compute the broad-phase collision detection
 		computeBroadPhase();
-		//Log.info("computeNarrowPhase();");
+		//LOGGER.info("computeNarrowPhase();");
 		// Compute the narrow-phase collision detection
 		computeNarrowPhase();
 	}
-	
+
 	/// Compute the narrow-phase collision detection
 	private void computeNarrowPhase() {
 		// Clear the set of overlapping pairs in narrow-phase contact
 		this.contactOverlappingPair.clear();
 		{
-			//Log.info("list elements:");
-			final Iterator<Map.Entry<PairDTree, OverlappingPair>> ittt = this.overlappingPairs.entrySet().iterator();
-			while (ittt.hasNext()) {
-				final Map.Entry<PairDTree, OverlappingPair> entry = ittt.next();
-				//Log.info("    " + entry.getKey() + "  " + entry.getValue());
-				
+			for (Entry<PairDTree, OverlappingPair> entry : this.overlappingPairs.entrySet()) {
+
 			}
 		}
 		// For each possible collision pair of bodies
 		final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator();
-		
+
 		while (it.hasNext()) {
 			final Map.Entry<PairDTree, OverlappingPair> entry = it.next();
 			final OverlappingPair pair = entry.getValue();
@@ -201,7 +202,8 @@ public class CollisionDetection implements NarrowPhaseCallback {
 			// Check if the collision filtering allows collision between the two shapes and
 			// that the two shapes are still overlapping. Otherwise, we destroy the
 			// overlapping pair
-			if (((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0 || (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0)
+			if (((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0
+					|| (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0)
 					|| !this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
 				// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
 				// Destroy the overlapping pair
@@ -234,23 +236,28 @@ public class CollisionDetection implements NarrowPhaseCallback {
 			// Notify the narrow-phase algorithm about the overlapping pair we are going to test
 			narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
 			// Create the CollisionShapeInfo objects
-			final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(), shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData());
-			
-			final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(), shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData());
-			
+			final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(),
+					shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData());
+
+			final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(),
+					shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData());
+
 			// Use the narrow-phase collision detection algorithm to check
 			// if there really is a collision. If a collision occurs, the
 			// notifyContact() callback method will be called.
 			narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, this);
 		}
-		
+
 		// Add all the contact manifolds (between colliding bodies) to the bodies
 		addAllContactManifoldsToBodies();
-		
+
 	}
-	
+
 	/// Compute the narrow-phase collision detection
-	public void computeNarrowPhaseBetweenShapes(final CollisionCallback _callback, final Set<DTree> _shapes1, final Set<DTree> _shapes2) {
+	public void computeNarrowPhaseBetweenShapes(
+			final CollisionCallback callback,
+			final Set<DTree> shapes1,
+			final Set<DTree> shapes2) {
 		this.contactOverlappingPair.clear();
 		// For each possible collision pair of bodies
 		final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator();
@@ -262,20 +269,24 @@ public class CollisionDetection implements NarrowPhaseCallback {
 			assert (shape1.broadPhaseID != shape2.broadPhaseID);
 			// If both shapes1 and shapes2 sets are non-empty, we check that
 			// shape1 is among on set and shape2 is among the other one
-			if (!_shapes1.isEmpty() && !_shapes2.isEmpty() && (_shapes1.count(shape1.broadPhaseID) == 0 || _shapes2.count(shape2.broadPhaseID) == 0)
-					&& (_shapes1.count(shape2.broadPhaseID) == 0 || _shapes2.count(shape1.broadPhaseID) == 0)) {
+			if (!shapes1.isEmpty() && !shapes2.isEmpty()
+					&& (shapes1.count(shape1.broadPhaseID) == 0 || shapes2.count(shape2.broadPhaseID) == 0)
+					&& (shapes1.count(shape2.broadPhaseID) == 0 || shapes2.count(shape1.broadPhaseID) == 0)) {
 				continue;
 			}
-			if (!_shapes1.isEmpty() && _shapes2.isEmpty() && _shapes1.count(shape1.broadPhaseID) == 0 && _shapes1.count(shape2.broadPhaseID) == 0) {
+			if (!shapes1.isEmpty() && shapes2.isEmpty() && shapes1.count(shape1.broadPhaseID) == 0
+					&& shapes1.count(shape2.broadPhaseID) == 0) {
 				continue;
 			}
-			if (!_shapes2.isEmpty() && _shapes1.isEmpty() && _shapes2.count(shape1.broadPhaseID) == 0 && _shapes2.count(shape2.broadPhaseID) == 0) {
+			if (!shapes2.isEmpty() && shapes1.isEmpty() && shapes2.count(shape1.broadPhaseID) == 0
+					&& shapes2.count(shape2.broadPhaseID) == 0) {
 				continue;
 			}
 			// Check if the collision filtering allows collision between the two shapes and
 			// that the two shapes are still overlapping. Otherwise, we destroy the
 			// overlapping pair
-			if (((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0 || (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0)
+			if (((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0
+					|| (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0)
 					|| !this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
 				// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
 				// Destroy the overlapping pair
@@ -309,129 +320,139 @@ public class CollisionDetection implements NarrowPhaseCallback {
 			// Notify the narrow-phase algorithm about the overlapping pair we are going to test
 			narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
 			// Create the CollisionShapeInfo objects
-			final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(), shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData());
-			
-			final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(), shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData());
-			
-			final TestCollisionBetweenShapesCallback narrowPhaseCallback = new TestCollisionBetweenShapesCallback(_callback);
+			final CollisionShapeInfo shape1Info = new CollisionShapeInfo(shape1, shape1.getCollisionShape(),
+					shape1.getLocalToWorldTransform(), pair, shape1.getCachedCollisionData());
+
+			final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(),
+					shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData());
+
+			final TestCollisionBetweenShapesCallback narrowPhaseCallback = new TestCollisionBetweenShapesCallback(
+					callback);
 			// Use the narrow-phase collision detection algorithm to check
 			// if there really is a collision
 			narrowPhaseAlgorithm.testCollision(shape1Info, shape2Info, narrowPhaseCallback);
 		}
-		
+
 		// Add all the contact manifolds (between colliding bodies) to the bodies
 		addAllContactManifoldsToBodies();
 	}
-	
-	void createContact(final OverlappingPair _overlappingPair, final ContactPointInfo _contactInfo) {
+
+	void createContact(final OverlappingPair overlappingPair, final ContactPointInfo contactInfo) {
 		// Create a new contact
-		final ContactPoint contact = new ContactPoint(_contactInfo);
+		final ContactPoint contact = new ContactPoint(contactInfo);
 		// Add the contact to the contact manifold set of the corresponding overlapping pair
-		_overlappingPair.addContact(contact);
-		// Add the overlapping pair int32_to the set of pairs in contact during narrow-phase
-		final PairDTree pairId = OverlappingPair.computeID(_overlappingPair.getShape1(), _overlappingPair.getShape2());
-		this.contactOverlappingPair.put(pairId, _overlappingPair);
+		overlappingPair.addContact(contact);
+		// Add the overlapping pair int32to the set of pairs in contact during narrow-phase
+		final PairDTree pairId = OverlappingPair.computeID(overlappingPair.getShape1(), overlappingPair.getShape2());
+		this.contactOverlappingPair.put(pairId, overlappingPair);
 	}
-	
+
 	/// Fill-in the collision detection matrix
 	private void fillInCollisionMatrix() {
 		// For each possible type of collision shape
 		for (int i = 0; i < CollisionShapeType.NB_COLLISION_SHAPE_TYPES; i++) {
 			for (int j = 0; j < CollisionShapeType.NB_COLLISION_SHAPE_TYPES; j++) {
-				this.collisionMatrix[i][j] = this.collisionDispatch.selectAlgorithm(CollisionShapeType.getType(i), CollisionShapeType.getType(j));
+				this.collisionMatrix[i][j] = this.collisionDispatch.selectAlgorithm(CollisionShapeType.getType(i),
+						CollisionShapeType.getType(j));
 			}
 		}
 	}
-	
+
 	/// Return the Narrow-phase collision detection algorithm to use between two types of shapes
-	public NarrowPhaseAlgorithm getCollisionAlgorithm(final CollisionShapeType _shape1Type, final CollisionShapeType _shape2Type) {
-		return this.collisionMatrix[_shape1Type.value][_shape2Type.value];
+	public NarrowPhaseAlgorithm getCollisionAlgorithm(
+			final CollisionShapeType shape1Type,
+			final CollisionShapeType shape2Type) {
+		return this.collisionMatrix[shape1Type.value][shape2Type.value];
 	}
-	
+
 	public GJKAlgorithm getNarrowPhaseGJKAlgorithm() {
 		return this.narrowPhaseGJKAlgorithm;
 	}
-	
+
 	/// Return a pointer to the world
 	public CollisionWorld getWorld() {
 		return this.world;
 	}
-	
+
 	/// Return the world event listener
 	public EventListener getWorldEventListener() {
 		return this.world.eventListener;
 	}
-	
+
 	/// Called by a narrow-phase collision algorithm when a new contact has been found
 	@Override
-	public void notifyContact(final OverlappingPair _overlappingPair, final ContactPointInfo _contactInfo) {
-		Log.error("Notify Contact ...     --------------------");
+	public void notifyContact(final OverlappingPair overlappingPair, final ContactPointInfo contactInfo) {
+		LOGGER.error("Notify Contact ...     --------------------");
 		// If it is the first contact since the pairs are overlapping
-		if (_overlappingPair.getNbContactPoints() == 0) {
+		if (overlappingPair.getNbContactPoints() == 0) {
 			// Trigger a callback event
 			if (this.world.eventListener != null) {
-				this.world.eventListener.beginContact(_contactInfo);
+				this.world.eventListener.beginContact(contactInfo);
 			}
 		}
 		// Create a new contact
-		createContact(_overlappingPair, _contactInfo);
+		createContact(overlappingPair, contactInfo);
 		// Trigger a callback event for the new contact
 		if (this.world.eventListener != null) {
-			this.world.eventListener.newContact(_contactInfo);
+			this.world.eventListener.newContact(contactInfo);
 		}
 	}
-	
+
 	/// Ray casting method
-	public void raycast(final RaycastCallback _raycastCallback, final Ray _ray, final int _raycastWithCategoryMaskBits) {
-		final RaycastTest rayCastTest = new RaycastTest(_raycastCallback);
+	public void raycast(final RaycastCallback raycastCallback, final Ray ray, final int raycastWithCategoryMaskBits) {
+		final RaycastTest rayCastTest = new RaycastTest(raycastCallback);
 		// Ask the broad-phase algorithm to call the testRaycastAgainstShape()
 		// callback method for each proxy shape hit by the ray in the broad-phase
-		this.broadPhaseAlgorithm.raycast(_ray, rayCastTest, _raycastWithCategoryMaskBits);
+		this.broadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits);
 	}
-	
+
 	/// Remove a pair of bodies that cannot collide with each other
-	public void removeNoCollisionPair(final CollisionBody _body1, final CollisionBody _body2) {
-		this.noCollisionPairs.erase(this.noCollisionPairs.find(OverlappingPair.computeBodiesIndexPair(_body1, _body2)));
+	public void removeNoCollisionPair(final CollisionBody body1, final CollisionBody body2) {
+		this.noCollisionPairs.erase(this.noCollisionPairs.find(OverlappingPair.computeBodiesIndexPair(body1, body2)));
 	}
-	
+
 	/// Remove a proxy collision shape from the collision detection
-	public void removeProxyCollisionShape(final ProxyShape _proxyShape) {
+	public void removeProxyCollisionShape(final ProxyShape proxyShape) {
 		// Remove all the overlapping pairs involving this proxy shape
 		final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator();
 		while (it.hasNext()) {
 			final Map.Entry<PairDTree, OverlappingPair> entry = it.next();
 			final OverlappingPair pair = entry.getValue();
-			if (pair.getShape1().broadPhaseID == _proxyShape.broadPhaseID || pair.getShape2().broadPhaseID == _proxyShape.broadPhaseID) {
+			if (pair.getShape1().broadPhaseID == proxyShape.broadPhaseID
+					|| pair.getShape2().broadPhaseID == proxyShape.broadPhaseID) {
 				// TODO : Remove all the contact manifold of the overlapping pair from the contact manifolds list of the two bodies involved
 				// Destroy the overlapping pair
 				it.remove();
 			}
 		}
 		// Remove the body from the broad-phase
-		this.broadPhaseAlgorithm.removeProxyCollisionShape(_proxyShape);
+		this.broadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
 	}
-	
+
 	/// Report collision between two sets of shapes
-	public void reportCollisionBetweenShapes(final CollisionCallback _callback, final Set<DTree> _shapes1, final Set<DTree> _shapes2) {
-		
-		// For each possible collision pair of bodies
-		final Iterator<Map.Entry<PairDTree, OverlappingPair>> it = this.overlappingPairs.entrySet().iterator();
-		while (it.hasNext()) {
-			final Map.Entry<PairDTree, OverlappingPair> entry = it.next();
+	public void reportCollisionBetweenShapes(
+			final CollisionCallback callback,
+			final Set<DTree> shapes1,
+			final Set<DTree> shapes2) {
+
+		for (Entry<PairDTree, OverlappingPair> entry : this.overlappingPairs.entrySet()) {
 			final OverlappingPair pair = entry.getValue();
 			final ProxyShape shape1 = pair.getShape1();
 			final ProxyShape shape2 = pair.getShape2();
 			assert (shape1.broadPhaseID != shape2.broadPhaseID);
 			// If both shapes1 and shapes2 sets are non-empty, we check that
 			// shape1 is among on set and shape2 is among the other one
-			if (!_shapes1.isEmpty() && !_shapes2.isEmpty() && (_shapes1.count(shape1.broadPhaseID) == 0 || _shapes2.count(shape2.broadPhaseID) == 0)
-					&& (_shapes1.count(shape2.broadPhaseID) == 0 || _shapes2.count(shape1.broadPhaseID) == 0)) {
+			if (!shapes1.isEmpty() && !shapes2.isEmpty()
+					&& (shapes1.count(shape1.broadPhaseID) == 0 || shapes2.count(shape2.broadPhaseID) == 0)
+					&& (shapes1.count(shape2.broadPhaseID) == 0 || shapes2.count(shape1.broadPhaseID) == 0)) {
 				continue;
 			}
-			if (!_shapes1.isEmpty() && _shapes2.isEmpty() && _shapes1.count(shape1.broadPhaseID) == 0 && _shapes1.count(shape2.broadPhaseID) == 0) {
+			if (!shapes1.isEmpty() && shapes2.isEmpty() && shapes1.count(shape1.broadPhaseID) == 0
+					&& shapes1.count(shape2.broadPhaseID) == 0) {
 				continue;
 			}
-			if (!_shapes2.isEmpty() && _shapes1.isEmpty() && _shapes2.count(shape1.broadPhaseID) == 0 && _shapes2.count(shape2.broadPhaseID) == 0) {
+			if (!shapes2.isEmpty() && shapes1.isEmpty() && shapes2.count(shape1.broadPhaseID) == 0
+					&& shapes2.count(shape2.broadPhaseID) == 0) {
 				continue;
 			}
 			// For each contact manifold set of the overlapping pair
@@ -442,56 +463,65 @@ public class CollisionDetection implements NarrowPhaseCallback {
 				for (int i = 0; i < manifold.getNbContactPoints(); i++) {
 					final ContactPoint contactPoint = manifold.getContactPoint(i);
 					// Create the contact info object for the contact
-					final ContactPointInfo contactInfo = new ContactPointInfo(manifold.getShape1(), manifold.getShape2(), manifold.getShape1().getCollisionShape(),
-							manifold.getShape2().getCollisionShape(), contactPoint.getNormal(), contactPoint.getPenetrationDepth(), contactPoint.getLocalPointOnBody1(),
+					final ContactPointInfo contactInfo = new ContactPointInfo(manifold.getShape1(),
+							manifold.getShape2(), manifold.getShape1().getCollisionShape(),
+							manifold.getShape2().getCollisionShape(), contactPoint.getNormal(),
+							contactPoint.getPenetrationDepth(), contactPoint.getLocalPointOnBody1(),
 							contactPoint.getLocalPointOnBody2());
 					// Notify the collision callback about this new contact
-					if (_callback != null) {
-						_callback.notifyContact(contactInfo);
+					if (callback != null) {
+						callback.notifyContact(contactInfo);
 					}
 				}
-				
+
 			}
 		}
 	}
-	
+
 	/// Set the collision dispatch configuration
-	public void setCollisionDispatch(final CollisionDispatch _collisionDispatch) {
-		this.collisionDispatch = _collisionDispatch;
+	public void setCollisionDispatch(final CollisionDispatch collisionDispatch) {
+		this.collisionDispatch = collisionDispatch;
 		this.collisionDispatch.init(this);
 		// Fill-in the collision matrix with the new algorithms to use
 		fillInCollisionMatrix();
 	}
-	
+
 	/// Test if the AABBs of two proxy shapes overlap
-	public boolean testAABBOverlap(final ProxyShape _shape1, final ProxyShape _shape2) {
+	public boolean testAABBOverlap(final ProxyShape shape1, final ProxyShape shape2) {
 		// If one of the shape's body is not active, we return no overlap
-		if (!_shape1.getBody().isActive() || !_shape2.getBody().isActive()) {
+		if (!shape1.getBody().isActive() || !shape2.getBody().isActive()) {
 			return false;
 		}
-		return this.broadPhaseAlgorithm.testOverlappingShapes(_shape1, _shape2);
+		return this.broadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
 	}
-	
+
 	/// Compute the collision detection
-	public void testCollisionBetweenShapes(final CollisionCallback _callback, final Set<DTree> _shapes1, final Set<DTree> _shapes2) {
+	public void testCollisionBetweenShapes(
+			final CollisionCallback callback,
+			final Set<DTree> shapes1,
+			final Set<DTree> shapes2) {
 		// Compute the broad-phase collision detection
 		computeBroadPhase();
 		// Delete all the contact points in the currently overlapping pairs
 		clearContactPoints();
 		// Compute the narrow-phase collision detection among given sets of shapes
-		computeNarrowPhaseBetweenShapes(_callback, _shapes1, _shapes2);
+		computeNarrowPhaseBetweenShapes(callback, shapes1, shapes2);
 	}
-	
+
 	/// Update a proxy collision shape (that has moved for instance)
-	public void updateProxyCollisionShape(final ProxyShape _shape, final AABB _aabb) {
-		updateProxyCollisionShape(_shape, _aabb, new Vector3f(0, 0, 0), false);
+	public void updateProxyCollisionShape(final ProxyShape shape, final AABB aabb) {
+		updateProxyCollisionShape(shape, aabb, new Vector3f(0, 0, 0), false);
 	}
-	
-	public void updateProxyCollisionShape(final ProxyShape _shape, final AABB _aabb, final Vector3f _displacement) {
-		updateProxyCollisionShape(_shape, _aabb, _displacement, false);
+
+	public void updateProxyCollisionShape(final ProxyShape shape, final AABB aabb, final Vector3f displacement) {
+		updateProxyCollisionShape(shape, aabb, displacement, false);
 	}
-	
-	public void updateProxyCollisionShape(final ProxyShape _shape, final AABB _aabb, final Vector3f _displacement, final boolean _forceReinsert) {
-		this.broadPhaseAlgorithm.updateProxyCollisionShape(_shape, _aabb, _displacement);
+
+	public void updateProxyCollisionShape(
+			final ProxyShape shape,
+			final AABB aabb,
+			final Vector3f displacement,
+			final boolean forceReinsert) {
+		this.broadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
 	}
 }

src/org/atriasoft/ephysics/collision/CollisionShapeInfo.java

@@ -1,30 +1,20 @@
 package org.atriasoft.ephysics.collision;
 
-import org.atriasoft.etk.math.Transform3D;
-
 import org.atriasoft.ephysics.collision.shapes.CollisionShape;
 import org.atriasoft.ephysics.engine.OverlappingPair;
+import org.atriasoft.etk.math.Transform3D;
 
 /**
  *  It regroups different things about a collision shape. This is
  * used to pass information about a collision shape to a collision algorithm.
  */
-public class CollisionShapeInfo {
+@SuppressWarnings("preview")
+public record CollisionShapeInfo(
+		ProxyShape proxyShape, //!< Proxy shape
+		CollisionShape collisionShape, //!< Pointer to the collision shape
+		Transform3D shapeToWorldTransform, //!< Transform3D that maps from collision shape local-space to world-space
+		OverlappingPair overlappingPair, //!< Broadphase overlapping pair
+		Object cachedCollisionData//!< Cached collision data of the proxy shape
+) {
 	
-	public final OverlappingPair overlappingPair; //!< Broadphase overlapping pair
-	public final ProxyShape proxyShape; //!< Proxy shape
-	public final CollisionShape collisionShape; //!< Pointer to the collision shape
-	public final Transform3D shapeToWorldTransform; //!< Transform3D that maps from collision shape local-space to world-space
-	public final Object cachedCollisionData; //!< Cached collision data of the proxy shape
-	/// Constructor
-	
-	public CollisionShapeInfo(final ProxyShape _proxyCollisionShape, final CollisionShape _shape, final Transform3D _shapeLocalToWorldTransform, final OverlappingPair _pair,
-			final Object _cachedData) {
-		this.overlappingPair = _pair;
-		this.proxyShape = _proxyCollisionShape;
-		this.collisionShape = _shape;
-		this.shapeToWorldTransform = _shapeLocalToWorldTransform;
-		this.cachedCollisionData = _cachedData;
-		
-	}
 }

src/org/atriasoft/ephysics/collision/ContactManifold.java

@@ -1,11 +1,3 @@
-/** @file
- * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
- * @author Daniel CHAPPUIS
- * @author Edouard DUPIN
- * @copyright 2010-2016, Daniel Chappuis
- * @copyright 2017, Edouard DUPIN
- * @license MPL v2.0 (see license file)
- */
 
 package org.atriasoft.ephysics.collision;
 
@@ -20,7 +12,7 @@ import org.atriasoft.etk.math.Vector3f;
  * The contact manifold is implemented in a way to cache the contact
  * points among the frames for better stability following the
  * "Contact Generation" presentation of Erwin Coumans at GDC 2010
- * conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf).
+ * conference (bullet.googlecode.com/files/GDC10CoumansErwinContact.pdf).
  * Some code of this class is based on the implementation of the
  * btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org).
  * The contacts between two bodies are added one after the other in the cache.
@@ -32,29 +24,29 @@ import org.atriasoft.etk.math.Vector3f;
 public class ContactManifold {
 	
 	//!< Maximum number of contacts in the manifold
-	public static final int MAX_CONTACT_POINTS_IN_MANIFOLD = 4;;
+	public static final int MAX_CONTACT_POINTS_IN_MANIFOLD = 4;
 	
-	private final ProxyShape shape1; //!< Pointer to the first proxy shape of the contact
+	private final ContactPoint[] contactPoints = new ContactPoint[ContactManifold.MAX_CONTACT_POINTS_IN_MANIFOLD]; //!< Contact points in the manifold
 	
-	private final ProxyShape shape2; //!< Pointer to the second proxy shape of the contact
-	private final ContactPoint[] contactPoints = new ContactPoint[MAX_CONTACT_POINTS_IN_MANIFOLD]; //!< Contact points in the manifold
-	private final int normalDirectionId; //!< Normal direction Id (Unique Id representing the normal direction)
-	private int nbContactPoints; //!< Number of contacts in the cache
-	private Vector3f frictionVector1 = new Vector3f(); //!< First friction vector of the contact manifold
-	private Vector3f frictionvec2 = new Vector3f(); //!< Second friction vector of the contact manifold
 	private float frictionImpulse1; //!< First friction raint accumulated impulse
-	
 	private float frictionImpulse2; //!< Second friction raint accumulated impulse
 	private float frictionTwistImpulse; //!< Twist friction raint accumulated impulse
-	private Vector3f rollingResistanceImpulse; //!< Accumulated rolling resistance impulse
+	private Vector3f frictionvec2 = Vector3f.ZERO; //!< Second friction vector of the contact manifold
+	private Vector3f frictionVector1 = Vector3f.ZERO; //!< First friction vector of the contact manifold
 	public boolean isAlreadyInIsland; //!< True if the contact manifold has already been added longo an island
 	/// Return the index of maximum area
 	/// Constructor
+	private int nbContactPoints; //!< Number of contacts in the cache
 	
-	public ContactManifold(final ProxyShape _shape1, final ProxyShape _shape2, final int _normalDirectionId) {
-		this.shape1 = _shape1;
-		this.shape2 = _shape2;
-		this.normalDirectionId = _normalDirectionId;
+	private final int normalDirectionId; //!< Normal direction Id (Unique Id representing the normal direction)
+	private Vector3f rollingResistanceImpulse; //!< Accumulated rolling resistance impulse
+	private final ProxyShape shape1; //!< Pointer to the first proxy shape of the contact
+	private final ProxyShape shape2; //!< Pointer to the second proxy shape of the contact
+	
+	public ContactManifold(final ProxyShape shape1, final ProxyShape shape2, final int normalDirectionId) {
+		this.shape1 = shape1;
+		this.shape2 = shape2;
+		this.normalDirectionId = normalDirectionId;
 		this.nbContactPoints = 0;
 		this.frictionImpulse1 = 0.0f;
 		this.frictionImpulse2 = 0.0f;
@@ -69,14 +61,14 @@ public class ContactManifold {
 		for (int iii = 0; iii < this.nbContactPoints; iii++) {
 			// Check if the new point point does not correspond to a same contact point
 			// already in the manifold.
-			final float distance = (this.contactPoints[iii].getWorldPointOnBody1().lessNew(contact.getWorldPointOnBody1()).length2());
+			final float distance = (this.contactPoints[iii].getWorldPointOnBody1().less(contact.getWorldPointOnBody1()).length2());
 			if (distance <= Configuration.PERSISTENT_CONTACT_DIST_THRESHOLD * Configuration.PERSISTENT_CONTACT_DIST_THRESHOLD) {
 				assert (this.nbContactPoints > 0);
 				return;
 			}
 		}
 		// If the contact manifold is full
-		if (this.nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
+		if (this.nbContactPoints == ContactManifold.MAX_CONTACT_POINTS_IN_MANIFOLD) {
 			final int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
 			final int indexToRemove = getIndexToRemove(indexMaxPenetration, contact.getLocalPointOnBody1());
 			removeContactPoint(indexToRemove);
@@ -100,11 +92,11 @@ public class ContactManifold {
 		if (this.nbContactPoints == 0) {
 			return new Vector3f(0, 0, 1);
 		}
-		final Vector3f averageNormal = new Vector3f();
+		Vector3f averageNormal = Vector3f.ZERO;
 		for (int iii = 0; iii < this.nbContactPoints; iii++) {
-			averageNormal.add(this.contactPoints[iii].getNormal());
+			averageNormal = averageNormal.add(this.contactPoints[iii].getNormal());
 		}
-		return averageNormal.safeNormalizeNew();
+		return averageNormal.safeNormalize();
 	}
 	
 	/// Return a pointer to the first body of the contact manifold
@@ -155,7 +147,7 @@ public class ContactManifold {
 	 * the new contact is the deepest.
 	 */
 	int getIndexOfDeepestPenetration(final ContactPoint newContact) {
-		assert (this.nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
+		assert (this.nbContactPoints == ContactManifold.MAX_CONTACT_POINTS_IN_MANIFOLD);
 		int indexMaxPenetrationDepth = -1;
 		float maxPenetrationDepth = newContact.getPenetrationDepth();
 		// For each contact in the cache
@@ -183,36 +175,36 @@ public class ContactManifold {
 	 * by Erwin Coumans (http://wwww.bulletphysics.org).
 	 */
 	int getIndexToRemove(final int indexMaxPenetration, final Vector3f newPoint) {
-		assert (this.nbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
+		assert (this.nbContactPoints == ContactManifold.MAX_CONTACT_POINTS_IN_MANIFOLD);
 		float area0 = 0.0f; // Area with contact 1,2,3 and newPoint
 		float area1 = 0.0f; // Area with contact 0,2,3 and newPoint
 		float area2 = 0.0f; // Area with contact 0,1,3 and newPoint
 		float area3 = 0.0f; // Area with contact 0,1,2 and newPoint
 		if (indexMaxPenetration != 0) {
 			// Compute the area
-			final Vector3f vector1 = newPoint.lessNew(this.contactPoints[1].getLocalPointOnBody1());
-			final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().lessNew(this.contactPoints[2].getLocalPointOnBody1());
+			final Vector3f vector1 = newPoint.less(this.contactPoints[1].getLocalPointOnBody1());
+			final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().less(this.contactPoints[2].getLocalPointOnBody1());
 			final Vector3f crossProduct = vector1.cross(vector2);
 			area0 = crossProduct.length2();
 		}
 		if (indexMaxPenetration != 1) {
 			// Compute the area
-			final Vector3f vector1 = newPoint.lessNew(this.contactPoints[0].getLocalPointOnBody1());
-			final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().lessNew(this.contactPoints[2].getLocalPointOnBody1());
+			final Vector3f vector1 = newPoint.less(this.contactPoints[0].getLocalPointOnBody1());
+			final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().less(this.contactPoints[2].getLocalPointOnBody1());
 			final Vector3f crossProduct = vector1.cross(vector2);
 			area1 = crossProduct.length2();
 		}
 		if (indexMaxPenetration != 2) {
 			// Compute the area
-			final Vector3f vector1 = newPoint.lessNew(this.contactPoints[0].getLocalPointOnBody1());
-			final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().lessNew(this.contactPoints[1].getLocalPointOnBody1());
+			final Vector3f vector1 = newPoint.less(this.contactPoints[0].getLocalPointOnBody1());
+			final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().less(this.contactPoints[1].getLocalPointOnBody1());
 			final Vector3f crossProduct = vector1.cross(vector2);
 			area2 = crossProduct.length2();
 		}
 		if (indexMaxPenetration != 3) {
 			// Compute the area
-			final Vector3f vector1 = newPoint.lessNew(this.contactPoints[0].getLocalPointOnBody1());
-			final Vector3f vector2 = this.contactPoints[2].getLocalPointOnBody1().lessNew(this.contactPoints[1].getLocalPointOnBody1());
+			final Vector3f vector1 = newPoint.less(this.contactPoints[0].getLocalPointOnBody1());
+			final Vector3f vector2 = this.contactPoints[2].getLocalPointOnBody1().less(this.contactPoints[1].getLocalPointOnBody1());
 			final Vector3f crossProduct = vector1.cross(vector2);
 			area3 = crossProduct.length2();
 		}
@@ -237,25 +229,24 @@ public class ContactManifold {
 			if (area1 < area2) {
 				if (area2 < area3) {
 					return 3;
-				} else {
-					return 2;
 				}
-			} else if (area1 < area3) {
-				return 3;
-			} else {
-				return 1;
-			}
-		} else if (area0 < area2) {
-			if (area2 < area3) {
-				return 3;
-			} else {
 				return 2;
 			}
-		} else if (area0 < area3) {
-			return 3;
-		} else {
-			return 0;
+			if (area1 < area3) {
+				return 3;
+			}
+			return 1;
 		}
+		if (area0 < area2) {
+			if (area2 < area3) {
+				return 3;
+			}
+			return 2;
+		}
+		if (area0 < area3) {
+			return 3;
+		}
+		return 0;
 	}
 	
 	/// Return the number of contact points in the manifold
@@ -344,10 +335,9 @@ public class ContactManifold {
 		}
 		// Update the world coordinates and penetration depth of the contact points in the manifold
 		for (int iii = 0; iii < this.nbContactPoints; iii++) {
-			this.contactPoints[iii].setWorldPointOnBody1(transform1.multiplyNew(this.contactPoints[iii].getLocalPointOnBody1()));
-			this.contactPoints[iii].setWorldPointOnBody2(transform2.multiplyNew(this.contactPoints[iii].getLocalPointOnBody2()));
-			this.contactPoints[iii]
-					.setPenetrationDepth((this.contactPoints[iii].getWorldPointOnBody1().lessNew(this.contactPoints[iii].getWorldPointOnBody2())).dot(this.contactPoints[iii].getNormal()));
+			this.contactPoints[iii].setWorldPointOnBody1(transform1.multiply(this.contactPoints[iii].getLocalPointOnBody1()));
+			this.contactPoints[iii].setWorldPointOnBody2(transform2.multiply(this.contactPoints[iii].getLocalPointOnBody2()));
+			this.contactPoints[iii].setPenetrationDepth((this.contactPoints[iii].getWorldPointOnBody1().less(this.contactPoints[iii].getWorldPointOnBody2())).dot(this.contactPoints[iii].getNormal()));
 		}
 		final float squarePersistentContactThreshold = Configuration.PERSISTENT_CONTACT_DIST_THRESHOLD * Configuration.PERSISTENT_CONTACT_DIST_THRESHOLD;
 		// Remove the contact points that don't represent very well the contact manifold
@@ -361,8 +351,8 @@ public class ContactManifold {
 			} else {
 				// Compute the distance of the two contact points in the plane
 				// orthogonal to the contact normal
-				final Vector3f projOfPoint1 = this.contactPoints[iii].getNormal().multiplyNew(distanceNormal).add(this.contactPoints[iii].getWorldPointOnBody1());
-				final Vector3f projDifference = this.contactPoints[iii].getWorldPointOnBody2().lessNew(projOfPoint1);
+				final Vector3f projOfPoint1 = this.contactPoints[iii].getNormal().multiply(distanceNormal).add(this.contactPoints[iii].getWorldPointOnBody1());
+				final Vector3f projDifference = this.contactPoints[iii].getWorldPointOnBody2().less(projOfPoint1);
 				// If the orthogonal distance is larger than the valid distance
 				// threshold, we remove the contact
 				if (projDifference.length2() > squarePersistentContactThreshold) {

src/org/atriasoft/ephysics/collision/ContactManifoldListElement.java

@@ -7,8 +7,8 @@ public class ContactManifoldListElement {
 	public ContactManifold contactManifold; //!< Pointer to the actual contact manifold
 	public ContactManifoldListElement next; //!< Next element of the list
 	
-	public ContactManifoldListElement(final ContactManifold _initContactManifold, final ContactManifoldListElement _initNext) {
-		this.contactManifold = _initContactManifold;
-		this.next = _initNext;
+	public ContactManifoldListElement(final ContactManifold initContactManifold, final ContactManifoldListElement initNext) {
+		this.contactManifold = initContactManifold;
+		this.next = initNext;
 	}
 }

src/org/atriasoft/ephysics/collision/ContactManifoldSet.java

@@ -25,12 +25,12 @@ public class ContactManifoldSet {
 	/// Create a new contact manifold and add it to the set
 	
 	/// Constructor
-	public ContactManifoldSet(final ProxyShape _shape1, final ProxyShape _shape2, final int _nbMaxManifolds) {
-		this.nbMaxManifolds = _nbMaxManifolds;
+	public ContactManifoldSet(final ProxyShape shape1, final ProxyShape shape2, final int nbMaxManifolds) {
+		this.nbMaxManifolds = nbMaxManifolds;
 		this.nbManifolds = 0;
-		this.shape1 = _shape1;
-		this.shape2 = _shape2;
-		assert (_nbMaxManifolds >= 1);
+		this.shape1 = shape1;
+		this.shape2 = shape2;
+		assert (nbMaxManifolds >= 1);
 	}
 	
 	/// Add a contact point to the manifold set
@@ -115,20 +115,20 @@ public class ContactManifoldSet {
 		assert (normal.length2() > Constant.FLOAT_EPSILON);
 		int faceNo;
 		float u, v;
-		final float max = FMath.max(FMath.abs(normal.x), FMath.abs(normal.y), FMath.abs(normal.z));
-		final Vector3f normalScaled = normal.divideNew(max);
-		if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
-			faceNo = normalScaled.x > 0 ? 0 : 1;
-			u = normalScaled.y;
-			v = normalScaled.z;
-		} else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) {
-			faceNo = normalScaled.y > 0 ? 2 : 3;
-			u = normalScaled.x;
-			v = normalScaled.z;
+		final float max = FMath.max(FMath.abs(normal.x()), FMath.abs(normal.y()), FMath.abs(normal.z()));
+		final Vector3f normalScaled = normal.divide(max);
+		if (normalScaled.x() >= normalScaled.y() && normalScaled.x() >= normalScaled.z()) {
+			faceNo = normalScaled.x() > 0 ? 0 : 1;
+			u = normalScaled.y();
+			v = normalScaled.z();
+		} else if (normalScaled.y() >= normalScaled.x() && normalScaled.y() >= normalScaled.z()) {
+			faceNo = normalScaled.y() > 0 ? 2 : 3;
+			u = normalScaled.x();
+			v = normalScaled.z();
 		} else {
-			faceNo = normalScaled.z > 0 ? 4 : 5;
-			u = normalScaled.x;
-			v = normalScaled.y;
+			faceNo = normalScaled.z() > 0 ? 4 : 5;
+			u = normalScaled.x();
+			v = normalScaled.y();
 		}
 		int indexU = FMath.floor(((u + 1) / 2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
 		int indexV = FMath.floor(((v + 1) / 2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
@@ -205,8 +205,8 @@ public class ContactManifoldSet {
 	public void update() {
 		for (int iii = this.nbManifolds - 1; iii >= 0; iii--) {
 			// Update the contact manifold
-			this.manifolds[iii].update(this.shape1.getBody().getTransform().multiplyNew(this.shape1.getLocalToBodyTransform()),
-					this.shape2.getBody().getTransform().multiplyNew(this.shape2.getLocalToBodyTransform()));
+			this.manifolds[iii].update(this.shape1.getBody().getTransform().multiply(this.shape1.getLocalToBodyTransform()),
+					this.shape2.getBody().getTransform().multiply(this.shape2.getLocalToBodyTransform()));
 			// Remove the contact manifold if has no contact points anymore
 			if (this.manifolds[iii].getNbContactPoints() == 0) {
 				removeManifold(iii);

src/org/atriasoft/ephysics/collision/ProxyShape.java

@@ -21,7 +21,7 @@ public class ProxyShape {
 	protected Object userData = null; //!< Pointer to user data
 	
 	/**
-	 * @brief Bits used to define the collision category of this shape.
+	 * Bits used to define the collision category of this shape.
 	 * You can set a single bit to one to define a category value for this
 	 * shape. This value is one (0x0001) by default. This variable can be used
 	 * together with the this.collideWithMaskBits variable so that given
@@ -30,7 +30,7 @@ public class ProxyShape {
 	 */
 	protected int collisionCategoryBits = 0x0001;
 	/**
-	 * @brief Bits mask used to state which collision categories this shape can
+	 * Bits mask used to state which collision categories this shape can
 	 * collide with. This value is 0xFFFF by default. It means that this
 	 * proxy shape will collide with every collision categories by default.
 	 */
@@ -96,7 +96,7 @@ public class ProxyShape {
 	
 	/// Return the local to world transform
 	public Transform3D getLocalToWorldTransform() {
-		return this.body.getTransform().multiplyNew(this.localToBodyTransform);
+		return this.body.getTransform().multiply(this.localToBodyTransform);
 	}
 	
 	/// Return the mass of the collision shape
@@ -116,7 +116,7 @@ public class ProxyShape {
 	
 	/**
 	 * @param ray Ray to use for the raycasting
-	 * @param[out] raycastInfo Result of the raycasting that is valid only if the
+	 * @param raycastInfo Result of the raycasting that is valid only if the
 	 *			 methods returned true
 	 * @return True if the ray hit the collision shape
 	 */
@@ -131,14 +131,14 @@ public class ProxyShape {
 		final Transform3D localToWorldTransform = getLocalToWorldTransform();
 		final Transform3D worldToLocalTransform = localToWorldTransform.inverseNew();
 		
-		final Ray rayLocal = new Ray(worldToLocalTransform.multiplyNew(ray.point1), worldToLocalTransform.multiplyNew(ray.point2), ray.maxFraction);
+		final Ray rayLocal = new Ray(worldToLocalTransform.multiply(ray.point1), worldToLocalTransform.multiply(ray.point2), ray.maxFraction);
 		
 		final boolean isHit = this.collisionShape.raycast(rayLocal, raycastInfo, this);
-		if (isHit == true) {
+		if (isHit) {
 			// Convert the raycast info longo world-space
-			raycastInfo.worldPoint = localToWorldTransform.multiplyNew(raycastInfo.worldPoint);
+			raycastInfo.worldPoint = localToWorldTransform.multiply(raycastInfo.worldPoint);
 			raycastInfo.worldNormal = localToWorldTransform.getOrientation().multiply(raycastInfo.worldNormal);
-			raycastInfo.worldNormal.normalize();
+			raycastInfo.worldNormal = raycastInfo.worldNormal.normalize();
 		}
 		return isHit;
 	}
@@ -193,7 +193,7 @@ public class ProxyShape {
 	 * @return True if the point is inside the collision shape
 	 */
 	public boolean testPointInside(final Vector3f worldPoint) {
-		final Transform3D localToWorld = this.body.getTransform().multiplyNew(this.localToBodyTransform);
+		final Transform3D localToWorld = this.body.getTransform().multiply(this.localToBodyTransform);
 		final Vector3f localPoint = localToWorld.inverseNew().multiply(worldPoint);
 		return this.collisionShape.testPointInside(localPoint, this);
 	}

src/org/atriasoft/ephysics/collision/TestCollisionBetweenShapesCallback.java

@@ -10,13 +10,14 @@ public class TestCollisionBetweenShapesCallback implements NarrowPhaseCallback {
 	private final CollisionCallback collisionCallback;
 	
 	// Constructor
-	public TestCollisionBetweenShapesCallback(final CollisionCallback _callback) {
-		this.collisionCallback = _callback;
+	public TestCollisionBetweenShapesCallback(final CollisionCallback callback) {
+		this.collisionCallback = callback;
 		
 	}
 	
 	// Called by a narrow-phase collision algorithm when a new contact has been found
-	public void notifyContact(final OverlappingPair _overlappingPair, final ContactPointInfo _contactInfo) {
-		this.collisionCallback.notifyContact(_contactInfo);
+	@Override
+	public void notifyContact(final OverlappingPair overlappingPair, final ContactPointInfo contactInfo) {
+		this.collisionCallback.notifyContact(contactInfo);
 	}
 }

src/org/atriasoft/ephysics/collision/Triangle.java

@@ -5,8 +5,20 @@ import org.atriasoft.etk.math.Vector3f;
 public class Triangle {
 	public final Vector3f[] value = new Vector3f[3];
 	
-	public Vector3f get(final int _id) {
-		return this.value[_id];
+	public Triangle() {}
+	
+	public Triangle(Vector3f data1, Vector3f data2, Vector3f data3) {
+		this.value[0] = data1;
+		this.value[1] = data2;
+		this.value[2] = data3;
+	}
+	
+	public Vector3f get(final int id) {
+		return this.value[id];
+	}
+	
+	public void set(final int id, Vector3f data) {
+		this.value[id] = data;
 	}
 	
 }

src/org/atriasoft/ephysics/collision/TriangleMesh.java

@@ -15,8 +15,8 @@ public class TriangleMesh {
 	/**
 	 *  Add a subpart of the mesh
 	 */
-	public void addSubpart(final TriangleVertexArray _triangleVertexArray) {
-		this.triangleArrays.add(_triangleVertexArray);
+	public void addSubpart(final TriangleVertexArray triangleVertexArray) {
+		this.triangleArrays.add(triangleVertexArray);
 	}
 	
 	/**
@@ -29,8 +29,8 @@ public class TriangleMesh {
 	/**
 	 *  Get a pointer to a given subpart (triangle vertex array) of the mesh
 	 */
-	public TriangleVertexArray getSubpart(final int _indexSubpart) {
-		assert (_indexSubpart < this.triangleArrays.size());
-		return this.triangleArrays.get(_indexSubpart);
+	public TriangleVertexArray getSubpart(final int indexSubpart) {
+		assert (indexSubpart < this.triangleArrays.size());
+		return this.triangleArrays.get(indexSubpart);
 	}
 }

src/org/atriasoft/ephysics/collision/TriangleVertexArray.java

@@ -14,28 +14,28 @@ import org.atriasoft.etk.math.Vector3f;
  * remains valid during the TriangleVertexArray life.
  */
 public class TriangleVertexArray {
-	/// Vertice list
-	protected final Vector3f[] vertices;
 	/// List of triangle (3 pos for each triangle)
 	protected final int[] triangles;
+	/// Vertice list
+	protected final Vector3f[] vertices;
 	
-	public TriangleVertexArray(final List<Vector3f> _vertices, final List<Integer> _triangles) {
-		this.vertices = _vertices.toArray(new Vector3f[0]);
+	public TriangleVertexArray(final List<Vector3f> vertices, final List<Integer> triangles) {
+		this.vertices = vertices.toArray(new Vector3f[0]);
 		
-		this.triangles = new int[_triangles.size()];
+		this.triangles = new int[triangles.size()];
 		for (int iii = 0; iii < this.triangles.length; iii++) {
-			this.triangles[iii] = _triangles.get(iii);
+			this.triangles[iii] = triangles.get(iii);
 		}
 	}
 	
 	/**
 	 * Constructor
-	 * @param _vertices List Of all vertices
-	 * @param _triangles List of all linked points
+	 * @param vertices List Of all vertices
+	 * @param triangles List of all linked points
 	 */
-	public TriangleVertexArray(final Vector3f[] _vertices, final int[] _triangles) {
-		this.vertices = _vertices;
-		this.triangles = _triangles;
+	public TriangleVertexArray(final Vector3f[] vertices, final int[] triangles) {
+		this.vertices = vertices;
+		this.triangles = triangles;
 	}
 	
 	/**
@@ -66,11 +66,11 @@ public class TriangleVertexArray {
 	 * Get a triangle at the specific ID
 	 * @return Buffer of 3 points
 	 */
-	public Triangle getTriangle(final int _id) {
+	public Triangle getTriangle(final int id) {
 		final Triangle out = new Triangle();
-		out.value[0] = this.vertices[this.triangles[_id * 3]];
-		out.value[1] = this.vertices[this.triangles[_id * 3 + 1]];
-		out.value[2] = this.vertices[this.triangles[_id * 3 + 2]];
+		out.value[0] = this.vertices[this.triangles[id * 3]];
+		out.value[1] = this.vertices[this.triangles[id * 3 + 1]];
+		out.value[2] = this.vertices[this.triangles[id * 3 + 2]];
 		return out;
 	}
 	
@@ -81,4 +81,4 @@ public class TriangleVertexArray {
 	public Vector3f[] getVertices() {
 		return this.vertices;
 	}
-};
+}

src/org/atriasoft/ephysics/collision/broadphase/BroadPhaseAlgorithm.java

@@ -51,33 +51,37 @@ public class BroadPhaseAlgorithm {
 	 */
 	public class BroadPhaseRaycastCallback implements CallbackRaycast {
 		private final DynamicAABBTree dynamicAABBTree;
-		private final int raycastWithCategoryMaskBits;
 		private final RaycastTest raycastTest;
+		private final int raycastWithCategoryMaskBits;
 		
 		// Constructor
-		public BroadPhaseRaycastCallback(final DynamicAABBTree _dynamicAABBTree, final int _raycastWithCategoryMaskBits, final RaycastTest _raycastTest) {
-			this.dynamicAABBTree = _dynamicAABBTree;
-			this.raycastWithCategoryMaskBits = _raycastWithCategoryMaskBits;
-			this.raycastTest = _raycastTest;
+		public BroadPhaseRaycastCallback(final DynamicAABBTree dynamicAABBTree, final int raycastWithCategoryMaskBits, final RaycastTest raycastTest) {
+			this.dynamicAABBTree = dynamicAABBTree;
+			this.raycastWithCategoryMaskBits = raycastWithCategoryMaskBits;
+			this.raycastTest = raycastTest;
 			
 		}
 		
 		@Override
-		public float callback(final DTree _node, final Ray _ray) {
+		public float callback(final DTree node, final Ray ray) {
 			float hitFraction = -1.0f;
 			// Get the proxy shape from the node
-			final ProxyShape proxyShape = (ProxyShape) this.dynamicAABBTree.getNodeDataPointer(_node);
+			final ProxyShape proxyShape = (ProxyShape) this.dynamicAABBTree.getNodeDataPointer(node);
 			// Check if the raycast filtering mask allows raycast against this shape
 			if ((this.raycastWithCategoryMaskBits & proxyShape.getCollisionCategoryBits()) != 0) {
 				// Ask the collision detection to perform a ray cast test against
 				// the proxy shape of this node because the ray is overlapping
 				// with the shape in the broad-phase
-				hitFraction = this.raycastTest.raycastAgainstShape(proxyShape, _ray);
+				hitFraction = this.raycastTest.raycastAgainstShape(proxyShape, ray);
 			}
 			return hitFraction;
 		}
-	};
+	}
 	
+	/**
+	 * Reference to the collision detection object
+	 */
+	protected CollisionDetection collisionDetection; // TODO revoked with generic interface for the callback...
 	/// A tree of data that is separated by a specific distance in AABB model.
 	protected DynamicAABBTree dynamicAABBTree;
 	/** 
@@ -89,32 +93,28 @@ public class BroadPhaseAlgorithm {
 	 * Temporary array of potential overlapping pairs (with potential duplicates)
 	 */
 	protected List<PairDTree> potentialPairs = new ArrayList<>();
-	/**
-	 * Reference to the collision detection object
-	 */
-	protected CollisionDetection collisionDetection; // TODO revoked with generic interface for the callback...
 	
-	public BroadPhaseAlgorithm(final CollisionDetection _collisionDetection) {
+	public BroadPhaseAlgorithm(final CollisionDetection collisionDetection) {
 		this.dynamicAABBTree = new DynamicAABBTree(Configuration.DYNAMIC_TREE_AABB_GAP);
-		this.collisionDetection = _collisionDetection;
+		this.collisionDetection = collisionDetection;
 	}
 	
 	/// Add a collision shape in the array of shapes that have moved in the last simulation step
 	/// and that need to be tested again for broad-phase overlapping.
-	public void addMovedCollisionShape(final DTree _broadPhaseID) {
-		//Log.info("  ** Element that has moved ... = " + _broadPhaseID);
-		this.movedShapes.add(_broadPhaseID);
+	public void addMovedCollisionShape(final DTree broadPhaseID) {
+		//LOGGER.info("  ** Element that has moved ... = " + broadPhaseID);
+		this.movedShapes.add(broadPhaseID);
 	}
 	
 	/// Add a proxy collision shape longo the broad-phase collision detection
-	public void addProxyCollisionShape(final ProxyShape _proxyShape, final AABB _aabb) {
+	public void addProxyCollisionShape(final ProxyShape proxyShape, final AABB aabb) {
 		// Add the collision shape longo the dynamic AABB tree and get its broad-phase ID
-		final DTree nodeId = this.dynamicAABBTree.addObject(_aabb, _proxyShape);
+		final DTree nodeId = this.dynamicAABBTree.addObject(aabb, proxyShape);
 		// Set the broad-phase ID of the proxy shape
-		_proxyShape.setBroadPhaseID(nodeId);
+		proxyShape.setBroadPhaseID(nodeId);
 		// Add the collision shape longo the array of bodies that have moved (or have been created)
 		// during the last simulation step
-		addMovedCollisionShape(_proxyShape.getBroadPhaseID());
+		addMovedCollisionShape(proxyShape.getBroadPhaseID());
 	}
 	
 	/// Compute all the overlapping pairs of collision shapes
@@ -122,10 +122,10 @@ public class BroadPhaseAlgorithm {
 		this.potentialPairs.clear();
 		// For all collision shapes that have moved (or have been created) during the
 		// last simulation step
-		//Log.info("moved shape = " + this.movedShapes.size());
+		//LOGGER.info("moved shape = " + this.movedShapes.size());
 		/*
 		for (final DTree it : this.movedShapes) {
-			Log.info("    - " + it);
+			LOGGER.info("    - " + it);
 		}
 		*/
 		for (final DTree it : this.movedShapes) {
@@ -137,31 +137,31 @@ public class BroadPhaseAlgorithm {
 			// by the dynamic AABB tree for each potential overlapping pair.
 			this.dynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, new CallbackOverlapping() {
 				@Override
-				public void callback(final DTree _nodeId) {
+				public void callback(final DTree nodeId) {
 					// TODO Auto-generated method stub// If both the nodes are the same, we do not create store the overlapping pair
-					if (it == _nodeId) {
+					if (it == nodeId) {
 						return;
 					}
 					// Add the new potential pair longo the array of potential overlapping pairs
-					self.potentialPairs.add(new PairDTree(it, _nodeId));
+					self.potentialPairs.add(new PairDTree(it, nodeId));
 				}
 			});
 		}
-		//Log.print("Find potential pair : " + this.potentialPairs.size());
+		//LOGGER.print("Find potential pair : " + this.potentialPairs.size());
 		// Reset the array of collision shapes that have move (or have been created) during the last simulation step
 		this.movedShapes.clear();
 		// Sort the array of potential overlapping pairs in order to remove duplicate pairs
 		this.potentialPairs.sort(new Comparator<PairDTree>() {
 			@Override
-			public int compare(final PairDTree _pair1, final PairDTree _pair2) {
-				if (_pair1.first.uid < _pair2.first.uid) {
+			public int compare(final PairDTree pair1, final PairDTree pair2) {
+				if (pair1.first().uid < pair2.first().uid) {
 					return -1;
 				}
-				if (_pair1.first.uid == _pair2.first.uid) {
-					if (_pair1.second.uid < _pair2.second.uid) {
+				if (pair1.first().uid == pair2.first().uid) {
+					if (pair1.second().uid < pair2.second().uid) {
 						return -1;
 					}
-					if (_pair1.second.uid == _pair2.second.uid) {
+					if (pair1.second().uid == pair2.second().uid) {
 						return 0;
 					}
 					return +1;
@@ -177,8 +177,8 @@ public class BroadPhaseAlgorithm {
 			final PairDTree pair = this.potentialPairs.get(iii);
 			++iii;
 			// Get the two collision shapes of the pair
-			final ProxyShape shape1 = (ProxyShape) (this.dynamicAABBTree.getNodeDataPointer(pair.first));
-			final ProxyShape shape2 = (ProxyShape) (this.dynamicAABBTree.getNodeDataPointer(pair.second));
+			final ProxyShape shape1 = (ProxyShape) (this.dynamicAABBTree.getNodeDataPointer(pair.first()));
+			final ProxyShape shape2 = (ProxyShape) (this.dynamicAABBTree.getNodeDataPointer(pair.second()));
 			// Notify the collision detection about the overlapping pair
 			this.collisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
 			// Skip the duplicate overlapping pairs
@@ -187,7 +187,7 @@ public class BroadPhaseAlgorithm {
 				final PairDTree nextPair = this.potentialPairs.get(iii);
 				// If the next pair is different from the previous one, we stop skipping pairs
 				// TODO check if it work without uid ...
-				if (nextPair.first.uid != pair.first.uid || nextPair.second.uid != pair.second.uid) {
+				if (nextPair.first().uid != pair.first().uid || nextPair.second().uid != pair.second().uid) {
 					break;
 				}
 				++iii;
@@ -196,26 +196,26 @@ public class BroadPhaseAlgorithm {
 	}
 	
 	/// Ray casting method
-	public void raycast(final Ray _ray, final RaycastTest _raycastTest, final int _raycastWithCategoryMaskBits) {
-		final BroadPhaseRaycastCallback broadPhaseRaycastCallback = new BroadPhaseRaycastCallback(this.dynamicAABBTree, _raycastWithCategoryMaskBits, _raycastTest);
-		this.dynamicAABBTree.raycast(_ray, broadPhaseRaycastCallback);
+	public void raycast(final Ray ray, final RaycastTest raycastTest, final int raycastWithCategoryMaskBits) {
+		final BroadPhaseRaycastCallback broadPhaseRaycastCallback = new BroadPhaseRaycastCallback(this.dynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
+		this.dynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
 	}
 	
 	/// Remove a collision shape from the array of shapes that have moved in the last simulation
 	/// step and that need to be tested again for broad-phase overlapping.
-	public void removeMovedCollisionShape(final DTree _broadPhaseID) {
+	public void removeMovedCollisionShape(final DTree broadPhaseID) {
 		final Iterator<DTree> it = this.movedShapes.iterator();
 		while (it.hasNext()) {
 			final DTree elem = it.next();
-			if (elem == _broadPhaseID) {
+			if (elem == broadPhaseID) {
 				it.remove();
 			}
 		}
 	}
 	
 	/// Remove a proxy collision shape from the broad-phase collision detection
-	public void removeProxyCollisionShape(final ProxyShape _proxyShape) {
-		final DTree broadPhaseID = _proxyShape.getBroadPhaseID();
+	public void removeProxyCollisionShape(final ProxyShape proxyShape) {
+		final DTree broadPhaseID = proxyShape.getBroadPhaseID();
 		// Remove the collision shape from the dynamic AABB tree
 		this.dynamicAABBTree.removeObject(broadPhaseID);
 		// Remove the collision shape longo the array of shapes that have moved (or have been created)
@@ -224,28 +224,28 @@ public class BroadPhaseAlgorithm {
 	}
 	
 	/// Return true if the two broad-phase collision shapes are overlapping
-	public boolean testOverlappingShapes(final ProxyShape _shape1, final ProxyShape _shape2) {
-		if (_shape1 == _shape2) {
+	public boolean testOverlappingShapes(final ProxyShape shape1, final ProxyShape shape2) {
+		if (shape1 == shape2) {
 			return false;
 		}
 		// Get the two AABBs of the collision shapes
-		final AABB aabb1 = this.dynamicAABBTree.getFatAABB(_shape1.getBroadPhaseID());
-		final AABB aabb2 = this.dynamicAABBTree.getFatAABB(_shape2.getBroadPhaseID());
+		final AABB aabb1 = this.dynamicAABBTree.getFatAABB(shape1.getBroadPhaseID());
+		final AABB aabb2 = this.dynamicAABBTree.getFatAABB(shape2.getBroadPhaseID());
 		// Check if the two AABBs are overlapping
 		return aabb1.testCollision(aabb2);
 	}
 	
 	/// Notify the broad-phase that a collision shape has moved and need to be updated
-	public void updateProxyCollisionShape(final ProxyShape _proxyShape, final AABB _aabb, final Vector3f _displacement) {
-		updateProxyCollisionShape(_proxyShape, _aabb, _displacement, false);
+	public void updateProxyCollisionShape(final ProxyShape proxyShape, final AABB aabb, final Vector3f displacement) {
+		updateProxyCollisionShape(proxyShape, aabb, displacement, false);
 	}
 	
-	public void updateProxyCollisionShape(final ProxyShape _proxyShape, final AABB _aabb, final Vector3f _displacement, final boolean _forceReinsert) {
-		final DTree broadPhaseID = _proxyShape.getBroadPhaseID();
+	public void updateProxyCollisionShape(final ProxyShape proxyShape, final AABB aabb, final Vector3f displacement, final boolean forceReinsert) {
+		final DTree broadPhaseID = proxyShape.getBroadPhaseID();
 		// Update the dynamic AABB tree according to the movement of the collision shape
-		final boolean hasBeenReInserted = this.dynamicAABBTree.updateObject(broadPhaseID, _aabb, _displacement, _forceReinsert);
+		final boolean hasBeenReInserted = this.dynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert);
 		// If the collision shape has moved out of its fat AABB (and therefore has been reinserted
-		//Log.error("                             ==> hasBeenReInserted = " + hasBeenReInserted);
+		//LOGGER.error("                             ==> hasBeenReInserted = " + hasBeenReInserted);
 		// longo the tree).
 		if (hasBeenReInserted) {
 			// Add the collision shape longo the array of shapes that have moved (or have been created)
@@ -253,4 +253,4 @@ public class BroadPhaseAlgorithm {
 			addMovedCollisionShape(broadPhaseID);
 		}
 	}
-};
+}

src/org/atriasoft/ephysics/collision/broadphase/CallbackOverlapping.java

@@ -1,5 +1,5 @@
 package org.atriasoft.ephysics.collision.broadphase;
 
 public interface CallbackOverlapping {
-	public void callback(DTree _nodeId);
-};
+	public void callback(DTree nodeId);
+}

src/org/atriasoft/ephysics/collision/broadphase/CallbackRaycast.java

@@ -3,5 +3,5 @@ package org.atriasoft.ephysics.collision.broadphase;
 import org.atriasoft.ephysics.mathematics.Ray;
 
 public interface CallbackRaycast {
-	public float callback(DTree _node, Ray _ray);
+	public float callback(DTree node, Ray ray);
 }

src/org/atriasoft/ephysics/collision/broadphase/DTreeLeafInt.java

@@ -1,13 +1,12 @@
 package org.atriasoft.ephysics.collision.broadphase;
 
 class DTreeLeafInt extends DTree {
-	int dataInt_0 = 0;
-	int dataInt_1 = 0;
+	int dataInt0 = 0;
+	int dataInt1 = 0;
 	
-	public DTreeLeafInt(final int dataInt_0, final int dataInt_1) {
-		super();
-		this.dataInt_0 = dataInt_0;
-		this.dataInt_1 = dataInt_1;
+	public DTreeLeafInt(final int dataInt0, final int dataInt1) {
+		this.dataInt0 = dataInt0;
+		this.dataInt1 = dataInt1;
 	}
 	
 	@Override

src/org/atriasoft/ephysics/collision/broadphase/DTreeNode.java

@@ -1,6 +1,6 @@
 package org.atriasoft.ephysics.collision.broadphase;
 
 class DTreeNode extends DTree {
-	DTree children_left = null; //!< Left child of the node
-	DTree children_right = null; //!< Right child of the node
+	DTree childrenleft = null; //!< Left child of the node
+	DTree childrenright = null; //!< Right child of the node
 }

src/org/atriasoft/ephysics/collision/broadphase/DynamicAABBTree.java

@@ -5,10 +5,11 @@ import java.util.Stack;
 
 import org.atriasoft.ephysics.Configuration;
 import org.atriasoft.ephysics.collision.shapes.AABB;
-import org.atriasoft.ephysics.internal.Log;
 import org.atriasoft.ephysics.mathematics.Ray;
 import org.atriasoft.etk.math.FMath;
 import org.atriasoft.etk.math.Vector3f;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 /**
  *  It implements a dynamic AABB tree that is used for broad-phase
@@ -18,12 +19,13 @@ import org.atriasoft.etk.math.Vector3f;
  * "Introduction to Game Physics with Box2D" by Ian Parberry.
  */
 public class DynamicAABBTree {
-	
-	private DTree rootNode; //!< Pointer to the memory location of the nodes of the tree
+	static final Logger LOGGER = LoggerFactory.getLogger(DynamicAABBTree.class);
 	
 	private final float extraAABBGap; //!< Extra AABB Gap used to allow the collision shape to move a little bit without triggering a large modification of the tree which can be costly
 	/// Allocate and return a node to use in the tree
 	
+	private DTree rootNode; //!< Pointer to the memory location of the nodes of the tree
+	
 	/// Constructor
 	public DynamicAABBTree() {
 		this(0.0f);
@@ -51,8 +53,8 @@ public class DynamicAABBTree {
 	/// Internally add an object into the tree
 	private void addObjectInternal(final AABB aabb, final DTree leafNode) {
 		// Create the fat aabb to use in the tree
-		leafNode.aabb.setMin(aabb.getMin().lessNew(this.extraAABBGap));
-		leafNode.aabb.setMax(aabb.getMax().addNew(this.extraAABBGap));
+		leafNode.aabb.setMin(aabb.getMin().less(this.extraAABBGap));
+		leafNode.aabb.setMax(aabb.getMax().add(this.extraAABBGap));
 		// Set the height of the node in the tree
 		leafNode.height = 0;
 		// Insert the new leaf node in the tree
@@ -61,35 +63,35 @@ public class DynamicAABBTree {
 	}
 	
 	/// Balance the sub-tree of a given node using left or right rotations.
-	private DTree balanceSubTreeAtNode(final DTree _node) {
-		assert (_node != null);
+	private DTree balanceSubTreeAtNode(final DTree node) {
+		assert (node != null);
 		// If the node is a leaf or the height of A's sub-tree is less than 2
-		if (_node.isLeaf() || _node.height < 2) {
+		if (node.isLeaf() || node.height < 2) {
 			// Do not perform any rotation
-			return _node;
+			return node;
 		}
-		final DTreeNode nodeA = (DTreeNode) _node;
+		final DTreeNode nodeA = (DTreeNode) node;
 		// Get the two children nodes
-		final DTree nodeB = nodeA.children_left;
-		final DTree nodeC = nodeA.children_right;
+		final DTree nodeB = nodeA.childrenleft;
+		final DTree nodeC = nodeA.childrenright;
 		// Compute the factor of the left and right sub-trees
 		final int balanceFactor = nodeC.height - nodeB.height;
 		// If the right node C is 2 higher than left node B
 		if (balanceFactor > 1) {
 			assert (!nodeC.isLeaf());
 			final DTreeNode nodeCTree = (DTreeNode) nodeC;
-			final DTree nodeF = nodeCTree.children_left;
-			final DTree nodeG = nodeCTree.children_right;
-			nodeCTree.children_left = _node;
+			final DTree nodeF = nodeCTree.childrenleft;
+			final DTree nodeG = nodeCTree.childrenright;
+			nodeCTree.childrenleft = node;
 			nodeCTree.parent = nodeA.parent;
-			nodeA.parent = new WeakReference<DTree>(nodeC);
+			nodeA.parent = new WeakReference<>(nodeC);
 			if (nodeC.parent != null) {
 				final DTreeNode nodeCParent = (DTreeNode) nodeC.parent.get();
-				if (nodeCParent.children_left == _node) {
-					nodeCParent.children_left = nodeC;
+				if (nodeCParent.childrenleft == node) {
+					nodeCParent.childrenleft = nodeC;
 				} else {
-					assert (nodeCParent.children_right == _node);
-					nodeCParent.children_right = nodeC;
+					assert (nodeCParent.childrenright == node);
+					nodeCParent.childrenright = nodeC;
 				}
 			} else {
 				this.rootNode = nodeC;
@@ -98,9 +100,9 @@ public class DynamicAABBTree {
 			assert (!nodeA.isLeaf());
 			// If the right node C was higher than left node B because of the F node
 			if (nodeF.height > nodeG.height) {
-				nodeCTree.children_right = nodeF;
-				nodeA.children_right = nodeG;
-				nodeG.parent = new WeakReference<DTree>(_node);
+				nodeCTree.childrenright = nodeF;
+				nodeA.childrenright = nodeG;
+				nodeG.parent = new WeakReference<>(node);
 				// Recompute the AABB of node A and C
 				nodeA.aabb.mergeTwoAABBs(nodeB.aabb, nodeG.aabb);
 				nodeC.aabb.mergeTwoAABBs(nodeA.aabb, nodeF.aabb);
@@ -111,9 +113,9 @@ public class DynamicAABBTree {
 				assert (nodeC.height > 0);
 			} else {
 				// If the right node C was higher than left node B because of node G
-				nodeCTree.children_right = nodeG;
-				nodeA.children_right = nodeF;
-				nodeF.parent = new WeakReference<DTree>(_node);
+				nodeCTree.childrenright = nodeG;
+				nodeA.childrenright = nodeF;
+				nodeF.parent = new WeakReference<>(node);
 				// Recompute the AABB of node A and C
 				nodeA.aabb.mergeTwoAABBs(nodeB.aabb, nodeF.aabb);
 				nodeC.aabb.mergeTwoAABBs(nodeA.aabb, nodeG.aabb);
@@ -130,18 +132,18 @@ public class DynamicAABBTree {
 		if (balanceFactor < -1) {
 			assert (!nodeB.isLeaf());
 			final DTreeNode nodeBTree = (DTreeNode) nodeB;
-			final DTree nodeF = nodeBTree.children_left;
-			final DTree nodeG = nodeBTree.children_right;
-			nodeBTree.children_left = _node;
+			final DTree nodeF = nodeBTree.childrenleft;
+			final DTree nodeG = nodeBTree.childrenright;
+			nodeBTree.childrenleft = node;
 			nodeB.parent = nodeA.parent;
-			nodeA.parent = new WeakReference<DTree>(nodeB);
+			nodeA.parent = new WeakReference<>(nodeB);
 			if (nodeB.parent != null) {
 				final DTreeNode nodeBParent = (DTreeNode) nodeB.parent.get();
-				if (nodeBParent.children_left == _node) {
-					nodeBParent.children_left = nodeB;
+				if (nodeBParent.childrenleft == node) {
+					nodeBParent.childrenleft = nodeB;
 				} else {
-					assert (nodeBParent.children_right == _node);
-					nodeBParent.children_right = nodeB;
+					assert (nodeBParent.childrenright == node);
+					nodeBParent.childrenright = nodeB;
 				}
 			} else {
 				this.rootNode = nodeB;
@@ -150,9 +152,9 @@ public class DynamicAABBTree {
 			assert (!nodeA.isLeaf());
 			// If the left node B was higher than right node C because of the F node
 			if (nodeF.height > nodeG.height) {
-				nodeBTree.children_right = nodeF;
-				nodeA.children_left = nodeG;
-				nodeG.parent = new WeakReference<DTree>(_node);
+				nodeBTree.childrenright = nodeF;
+				nodeA.childrenleft = nodeG;
+				nodeG.parent = new WeakReference<>(node);
 				// Recompute the AABB of node A and B
 				nodeA.aabb.mergeTwoAABBs(nodeC.aabb, nodeG.aabb);
 				nodeB.aabb.mergeTwoAABBs(nodeA.aabb, nodeF.aabb);
@@ -163,9 +165,9 @@ public class DynamicAABBTree {
 				assert (nodeB.height > 0);
 			} else {
 				// If the left node B was higher than right node C because of node G
-				nodeBTree.children_right = nodeG;
-				nodeA.children_left = nodeF;
-				nodeF.parent = new WeakReference<DTree>(_node);
+				nodeBTree.childrenright = nodeG;
+				nodeA.childrenleft = nodeF;
+				nodeF.parent = new WeakReference<>(node);
 				// Recompute the AABB of node A and B
 				nodeA.aabb.mergeTwoAABBs(nodeC.aabb, nodeF.aabb);
 				nodeB.aabb.mergeTwoAABBs(nodeA.aabb, nodeG.aabb);
@@ -179,7 +181,7 @@ public class DynamicAABBTree {
 			return nodeB;
 		}
 		// If the sub-tree is balanced, return the current root node
-		return _node;
+		return node;
 	}
 	
 	/// Compute the height of the tree
@@ -188,16 +190,16 @@ public class DynamicAABBTree {
 	}
 	
 	/// Compute the height of a given node in the tree
-	private int computeHeight(final DTree _node) {
+	private int computeHeight(final DTree node) {
 		// If the node is a leaf, its height is zero
-		if (_node.isLeaf()) {
+		if (node.isLeaf()) {
 			return 0;
 		}
-		final DTreeNode nodeTree = (DTreeNode) _node;
+		final DTreeNode nodeTree = (DTreeNode) node;
 		
 		// Compute the height of the left and right sub-tree
-		final int leftHeight = computeHeight(nodeTree.children_left);
-		final int rightHeight = computeHeight(nodeTree.children_right);
+		final int leftHeight = computeHeight(nodeTree.childrenleft);
+		final int rightHeight = computeHeight(nodeTree.childrenright);
 		// Return the height of the node
 		return 1 + Math.max(leftHeight, rightHeight);
 	}
@@ -207,14 +209,14 @@ public class DynamicAABBTree {
 		return node.aabb;
 	}
 	
-	public int getNodeDataInt_0(final DTree node) {
+	public int getNodeDataInt0(final DTree node) {
 		assert (node.isLeaf());
-		return ((DTreeLeafInt) node).dataInt_0;
+		return ((DTreeLeafInt) node).dataInt0;
 	}
 	
-	public int getNodeDataInt_1(final DTree node) {
+	public int getNodeDataInt1(final DTree node) {
 		assert (node.isLeaf());
-		return ((DTreeLeafInt) node).dataInt_1;
+		return ((DTreeLeafInt) node).dataInt1;
 	}
 	
 	/// Return the data pointer of a given leaf node of the tree
@@ -234,19 +236,19 @@ public class DynamicAABBTree {
 	}
 	
 	/// Insert a leaf node in the tree
-	private void insertLeafNode(final DTree _node) {
+	private void insertLeafNode(final DTree inNode) {
 		// If the tree is empty
 		if (this.rootNode == null) {
-			this.rootNode = _node;
+			this.rootNode = inNode;
 			return;
 		}
 		// Find the best sibling node for the new node
-		final AABB newNodeAABB = _node.aabb;
+		final AABB newNodeAABB = inNode.aabb;
 		DTree currentNode = this.rootNode;
 		while (!currentNode.isLeaf()) {
 			final DTreeNode node = (DTreeNode) currentNode;
-			final DTree leftChild = node.children_left;
-			final DTree rightChild = node.children_right;
+			final DTree leftChild = node.childrenleft;
+			final DTree rightChild = node.childrenright;
 			// Compute the merged AABB
 			final float volumeAABB = currentNode.aabb.getVolume();
 			final AABB mergedAABBs = new AABB();
@@ -299,20 +301,20 @@ public class DynamicAABBTree {
 		if (oldParentNode != null) {
 			// replace in parent the child with the new child
 			final DTreeNode parentNode = (DTreeNode) oldParentNode.get();
-			if (parentNode.children_left == siblingNode) {
-				parentNode.children_left = newParentNode;
+			if (parentNode.childrenleft == siblingNode) {
+				parentNode.childrenleft = newParentNode;
 			} else {
-				parentNode.children_right = newParentNode;
+				parentNode.childrenright = newParentNode;
 			}
 		} else {
 			// If the sibling node was the root node
 			this.rootNode = newParentNode;
 		}
 		// setup the children
-		newParentNode.children_left = siblingNode;
-		newParentNode.children_right = _node;
-		siblingNode.parent = new WeakReference<DTree>(newParentNode);
-		_node.parent = new WeakReference<DTree>(newParentNode);
+		newParentNode.childrenleft = siblingNode;
+		newParentNode.childrenright = inNode;
+		siblingNode.parent = new WeakReference<>(newParentNode);
+		inNode.parent = new WeakReference<>(newParentNode);
 		
 		// Move up in the tree to change the AABBs that have changed
 		currentNode = newParentNode;
@@ -320,11 +322,11 @@ public class DynamicAABBTree {
 		while (currentNode != null) {
 			// Balance the sub-tree of the current node if it is not balanced
 			currentNode = balanceSubTreeAtNode(currentNode);
-			assert (_node.isLeaf());
+			assert (inNode.isLeaf());
 			assert (!currentNode.isLeaf());
 			final DTreeNode nodeDouble = (DTreeNode) currentNode;
-			final DTree leftChild = nodeDouble.children_left;
-			final DTree rightChild = nodeDouble.children_right;
+			final DTree leftChild = nodeDouble.childrenleft;
+			final DTree rightChild = nodeDouble.childrenright;
 			assert (leftChild != null);
 			assert (rightChild != null);
 			// Recompute the height of the node in the tree
@@ -338,18 +340,18 @@ public class DynamicAABBTree {
 				currentNode = null;
 			}
 		}
-		assert (_node.isLeaf());
+		assert (inNode.isLeaf());
 	}
 	
 	/// Ray casting method
-	public void raycast(final Ray _ray, final CallbackRaycast _callback) {
-		if (_callback == null) {
-			Log.error("call with null callback");
+	public void raycast(final Ray ray, final CallbackRaycast callback) {
+		if (callback == null) {
+			LOGGER.error("call with null callback");
 			return;
 		}
-		float maxFraction = _ray.maxFraction;
+		float maxFraction = ray.maxFraction;
 		// 128 max
-		final Stack<DTree> stack = new Stack<DTree>();
+		final Stack<DTree> stack = new Stack<>();
 		stack.push(this.rootNode);
 		// Walk through the tree from the root looking for proxy shapes
 		// that overlap with the ray AABB
@@ -360,15 +362,15 @@ public class DynamicAABBTree {
 			if (node == null) {
 				continue;
 			}
-			final Ray rayTemp = new Ray(_ray.point1, _ray.point2, maxFraction);
+			final Ray rayTemp = new Ray(ray.point1, ray.point2, maxFraction);
 			// Test if the ray intersects with the current node AABB
-			if (node.aabb.testRayIntersect(rayTemp) == false) {
+			if (!node.aabb.testRayIntersect(rayTemp)) {
 				continue;
 			}
 			// If the node is a leaf of the tree
 			if (node.isLeaf()) {
 				// Call the callback that will raycast again the broad-phase shape
-				final float hitFraction = _callback.callback(node, rayTemp);
+				final float hitFraction = callback.callback(node, rayTemp);
 				// If the user returned a hitFraction of zero, it means that
 				// the raycasting should stop here
 				if (hitFraction == 0.0f) {
@@ -387,34 +389,34 @@ public class DynamicAABBTree {
 			} else { // If the node has children
 				final DTreeNode tmpNode = (DTreeNode) node;
 				// Push its children in the stack of nodes to explore
-				stack.push(tmpNode.children_left);
-				stack.push(tmpNode.children_right);
+				stack.push(tmpNode.childrenleft);
+				stack.push(tmpNode.childrenright);
 			}
 		}
 		
 	}
 	
 	/// Release a node
-	private void releaseNode(final DTree _node) {
+	private void releaseNode(final DTree node) {
 		//this.numberNodes--;
 	}
 	
 	/// Remove a leaf node from the tree
-	private void removeLeafNode(final DTree _node) {
-		assert (_node.isLeaf());
+	private void removeLeafNode(final DTree node) {
+		assert (node.isLeaf());
 		// If we are removing the root node (root node is a leaf in this case)
-		if (this.rootNode == _node) {
+		if (this.rootNode == node) {
 			this.rootNode = null;
 			return;
 		}
 		// parent can not be null.
-		final DTreeNode parentNode = (DTreeNode) _node.parent.get();
+		final DTreeNode parentNode = (DTreeNode) node.parent.get();
 		final WeakReference<DTree> grandParentNodeWeak = parentNode.parent;
 		DTree siblingNode;
-		if (parentNode.children_left == _node) {
-			siblingNode = parentNode.children_right;
+		if (parentNode.childrenleft == node) {
+			siblingNode = parentNode.childrenright;
 		} else {
-			siblingNode = parentNode.children_left;
+			siblingNode = parentNode.childrenleft;
 		}
 		// If the parent of the node to remove is not the root node
 		if (grandParentNodeWeak == null) {
@@ -426,10 +428,10 @@ public class DynamicAABBTree {
 		} else {
 			final DTreeNode grandParentNode = (DTreeNode) grandParentNodeWeak.get();
 			// Destroy the parent node
-			if (grandParentNode.children_left == parentNode) {
-				grandParentNode.children_left = siblingNode;
+			if (grandParentNode.childrenleft == parentNode) {
+				grandParentNode.childrenleft = siblingNode;
 			} else {
-				grandParentNode.children_right = siblingNode;
+				grandParentNode.childrenright = siblingNode;
 			}
 			siblingNode.parent = parentNode.parent;
 			releaseNode(parentNode);
@@ -442,8 +444,8 @@ public class DynamicAABBTree {
 				assert (!currentNode.isLeaf());
 				final DTreeNode currentTreeNode = (DTreeNode) currentNode;
 				// Get the two children of the current node
-				final DTree leftChild = currentTreeNode.children_left;
-				final DTree rightChild = currentTreeNode.children_right;
+				final DTree leftChild = currentTreeNode.childrenleft;
+				final DTree rightChild = currentTreeNode.childrenright;
 				// Recompute the AABB and the height of the current node
 				currentNode.aabb.mergeTwoAABBs(leftChild.aabb, rightChild.aabb);
 				currentNode.height = FMath.max(leftChild.height, rightChild.height) + 1;
@@ -458,25 +460,25 @@ public class DynamicAABBTree {
 	}
 	
 	/// Remove an object from the tree
-	public void removeObject(final DTree _node) {
-		assert (_node.isLeaf());
+	public void removeObject(final DTree node) {
+		assert (node.isLeaf());
 		// Remove the node from the tree
-		removeLeafNode(_node);
-		releaseNode(_node);
+		removeLeafNode(node);
+		releaseNode(node);
 	}
 	
 	/// Report all shapes overlapping with the AABB given in parameter.
-	public void reportAllShapesOverlappingWithAABB(final AABB _aabb, final CallbackOverlapping _callback) {
-		if (_callback == null) {
-			Log.error("call with null callback");
+	public void reportAllShapesOverlappingWithAABB(final AABB aabb, final CallbackOverlapping callback) {
+		if (callback == null) {
+			LOGGER.error("call with null callback");
 			return;
 		}
-		//Log.error("reportAllShapesOverlappingWithAABB");
+		//LOGGER.error("reportAllShapesOverlappingWithAABB");
 		// Create a stack with the nodes to visit
-		final Stack<DTree> stack = new Stack<DTree>();
+		final Stack<DTree> stack = new Stack<>();
 		// 64 max
 		stack.push(this.rootNode);
-		//Log.error("    add stack: " + this.rootNode);
+		//LOGGER.error("    add stack: " + this.rootNode);
 		// While there are still nodes to visit
 		while (stack.size() > 0) {
 			// Get the next node ID to visit
@@ -487,26 +489,26 @@ public class DynamicAABBTree {
 			}
 			// Get the corresponding node
 			final DTree nodeToVisit = nodeIDToVisit;
-			//Log.error("      check colision: " + nodeIDToVisit);
+			//LOGGER.error("      check colision: " + nodeIDToVisit);
 			// If the AABB in parameter overlaps with the AABB of the node to visit
-			if (_aabb.testCollision(nodeToVisit.aabb)) {
+			if (aabb.testCollision(nodeToVisit.aabb)) {
 				// If the node is a leaf
 				if (nodeToVisit.isLeaf()) {
 					/*
-					if (_aabb != nodeToVisit.aabb) {
-						Log.error("           ======> Real collision ...");
+					if (aabb != nodeToVisit.aabb) {
+						LOGGER.error("           ======> Real collision ...");
 					}
 					*/
 					// Notify the broad-phase about a new potential overlapping pair
-					_callback.callback(nodeIDToVisit);
+					callback.callback(nodeIDToVisit);
 				} else {
 					final DTreeNode tmp = (DTreeNode) nodeToVisit;
 					// If the node is not a leaf
 					// We need to visit its children
-					stack.push(tmp.children_left);
-					stack.push(tmp.children_right);
-					//Log.error("    add stack: " + tmp.children_left);
-					//Log.error("    add stack: " + tmp.children_right);
+					stack.push(tmp.childrenleft);
+					stack.push(tmp.childrenright);
+					//LOGGER.error("    add stack: " + tmp.childrenleft);
+					//LOGGER.error("    add stack: " + tmp.childrenright);
 				}
 			}
 		}
@@ -525,51 +527,63 @@ public class DynamicAABBTree {
 	/// argument is the linear velocity of the AABB multiplied by the elapsed time between two
 	/// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node
 	/// (this can be useful if the shape AABB has become much smaller than the previous one for instance).
-	public boolean updateObject(final DTree _node, final AABB _newAABB, final Vector3f _displacement) {
-		return updateObject(_node, _newAABB, _displacement, false);
+	public boolean updateObject(final DTree node, final AABB newAABB, final Vector3f displacement) {
+		return updateObject(node, newAABB, displacement, false);
 	}
 	
-	public boolean updateObject(final DTree _node, final AABB _newAABB, final Vector3f _displacement, final boolean _forceReinsert) {
-		assert (_node.isLeaf());
-		assert (_node.height >= 0);
-		//Log.verbose(" compare : " + _node.aabb.getMin() + " " + _node.aabb.getMax());
-		//Log.verbose("         : " + _newAABB.getMin() + " " + _newAABB.getMax());
+	public boolean updateObject(
+			final DTree node,
+			final AABB newAABB,
+			final Vector3f displacement,
+			final boolean forceReinsert) {
+		assert (node.isLeaf());
+		assert (node.height >= 0);
+		//LOGGER.verbose(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax());
+		//LOGGER.verbose("         : " + newAABB.getMin() + " " + newAABB.getMax());
 		// If the new AABB is still inside the fat AABB of the node
-		if (_forceReinsert == false && _node.aabb.contains(_newAABB)) {
+		if (!forceReinsert && node.aabb.contains(newAABB)) {
 			return false;
 		}
 		// If the new AABB is outside the fat AABB, we remove the corresponding node
-		removeLeafNode(_node);
+		removeLeafNode(node);
 		// Compute the fat AABB by inflating the AABB with a ant gap
-		_node.aabb = _newAABB;
+		node.aabb = newAABB;
 		
 		final Vector3f gap = new Vector3f(this.extraAABBGap, this.extraAABBGap, this.extraAABBGap);
-		_node.aabb.getMin().less(gap);
-		_node.aabb.getMax().add(gap);
+		node.aabb.setMin(node.aabb.getMin().less(gap));
+		node.aabb.setMax(node.aabb.getMax().add(gap));
 		// Inflate the fat AABB in direction of the linear motion of the AABB
-		if (_displacement.x < 0.0f) {
-			_node.aabb.getMin().setX(_node.aabb.getMin().x + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.x);
+		float xmin = node.aabb.getMin().x();
+		float ymin = node.aabb.getMin().y();
+		float zmin = node.aabb.getMin().z();
+		float xmax = node.aabb.getMax().x();
+		float ymax = node.aabb.getMax().y();
+		float zmax = node.aabb.getMax().z();
+		if (displacement.x() < 0.0f) {
+			xmin = node.aabb.getMin().x() + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * displacement.x();
 		} else {
-			_node.aabb.getMax().setX(_node.aabb.getMax().x + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.x);
+			xmax = node.aabb.getMax().x() + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * displacement.x();
 		}
-		if (_displacement.y < 0.0f) {
-			_node.aabb.getMin().setY(_node.aabb.getMin().y + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.y);
+		if (displacement.y() < 0.0f) {
+			ymin = node.aabb.getMin().y() + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * displacement.y();
 		} else {
-			_node.aabb.getMax().setY(_node.aabb.getMax().y + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.y);
+			ymax = node.aabb.getMax().y() + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * displacement.y();
 		}
-		if (_displacement.z < 0.0f) {
-			_node.aabb.getMin().setZ(_node.aabb.getMin().z + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.z);
+		if (displacement.z() < 0.0f) {
+			zmin = node.aabb.getMin().z() + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * displacement.z();
 		} else {
-			_node.aabb.getMax().setZ(_node.aabb.getMax().z + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.z);
+			zmax = node.aabb.getMax().z() + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * displacement.z();
 		}
-		//Log.error(" compare : " + _node.aabb.getMin() + " " + _node.aabb.getMax());
-		//Log.error("         : " + _newAABB.getMin() + " " + _newAABB.getMax());
-		if (_node.aabb.contains(_newAABB) == false) {
-			//Log.critical("ERROR");
+		node.aabb.setMin(new Vector3f(xmin, ymin, zmin));
+		node.aabb.setMax(new Vector3f(xmax, ymax, zmax));
+		//LOGGER.error(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax());
+		//LOGGER.error("         : " + newAABB.getMin() + " " + newAABB.getMax());
+		if (!node.aabb.contains(newAABB)) {
+			//LOGGER.critical("ERROR");
 		}
-		assert (_node.aabb.contains(_newAABB));
+		assert (node.aabb.contains(newAABB));
 		// Reinsert the node into the tree
-		insertLeafNode(_node);
+		insertLeafNode(node);
 		return true;
 	}
 	

src/org/atriasoft/ephysics/collision/broadphase/PairDTree.java

@@ -2,9 +2,9 @@ package org.atriasoft.ephysics.collision.broadphase;
 
 import java.util.Set;
 
-import javafx.collections.transformation.SortedList;
-
-public class PairDTree {
+public record PairDTree(
+		DTree first,
+		DTree second) {
 	public static int countInSet(final Set<PairDTree> values, final PairDTree sample) {
 		int count = 0;
 		for (final PairDTree elem : values) {
@@ -19,24 +19,6 @@ public class PairDTree {
 		return count;
 	}
 	
-	public static int countInSet(final SortedList<PairDTree> values, final PairDTree sample) {
-		int count = 0;
-		for (final PairDTree elem : values) {
-			if (elem.first != sample.first) {
-				continue;
-			}
-			if (elem.second != sample.second) {
-				continue;
-			}
-			count++;
-		}
-		return count;
-	}
-	
-	public final DTree first;
-	
-	public final DTree second;
-	
 	public PairDTree(final DTree first, final DTree second) {
 		if (first.uid < second.uid) {
 			this.first = first;
@@ -48,24 +30,6 @@ public class PairDTree {
 		}
 	}
 	
-	@Override
-	public boolean equals(final Object obj) {
-		if (this == obj) {
-			return true;
-		}
-		if (obj == null || getClass() != obj.getClass()) {
-			return false;
-		}
-		final PairDTree tmp = (PairDTree) obj;
-		if (this.first != tmp.first) {
-			return false;
-		}
-		if (this.second != tmp.second) {
-			return false;
-		}
-		return true;
-	}
-	
 	@Override
 	public String toString() {
 		return "PairDTree [first=" + this.first.uid + ", second=" + this.second.uid + "]";

src/org/atriasoft/ephysics/collision/narrowphase/CollisionDispatch.java

@@ -11,11 +11,11 @@ import org.atriasoft.ephysics.collision.shapes.CollisionShapeType;
 public abstract class CollisionDispatch {
 	
 	/// Initialize the collision dispatch configuration
-	public void init(final CollisionDetection _collisionDetection) {
+	public void init(final CollisionDetection collisionDetection) {
 		// Nothing to do ...
 	}
 	
 	/// Select and return the narrow-phase collision detection algorithm to
 	/// use between two types of collision shapes.
-	public abstract NarrowPhaseAlgorithm selectAlgorithm(CollisionShapeType _shape1Type, CollisionShapeType _shape2Type);
+	public abstract NarrowPhaseAlgorithm selectAlgorithm(CollisionShapeType shape1Type, CollisionShapeType shape2Type);
 }

src/org/atriasoft/ephysics/collision/narrowphase/ConcaveVsConvexAlgorithm.java

@@ -4,9 +4,6 @@ import java.util.ArrayList;
 import java.util.Comparator;
 import java.util.List;
 
-import org.atriasoft.etk.math.Transform3D;
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.collision.CollisionDetection;
 import org.atriasoft.ephysics.collision.CollisionShapeInfo;
 import org.atriasoft.ephysics.collision.ProxyShape;
@@ -19,8 +16,10 @@ import org.atriasoft.ephysics.constraint.ContactPointInfo;
 import org.atriasoft.ephysics.engine.OverlappingPair;
 import org.atriasoft.ephysics.mathematics.Mathematics;
 import org.atriasoft.ephysics.mathematics.PairIntVector3f;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
 
-//static boolean sortFunction(SmoothMeshContactInfo&_contact1,SmoothMeshContactInfo&_contact2){return _contact1.contactInfo.penetrationDepth<=_contact2.contactInfo.penetrationDepth;}
+//static boolean sortFunction(SmoothMeshContactInfo&contact1,SmoothMeshContactInfo&contact2){return contact1.contactInfo.penetrationDepth<=contact2.contactInfo.penetrationDepth;}
 
 /**
  *  This class is used to compute the narrow-phase collision detection
@@ -37,53 +36,53 @@ public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
 	class ConvexVsTriangleCallback implements ConcaveShape.TriangleCallback {
 		
 		protected CollisionDetection collisionDetection; //!< Pointer to the collision detection object
-		protected NarrowPhaseCallback narrowPhaseCallback; //!< Narrow-phase collision callback
-		protected ConvexShape convexShape; //!< Convex collision shape to test collision with
+		protected ProxyShape concaveProxyShape; //!< Proxy shape of the concave collision shape
 		protected ConcaveShape concaveShape; //!< Concave collision shape
 		protected ProxyShape convexProxyShape; //!< Proxy shape of the convex collision shape
-		protected ProxyShape concaveProxyShape; //!< Proxy shape of the concave collision shape
+		protected ConvexShape convexShape; //!< Convex collision shape to test collision with
+		protected NarrowPhaseCallback narrowPhaseCallback; //!< Narrow-phase collision callback
 		protected OverlappingPair overlappingPair; //!< Broadphase overlapping pair
 		
-		//  protected static boolean contactsDepthCompare(ContactPointInfo _contact1, ContactPointInfo _contact2);
+		//  protected static boolean contactsDepthCompare(ContactPointInfo contact1, ContactPointInfo contact2);
 		
 		/// Set the collision detection pointer
-		public void setCollisionDetection(final CollisionDetection _collisionDetection) {
-			this.collisionDetection = _collisionDetection;
+		public void setCollisionDetection(final CollisionDetection collisionDetection) {
+			this.collisionDetection = collisionDetection;
 		}
 		
 		/// Set the concave collision shape
-		public void setConcaveShape(final ConcaveShape _concaveShape) {
-			this.concaveShape = _concaveShape;
+		public void setConcaveShape(final ConcaveShape concaveShape) {
+			this.concaveShape = concaveShape;
 		}
 		
 		/// Set the convex collision shape to test collision with
-		public void setConvexShape(final ConvexShape _convexShape) {
-			this.convexShape = _convexShape;
+		public void setConvexShape(final ConvexShape convexShape) {
+			this.convexShape = convexShape;
 		}
 		
 		/// Set the narrow-phase collision callback
-		public void setNarrowPhaseCallback(final NarrowPhaseCallback _callback) {
-			this.narrowPhaseCallback = _callback;
+		public void setNarrowPhaseCallback(final NarrowPhaseCallback callback) {
+			this.narrowPhaseCallback = callback;
 		}
 		
 		/// Set the broadphase overlapping pair
-		public void setOverlappingPair(final OverlappingPair _overlappingPair) {
-			this.overlappingPair = _overlappingPair;
+		public void setOverlappingPair(final OverlappingPair overlappingPair) {
+			this.overlappingPair = overlappingPair;
 		}
 		
 		/// Set the proxy shapes of the two collision shapes
-		public void setProxyShapes(final ProxyShape _convexProxyShape, final ProxyShape _concaveProxyShape) {
-			this.convexProxyShape = _convexProxyShape;
-			this.concaveProxyShape = _concaveProxyShape;
+		public void setProxyShapes(final ProxyShape convexProxyShape, final ProxyShape concaveProxyShape) {
+			this.convexProxyShape = convexProxyShape;
+			this.concaveProxyShape = concaveProxyShape;
 		}
 		
 		/// Test collision between a triangle and the convex mesh shape
 		@Override
-		public void testTriangle(final Vector3f[] _trianglePoints) {
+		public void testTriangle(final Vector3f[] trianglePoints) {
 			// Create a triangle collision shape
 			final float margin = this.concaveShape.getTriangleMargin();
 			
-			final TriangleShape triangleShape = new TriangleShape(_trianglePoints[0], _trianglePoints[1], _trianglePoints[2], margin);
+			final TriangleShape triangleShape = new TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
 			// Select the collision algorithm to use between the triangle and the convex shape
 			final NarrowPhaseAlgorithm algo = this.collisionDetection.getCollisionAlgorithm(triangleShape.getType(), this.convexShape.getType());
 			// If there is no collision algorithm between those two kinds of shapes
@@ -113,33 +112,33 @@ public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
 		private final List<SmoothMeshContactInfo> contactPoints;
 		
 		// Constructor
-		public SmoothCollisionNarrowPhaseCallback(final List<SmoothMeshContactInfo> _contactPoints) {
-			this.contactPoints = new ArrayList<>(_contactPoints);
+		public SmoothCollisionNarrowPhaseCallback(final List<SmoothMeshContactInfo> contactPoints) {
+			this.contactPoints = new ArrayList<>(contactPoints);
 		}
 		
 		/// Called by a narrow-phase collision algorithm when a new contact has been found
 		@Override
-		public void notifyContact(final OverlappingPair _overlappingPair, final ContactPointInfo _contactInfo) {
+		public void notifyContact(final OverlappingPair overlappingPair, final ContactPointInfo contactInfo) {
 			final Vector3f[] triangleVertices = new Vector3f[3];
 			boolean isFirstShapeTriangle;
 			// If the collision shape 1 is the triangle
-			if (_contactInfo.collisionShape1.getType() == CollisionShapeType.TRIANGLE) {
-				assert (_contactInfo.collisionShape2.getType() != CollisionShapeType.TRIANGLE);
-				final TriangleShape triangleShape = (TriangleShape) _contactInfo.collisionShape1;
+			if (contactInfo.collisionShape1.getType() == CollisionShapeType.TRIANGLE) {
+				assert (contactInfo.collisionShape2.getType() != CollisionShapeType.TRIANGLE);
+				final TriangleShape triangleShape = (TriangleShape) contactInfo.collisionShape1;
 				triangleVertices[0] = triangleShape.getVertex(0);
 				triangleVertices[1] = triangleShape.getVertex(1);
 				triangleVertices[2] = triangleShape.getVertex(2);
 				isFirstShapeTriangle = true;
 			} else { // If the collision shape 2 is the triangle
-				assert (_contactInfo.collisionShape2.getType() == CollisionShapeType.TRIANGLE);
-				final TriangleShape triangleShape = (TriangleShape) _contactInfo.collisionShape2;
+				assert (contactInfo.collisionShape2.getType() == CollisionShapeType.TRIANGLE);
+				final TriangleShape triangleShape = (TriangleShape) contactInfo.collisionShape2;
 				triangleVertices[0] = triangleShape.getVertex(0);
 				triangleVertices[1] = triangleShape.getVertex(1);
 				triangleVertices[2] = triangleShape.getVertex(2);
 				isFirstShapeTriangle = false;
 			}
 			
-			final SmoothMeshContactInfo smoothContactInfo = new SmoothMeshContactInfo(_contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
+			final SmoothMeshContactInfo smoothContactInfo = new SmoothMeshContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
 			// Add the narrow-phase contact into the list of contact to process for
 			// smooth mesh collision
 			this.contactPoints.add(smoothContactInfo);
@@ -158,17 +157,16 @@ public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
 		public Vector3f[] triangleVertices = new Vector3f[3];
 		
 		public SmoothMeshContactInfo() {
-			// TODO: add it for List
+			// TODO add it for List
 		}
 		
 		/// Constructor
-		public SmoothMeshContactInfo(final ContactPointInfo _contact, final boolean _firstShapeTriangle, final Vector3f _trianglePoint1, final Vector3f _trianglePoint2,
-				final Vector3f _trianglePoint3) {
-			this.contactInfo = new ContactPointInfo(_contact);
-			this.isFirstShapeTriangle = _firstShapeTriangle;
-			this.triangleVertices[0] = _trianglePoint1;
-			this.triangleVertices[1] = _trianglePoint2;
-			this.triangleVertices[2] = _trianglePoint3;
+		public SmoothMeshContactInfo(final ContactPointInfo contact, final boolean firstShapeTriangle, final Vector3f trianglePoint1, final Vector3f trianglePoint2, final Vector3f trianglePoint3) {
+			this.contactInfo = new ContactPointInfo(contact);
+			this.isFirstShapeTriangle = firstShapeTriangle;
+			this.triangleVertices[0] = trianglePoint1;
+			this.triangleVertices[1] = trianglePoint2;
+			this.triangleVertices[2] = trianglePoint3;
 		}
 	}
 	
@@ -178,27 +176,27 @@ public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
 	}
 	
 	/// Add a triangle vertex into the set of processed triangles
-	protected void addProcessedVertex(final List<PairIntVector3f> _processTriangleVertices, final Vector3f _vertex) {
-		_processTriangleVertices.add(new PairIntVector3f((int) (_vertex.x * _vertex.y * _vertex.z), _vertex));
+	protected void addProcessedVertex(final List<PairIntVector3f> processTriangleVertices, final Vector3f vertex) {
+		processTriangleVertices.add(new PairIntVector3f((int) (vertex.x() * vertex.y() * vertex.z()), vertex));
 	}
 	
 	/// Return true if the vertex is in the set of already processed vertices
-	protected boolean hasVertexBeenProcessed(final List<PairIntVector3f> _processTriangleVertices, final Vector3f _vertex) {
+	protected boolean hasVertexBeenProcessed(final List<PairIntVector3f> processTriangleVertices, final Vector3f vertex) {
 		/* TODO : List<etk::Pair<int, Vector3f>> was an unordered map ... ==> stupid idee... I replace code because I do not have enouth time to do something good...
-		int key = int(_vertex.x * _vertex.y * _vertex.z);
-		auto range = _processTriangleVertices.equal_range(key);
+		int key = int(vertex.x * vertex.y * vertex.z);
+		auto range = processTriangleVertices.equalrange(key);
 		for (auto it = range.first; it != range.second; ++it) {
-			if (    _vertex.x == it.second.x
-			     && _vertex.y == it.second.y
-			     && _vertex.z == it.second.z) {
+			if (    vertex.x == it.second.x
+			     && vertex.y == it.second.y
+			     && vertex.z == it.second.z) {
 				return true;
 			}
 		}
 		return false;
 		*/
 		// TODO : This is not really the same ...
-		for (final PairIntVector3f it : _processTriangleVertices) {
-			if (_vertex.x == it.second.x && _vertex.y == it.second.y && _vertex.z == it.second.z) {
+		for (final PairIntVector3f it : processTriangleVertices) {
+			if (vertex.x() == it.second.x() && vertex.y() == it.second.y() && vertex.z() == it.second.z()) {
 				return true;
 			}
 		}
@@ -206,17 +204,17 @@ public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
 	}
 	
 	/// Process the concave triangle mesh collision using the smooth mesh collision algorithm
-	protected void processSmoothMeshCollision(final OverlappingPair _overlappingPair, final List<SmoothMeshContactInfo> _contactPoints, final NarrowPhaseCallback _callback) {
+	protected void processSmoothMeshCollision(final OverlappingPair overlappingPair, final List<SmoothMeshContactInfo> contactPoints, final NarrowPhaseCallback callback) {
 		// Set with the triangle vertices already processed to void further contacts with same triangle
 		final List<PairIntVector3f> processTriangleVertices = new ArrayList<>();
 		// Sort the list of narrow-phase contacts according to their penetration depth
-		_contactPoints.sort(new Comparator<SmoothMeshContactInfo>() {
+		contactPoints.sort(new Comparator<SmoothMeshContactInfo>() {
 			@Override
-			public int compare(final SmoothMeshContactInfo _pair1, final SmoothMeshContactInfo _pair2) {
-				if (_pair1.contactInfo.penetrationDepth < _pair2.contactInfo.penetrationDepth) {
+			public int compare(final SmoothMeshContactInfo pair1, final SmoothMeshContactInfo pair2) {
+				if (pair1.contactInfo.penetrationDepth < pair2.contactInfo.penetrationDepth) {
 					return -1;
 				}
-				if (_pair1.contactInfo.penetrationDepth == _pair2.contactInfo.penetrationDepth) {
+				if (pair1.contactInfo.penetrationDepth == pair2.contactInfo.penetrationDepth) {
 					return 0;
 				}
 				return +1;
@@ -224,15 +222,15 @@ public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
 		});
 		
 		// For each contact point (from smaller penetration depth to larger)
-		for (final SmoothMeshContactInfo info : _contactPoints) {
+		for (final SmoothMeshContactInfo info : contactPoints) {
 			final Vector3f contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
 			// Compute the barycentric coordinates of the point in the triangle
 			final Float u = 0.0f, v = 0.0f, w = 0.0f;
 			Mathematics.computeBarycentricCoordinatesInTriangle(info.triangleVertices[0], info.triangleVertices[1], info.triangleVertices[2], contactPoint, u, v, w);
 			int nbZeros = 0;
-			final boolean isUZero = Mathematics.ApproxEqual(u, 0.0f, 0.0001f);
-			final boolean isVZero = Mathematics.ApproxEqual(v, 0.0f, 0.0001f);
-			final boolean isWZero = Mathematics.ApproxEqual(w, 0.0f, 0.0001f);
+			final boolean isUZero = Mathematics.approxEqual(u, 0.0f, 0.0001f);
+			final boolean isVZero = Mathematics.approxEqual(v, 0.0f, 0.0001f);
+			final boolean isWZero = Mathematics.approxEqual(w, 0.0f, 0.0001f);
 			if (isUZero) {
 				nbZeros++;
 			}
@@ -248,7 +246,7 @@ public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
 				// Check that this triangle vertex has not been processed yet
 				if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
 					// Keep the contact as it is and report it
-					_callback.notifyContact(_overlappingPair, info.contactInfo);
+					callback.notifyContact(overlappingPair, info.contactInfo);
 				}
 			} else if (nbZeros == 1) {
 				// If it is an edge contact
@@ -257,7 +255,7 @@ public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
 				// Check that this triangle edge has not been processed yet
 				if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) && !hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
 					// Keep the contact as it is and report it
-					_callback.notifyContact(_overlappingPair, info.contactInfo);
+					callback.notifyContact(overlappingPair, info.contactInfo);
 				}
 			} else {
 				// If it is a face contact
@@ -265,36 +263,36 @@ public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
 				ProxyShape firstShape;
 				ProxyShape secondShape;
 				if (info.isFirstShapeTriangle) {
-					firstShape = _overlappingPair.getShape1();
-					secondShape = _overlappingPair.getShape2();
+					firstShape = overlappingPair.getShape1();
+					secondShape = overlappingPair.getShape2();
 				} else {
-					firstShape = _overlappingPair.getShape2();
-					secondShape = _overlappingPair.getShape1();
+					firstShape = overlappingPair.getShape2();
+					secondShape = overlappingPair.getShape1();
 				}
 				// We use the triangle normal as the contact normal
-				final Vector3f a = info.triangleVertices[1].lessNew(info.triangleVertices[0]);
-				final Vector3f b = info.triangleVertices[2].lessNew(info.triangleVertices[0]);
+				final Vector3f a = info.triangleVertices[1].less(info.triangleVertices[0]);
+				final Vector3f b = info.triangleVertices[2].less(info.triangleVertices[0]);
 				final Vector3f localNormal = a.cross(b);
 				newContactInfo.normal = firstShape.getLocalToWorldTransform().getOrientation().multiply(localNormal);
 				final Vector3f firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
-				final Vector3f firstWorldPoint = firstShape.getLocalToWorldTransform().multiplyNew(firstLocalPoint);
-				newContactInfo.normal.normalize();
+				final Vector3f firstWorldPoint = firstShape.getLocalToWorldTransform().multiply(firstLocalPoint);
+				newContactInfo.normal = newContactInfo.normal.normalize();
 				if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
-					newContactInfo.normal.multiply(-1);
+					newContactInfo.normal = newContactInfo.normal.multiply(-1);
 				}
 				// We recompute the contact point on the second body with the new normal as described in
 				// the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
 				// Dirk Gregorius) to avoid adding torque
 				final Transform3D worldToLocalSecondPoint = secondShape.getLocalToWorldTransform().inverseNew();
 				if (info.isFirstShapeTriangle) {
-					final Vector3f newSecondWorldPoint = firstWorldPoint.addNew(newContactInfo.normal);
-					newContactInfo.localPoint2 = worldToLocalSecondPoint.multiplyNew(newSecondWorldPoint);
+					final Vector3f newSecondWorldPoint = firstWorldPoint.add(newContactInfo.normal);
+					newContactInfo.localPoint2 = worldToLocalSecondPoint.multiply(newSecondWorldPoint);
 				} else {
-					final Vector3f newSecondWorldPoint = firstWorldPoint.lessNew(newContactInfo.normal);
-					newContactInfo.localPoint1 = worldToLocalSecondPoint.multiplyNew(newSecondWorldPoint);
+					final Vector3f newSecondWorldPoint = firstWorldPoint.less(newContactInfo.normal);
+					newContactInfo.localPoint1 = worldToLocalSecondPoint.multiply(newSecondWorldPoint);
 				}
 				// Report the contact
-				_callback.notifyContact(_overlappingPair, newContactInfo);
+				callback.notifyContact(overlappingPair, newContactInfo);
 			}
 			
 			// Add the three vertices of the triangle to the set of processed
@@ -308,23 +306,23 @@ public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
 	
 	/// Compute a contact info if the two bounding volume collide
 	@Override
-	public void testCollision(final CollisionShapeInfo _shape1Info, final CollisionShapeInfo _shape2Info, final NarrowPhaseCallback _callback) {
+	public void testCollision(final CollisionShapeInfo shape1Info, final CollisionShapeInfo shape2Info, final NarrowPhaseCallback callback) {
 		ProxyShape convexProxyShape;
 		ProxyShape concaveProxyShape;
 		ConvexShape convexShape;
 		ConcaveShape concaveShape;
 		// Collision shape 1 is convex, collision shape 2 is concave
-		if (_shape1Info.collisionShape.isConvex()) {
-			convexProxyShape = _shape1Info.proxyShape;
-			convexShape = (ConvexShape) _shape1Info.collisionShape;
-			concaveProxyShape = _shape2Info.proxyShape;
-			concaveShape = (ConcaveShape) _shape2Info.collisionShape;
+		if (shape1Info.collisionShape().isConvex()) {
+			convexProxyShape = shape1Info.proxyShape();
+			convexShape = (ConvexShape) shape1Info.collisionShape();
+			concaveProxyShape = shape2Info.proxyShape();
+			concaveShape = (ConcaveShape) shape2Info.collisionShape();
 		} else {
 			// Collision shape 2 is convex, collision shape 1 is concave
-			convexProxyShape = _shape2Info.proxyShape;
-			convexShape = (ConvexShape) _shape2Info.collisionShape;
-			concaveProxyShape = _shape1Info.proxyShape;
-			concaveShape = (ConcaveShape) _shape1Info.collisionShape;
+			convexProxyShape = shape2Info.proxyShape();
+			convexShape = (ConvexShape) shape2Info.collisionShape();
+			concaveProxyShape = shape1Info.proxyShape();
+			concaveShape = (ConcaveShape) shape1Info.collisionShape();
 		}
 		// Set the parameters of the callback object
 		final ConvexVsTriangleCallback convexVsTriangleCallback = new ConvexVsTriangleCallback();
@@ -332,7 +330,7 @@ public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
 		convexVsTriangleCallback.setConvexShape(convexShape);
 		convexVsTriangleCallback.setConcaveShape(concaveShape);
 		convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
-		convexVsTriangleCallback.setOverlappingPair(_shape1Info.overlappingPair);
+		convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair());
 		// Compute the convex shape AABB in the local-space of the convex shape
 		final AABB aabb = new AABB();
 		convexShape.computeAABB(aabb, convexProxyShape.getLocalToWorldTransform());
@@ -345,9 +343,9 @@ public class ConcaveVsConvexAlgorithm extends NarrowPhaseAlgorithm {
 			// Call the convex vs triangle callback for each triangle of the concave shape
 			concaveShape.testAllTriangles(convexVsTriangleCallback, aabb);
 			// Run the smooth mesh collision algorithm
-			processSmoothMeshCollision(_shape1Info.overlappingPair, contactPoints, _callback);
+			processSmoothMeshCollision(shape1Info.overlappingPair(), contactPoints, callback);
 		} else {
-			convexVsTriangleCallback.setNarrowPhaseCallback(_callback);
+			convexVsTriangleCallback.setNarrowPhaseCallback(callback);
 			// Call the convex vs triangle callback for each triangle of the concave shape
 			concaveShape.testAllTriangles(convexVsTriangleCallback, aabb);
 		}

src/org/atriasoft/ephysics/collision/narrowphase/DefaultCollisionDispatch.java

@@ -12,32 +12,33 @@ import org.atriasoft.ephysics.collision.shapes.CollisionShapeType;
  */
 public class DefaultCollisionDispatch extends CollisionDispatch {
 	
-	//!< Sphere vs Sphere collision algorithm
-	protected SphereVsSphereAlgorithm sphereVsSphereAlgorithm;
 	//!< Concave vs Convex collision algorithm
 	protected ConcaveVsConvexAlgorithm concaveVsConvexAlgorithm;
 	//!< GJK Algorithm
-	protected GJKAlgorithm GJKAlgorithm;
+	protected GJKAlgorithm gjkAlgorithm;
+	//!< Sphere vs Sphere collision algorithm
+	protected SphereVsSphereAlgorithm sphereVsSphereAlgorithm;
 	
 	@Override
-	public void init(final CollisionDetection _collisionDetection) {
+	public void init(final CollisionDetection collisionDetection) {
 		// Initialize the collision algorithms
-		this.sphereVsSphereAlgorithm = new SphereVsSphereAlgorithm(_collisionDetection);
-		this.GJKAlgorithm = new GJKAlgorithm(_collisionDetection);
-		this.concaveVsConvexAlgorithm = new ConcaveVsConvexAlgorithm(_collisionDetection);
+		this.sphereVsSphereAlgorithm = new SphereVsSphereAlgorithm(collisionDetection);
+		this.gjkAlgorithm = new GJKAlgorithm(collisionDetection);
+		this.concaveVsConvexAlgorithm = new ConcaveVsConvexAlgorithm(collisionDetection);
 	}
 	
 	@Override
-	public NarrowPhaseAlgorithm selectAlgorithm(final CollisionShapeType _type1, final CollisionShapeType _type2) {
+	public NarrowPhaseAlgorithm selectAlgorithm(final CollisionShapeType type1, final CollisionShapeType type2) {
 		// Sphere vs Sphere algorithm
-		if (_type1 == CollisionShapeType.SPHERE && _type2 == CollisionShapeType.SPHERE) {
+		if (type1 == CollisionShapeType.SPHERE && type2 == CollisionShapeType.SPHERE) {
 			return this.sphereVsSphereAlgorithm;
-		} else if ((!CollisionShape.isConvex(_type1) && CollisionShape.isConvex(_type2)) || (!CollisionShape.isConvex(_type2) && CollisionShape.isConvex(_type1))) {
+		}
+		if ((!CollisionShape.isConvex(type1) && CollisionShape.isConvex(type2)) || (!CollisionShape.isConvex(type2) && CollisionShape.isConvex(type1))) {
 			// Concave vs Convex algorithm
 			return this.concaveVsConvexAlgorithm;
-		} else if (CollisionShape.isConvex(_type1) && CollisionShape.isConvex(_type2)) {
+		} else if (CollisionShape.isConvex(type1) && CollisionShape.isConvex(type2)) {
 			// Convex vs Convex algorithm (GJK algorithm)
-			return this.GJKAlgorithm;
+			return this.gjkAlgorithm;
 		} else {
 			return null;
 		}

src/org/atriasoft/ephysics/collision/narrowphase/EPA/EPAAlgorithm.java

@@ -33,21 +33,21 @@ import org.atriasoft.etk.math.Vector3f;
  */
 public class EPAAlgorithm {
 	/// Add a triangle face in the candidate triangle heap
-	private void addFaceCandidate(final TriangleEPA _triangle, final SortedSet<TriangleEPA> _heap, final float _upperBoundSquarePenDepth) {
-		////Log.info("addFaceCandidate: " + _triangle.get(0) + ", " + _triangle.get(1) + ", " + _triangle.get(2) + "    " + _upperBoundSquarePenDepth);
+	private void addFaceCandidate(final TriangleEPA triangle, final SortedSet<TriangleEPA> heap, final float upperBoundSquarePenDepth) {
+		////LOGGER.info("addFaceCandidate: " + triangle.get(0) + ", " + triangle.get(1) + ", " + triangle.get(2) + "    " + upperBoundSquarePenDepth);
 		// If the closest point of the affine hull of triangle
 		// points is internal to the triangle and if the distance
 		// of the closest point from the origin is at most the
 		// penetration depth upper bound
-		////Log.info("    _triangle.isClosestPointInternalToTriangle(): " + _triangle.isClosestPointInternalToTriangle());
-		////Log.info("    _triangle.getDistSquare(): " + _triangle.getDistSquare());
-		if (_triangle.isClosestPointInternalToTriangle() && _triangle.getDistSquare() <= _upperBoundSquarePenDepth) {
+		////LOGGER.info("    triangle.isClosestPointInternalToTriangle(): " + triangle.isClosestPointInternalToTriangle());
+		////LOGGER.info("    triangle.getDistSquare(): " + triangle.getDistSquare());
+		if (triangle.isClosestPointInternalToTriangle() && triangle.getDistSquare() <= upperBoundSquarePenDepth) {
 			// Add the triangle face to the list of candidates
-			_heap.add(_triangle);
-			////Log.info("add in heap:");
+			heap.add(triangle);
+			////LOGGER.info("add in heap:");
 			//int iii = 0;
-			//for (final TriangleEPA elem : _heap) {
-			//	////Log.info("    [" + iii + "] " + elem.getDistSquare());
+			//for (final TriangleEPA elem : heap) {
+			//	////LOGGER.info("    [" + iii + "] " + elem.getDistSquare());
 			//	++iii;
 			//}
 		}
@@ -59,27 +59,27 @@ public class EPAAlgorithm {
 	/// intersect. An initial simplex that contains origin has been computed with
 	/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
 	/// the correct penetration depth
-	public void computePenetrationDepthAndContactPoints(final Simplex _simplex, final CollisionShapeInfo _shape1Info, final Transform3D _transform1, final CollisionShapeInfo _shape2Info,
-			final Transform3D _transform2, final Vector3f _vector, final NarrowPhaseCallback _narrowPhaseCallback) {
-		////Log.info("computePenetrationDepthAndContactPoints()");
-		assert (_shape1Info.collisionShape.isConvex());
-		assert (_shape2Info.collisionShape.isConvex());
-		final ConvexShape shape1 = (ConvexShape) (_shape1Info.collisionShape);
-		final ConvexShape shape2 = (ConvexShape) (_shape2Info.collisionShape);
-		final CacheData shape1CachedCollisionData = (CacheData) _shape1Info.cachedCollisionData;
-		final CacheData shape2CachedCollisionData = (CacheData) _shape2Info.cachedCollisionData;
-		final Vector3f suppPointsA[] = new Vector3f[EdgeEPA.MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
-		final Vector3f suppPointsB[] = new Vector3f[EdgeEPA.MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
-		final Vector3f points[] = new Vector3f[EdgeEPA.MAX_SUPPORT_POINTS]; // Current points
+	public Vector3f computePenetrationDepthAndContactPoints(final Simplex simplex, final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info,
+			final Transform3D transform2, Vector3f vector, final NarrowPhaseCallback narrowPhaseCallback) {
+		////LOGGER.info("computePenetrationDepthAndContactPoints()");
+		assert (shape1Info.collisionShape().isConvex());
+		assert (shape2Info.collisionShape().isConvex());
+		final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape());
+		final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape());
+		final CacheData shape1CachedCollisionData = (CacheData) shape1Info.cachedCollisionData();
+		final CacheData shape2CachedCollisionData = (CacheData) shape2Info.cachedCollisionData();
+		final Vector3f[] suppPointsA = new Vector3f[EdgeEPA.MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
+		final Vector3f[] suppPointsB = new Vector3f[EdgeEPA.MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
+		final Vector3f[] points = new Vector3f[EdgeEPA.MAX_SUPPORT_POINTS]; // Current points
 		final TrianglesStore triangleStore = new TrianglesStore(); // Store the triangles
 		
 		//https://docs.oracle.com/en/java/javase/11/docs/api/java.base/java/util/SortedSet.html
 		//	https://stackoverflow.com/questions/38066291/how-to-define-comparator-on-sortedset-like-treeset
 		final SortedSet<TriangleEPA> triangleHeap = new TreeSet<>(new Comparator<TriangleEPA>() {
 			@Override
-			public int compare(final TriangleEPA _face1, final TriangleEPA _face2) {
-				final float d1 = _face1.getDistSquare();
-				final float d2 = _face2.getDistSquare();
+			public int compare(final TriangleEPA face1, final TriangleEPA face2) {
+				final float d1 = face1.getDistSquare();
+				final float d2 = face2.getDistSquare();
 				if (d1 < d2) {
 					return -1;
 				}
@@ -91,26 +91,26 @@ public class EPAAlgorithm {
 		});
 		// Transform3D a point from local space of body 2 to local
 		// space of body 1 (the GJK algorithm is done in local space of body 1)
-		final Transform3D body2Tobody1 = _transform1.inverseNew().multiplyNew(_transform2);
+		final Transform3D body2Tobody1 = transform1.inverseNew().multiply(transform2);
 		// Matrix that transform a direction from local
 		// space of body 1 into local space of body 2
-		final Quaternion rotateToBody2 = _transform2.getOrientation().inverseNew().multiplyNew(_transform1.getOrientation());
+		final Quaternion rotateToBody2 = transform2.getOrientation().inverse().multiply(transform1.getOrientation());
 		// Get the simplex computed previously by the GJK algorithm
-		int nbVertices = _simplex.getSimplex(suppPointsA, suppPointsB, points);
+		int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
 		// Compute the tolerance
-		final float tolerance = Constant.FLOAT_EPSILON * _simplex.getMaxLengthSquareOfAPoint();
+		final float tolerance = Constant.FLOAT_EPSILON * simplex.getMaxLengthSquareOfAPoint();
 		// Clear the storing of triangles
 		triangleStore.clear();
 		// Select an action according to the number of points in the simplex
 		// computed with GJK algorithm in order to obtain an initial polytope for
 		// The EPA algorithm.
-		////Log.info(">>>>>>>>>>>>>>>>>> *** " + nbVertices);
+		////LOGGER.info(">>>>>>>>>>>>>>>>>> *** " + nbVertices);
 		switch (nbVertices) {
 			case 1:
 				// Only one point in the simplex (which should be the origin).
 				// We have a touching contact with zero penetration depth.
 				// We drop that kind of contact. Therefore, we return false
-				return;
+				return vector;
 			case 2: {
 				// The simplex returned by GJK is a line segment d containing the origin.
 				// We add two additional support points to ruct a hexahedron (two tetrahedron
@@ -120,30 +120,30 @@ public class EPAAlgorithm {
 				// ruct the polytope are the three support points in those three directions
 				// v1, v2 and v3.
 				// Direction of the segment
-				final Vector3f d = points[1].lessNew(points[0]).safeNormalizeNew();
+				final Vector3f d = points[1].less(points[0]).safeNormalize();
 				// Choose the coordinate axis from the minimal absolute component of the vector d
 				final int minAxis = d.abs().getMinAxis();
 				// Compute sin(60)
 				final float sin60 = FMath.sqrt(3.0f) * 0.5f;
 				// Create a rotation quaternion to rotate the vector v1 to get the vectors
 				// v2 and v3
-				final Quaternion rotationQuat = new Quaternion(d.x * sin60, d.y * sin60, d.z * sin60, 0.5f);
+				final Quaternion rotationQuat = new Quaternion(d.x() * sin60, d.y() * sin60, d.z() * sin60, 0.5f);
 				// Compute the vector v1, v2, v3
 				final Vector3f v1 = d.cross(new Vector3f(minAxis == 0 ? 1.0f : 0.0f, minAxis == 1 ? 1.0f : 0.0f, minAxis == 2 ? 1.0f : 0.0f));
 				final Vector3f v2 = rotationQuat.multiply(v1);
 				final Vector3f v3 = rotationQuat.multiply(v2);
 				// Compute the support point in the direction of v1
 				suppPointsA[2] = shape1.getLocalSupportPointWithMargin(v1, shape1CachedCollisionData);
-				suppPointsB[2] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v1.multiplyNew(-1)), shape2CachedCollisionData));
-				points[2] = suppPointsA[2].lessNew(suppPointsB[2]);
+				suppPointsB[2] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v1.multiply(-1)), shape2CachedCollisionData));
+				points[2] = suppPointsA[2].less(suppPointsB[2]);
 				// Compute the support point in the direction of v2
 				suppPointsA[3] = shape1.getLocalSupportPointWithMargin(v2, shape1CachedCollisionData);
-				suppPointsB[3] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v2.multiplyNew(-1)), shape2CachedCollisionData));
-				points[3] = suppPointsA[3].lessNew(suppPointsB[3]);
+				suppPointsB[3] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v2.multiply(-1)), shape2CachedCollisionData));
+				points[3] = suppPointsA[3].less(suppPointsB[3]);
 				// Compute the support point in the direction of v3
 				suppPointsA[4] = shape1.getLocalSupportPointWithMargin(v3, shape1CachedCollisionData);
-				suppPointsB[4] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v3.multiplyNew(-1)), shape2CachedCollisionData));
-				points[4] = suppPointsA[4].lessNew(suppPointsB[4]);
+				suppPointsB[4] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v3.multiply(-1)), shape2CachedCollisionData));
+				points[4] = suppPointsA[4].less(suppPointsB[4]);
 				// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
 				// tetrahedron that contains the origin in order that the initial polytope of the
 				// EPA algorithm is a tetrahedron, which is simpler to deal with.
@@ -162,7 +162,7 @@ public class EPAAlgorithm {
 					points[0] = points[4];
 				} else {
 					// The origin is not in the initial polytope
-					return;
+					return vector;
 				}
 				// The polytope contains now 4 vertices
 				nbVertices = 4;
@@ -179,18 +179,18 @@ public class EPAAlgorithm {
 					// The tetrahedron is a correct initial polytope for the EPA algorithm.
 					// Therefore, we ruct the tetrahedron.
 					// Comstruct the 4 triangle faces of the tetrahedron
-					////Log.error("befor call: (points, 0, 1, 2)");
+					////LOGGER.error("befor call: (points, 0, 1, 2)");
 					final TriangleEPA face0 = triangleStore.newTriangle(points, 0, 1, 2);
-					////Log.error("befor call: (points, 0, 3, 1)");
+					////LOGGER.error("befor call: (points, 0, 3, 1)");
 					final TriangleEPA face1 = triangleStore.newTriangle(points, 0, 3, 1);
-					////Log.error("befor call: (points, 0, 2, 3)");
+					////LOGGER.error("befor call: (points, 0, 2, 3)");
 					final TriangleEPA face2 = triangleStore.newTriangle(points, 0, 2, 3);
-					////Log.error("befor call: (points, 1, 3, 2)");
+					////LOGGER.error("befor call: (points, 1, 3, 2)");
 					final TriangleEPA face3 = triangleStore.newTriangle(points, 1, 3, 2);
 					// If the ructed tetrahedron is not correct
 					if (!((face0 != null) && (face1 != null) && (face2 != null) && (face3 != null) && face0.getDistSquare() > 0.0f && face1.getDistSquare() > 0.0 && face2.getDistSquare() > 0.0f
 							&& face3.getDistSquare() > 0.0)) {
-						return;
+						return vector;
 					}
 					// Associate the edges of neighbouring triangle faces
 					TriangleEPA.link(new EdgeEPA(face0, 0), new EdgeEPA(face1, 2));
@@ -224,26 +224,26 @@ public class EPAAlgorithm {
 				// where "n" is the normal of the triangle. Then, we use only the
 				// tetrahedron that contains the origin.
 				// Compute the normal of the triangle
-				final Vector3f v1 = points[1].lessNew(points[0]);
-				final Vector3f v2 = points[2].lessNew(points[0]);
+				final Vector3f v1 = points[1].less(points[0]);
+				final Vector3f v2 = points[2].less(points[0]);
 				final Vector3f n = v1.cross(v2);
-				////Log.info(">>>>>>>>>>>>>>>>>>");
-				////Log.info("    v1 = " + v1);
-				////Log.info("    v2 = " + v2);
-				////Log.info("    n = " + n);
+				////LOGGER.info(">>>>>>>>>>>>>>>>>>");
+				////LOGGER.info("    v1 = " + v1);
+				////LOGGER.info("    v2 = " + v2);
+				////LOGGER.info("    n = " + n);
 				// Compute the two new vertices to obtain a hexahedron
 				suppPointsA[3] = shape1.getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
-				suppPointsB[3] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n.multiplyNew(-1)), shape2CachedCollisionData));
-				points[3] = suppPointsA[3].lessNew(suppPointsB[3]);
-				////Log.info("    suppPointsA[3]= " + suppPointsA[3]);
-				////Log.info("    suppPointsB[3]= " + suppPointsB[3]);
-				////Log.info("    points[3] = " + points[3]);
-				suppPointsA[4] = shape1.getLocalSupportPointWithMargin(n.multiplyNew(-1), shape1CachedCollisionData);
-				suppPointsB[4] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n), shape2CachedCollisionData));
-				points[4] = suppPointsA[4].lessNew(suppPointsB[4]);
-				////Log.info("    suppPointsA[4]= " + suppPointsA[4]);
-				////Log.info("    suppPointsB[4]= " + suppPointsB[4]);
-				////Log.info("    points[4]= " + points[4]);
+				suppPointsB[3] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n.multiply(-1)), shape2CachedCollisionData));
+				points[3] = suppPointsA[3].less(suppPointsB[3]);
+				////LOGGER.info("    suppPointsA[3]= " + suppPointsA[3]);
+				////LOGGER.info("    suppPointsB[3]= " + suppPointsB[3]);
+				////LOGGER.info("    points[3] = " + points[3]);
+				suppPointsA[4] = shape1.getLocalSupportPointWithMargin(n.multiply(-1), shape1CachedCollisionData);
+				suppPointsB[4] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n), shape2CachedCollisionData));
+				points[4] = suppPointsA[4].less(suppPointsB[4]);
+				////LOGGER.info("    suppPointsA[4]= " + suppPointsA[4]);
+				////LOGGER.info("    suppPointsB[4]= " + suppPointsB[4]);
+				////LOGGER.info("    points[4]= " + points[4]);
 				TriangleEPA face0 = null;
 				TriangleEPA face1 = null;
 				TriangleEPA face2 = null;
@@ -253,33 +253,33 @@ public class EPAAlgorithm {
 					// The tetrahedron is a correct initial polytope for the EPA algorithm.
 					// Therefore, we ruct the tetrahedron.
 					// Comstruct the 4 triangle faces of the tetrahedron
-					////Log.error("befor call: (points, 0, 1, 2)");
+					////LOGGER.error("befor call: (points, 0, 1, 2)");
 					face0 = triangleStore.newTriangle(points, 0, 1, 2);
-					////Log.error("befor call: (points, 0, 2, 1)");
+					////LOGGER.error("befor call: (points, 0, 2, 1)");
 					face1 = triangleStore.newTriangle(points, 0, 3, 1);
-					////Log.error("befor call: (points, 0, 2, 3)");
+					////LOGGER.error("befor call: (points, 0, 2, 3)");
 					face2 = triangleStore.newTriangle(points, 0, 2, 3);
-					////Log.error("befor call: (points, 1, 3, 2)");
+					////LOGGER.error("befor call: (points, 1, 3, 2)");
 					face3 = triangleStore.newTriangle(points, 1, 3, 2);
 				} else if (isOriginInTetrahedron(points[0], points[1], points[2], points[4]) == 0) {
 					// The tetrahedron is a correct initial polytope for the EPA algorithm.
 					// Therefore, we ruct the tetrahedron.
 					// Comstruct the 4 triangle faces of the tetrahedron
-					////Log.error("befor call: (points, 0, 1, 2)");
+					////LOGGER.error("befor call: (points, 0, 1, 2)");
 					face0 = triangleStore.newTriangle(points, 0, 1, 2);
-					////Log.error("befor call: (points, 0, 4, 1)");
+					////LOGGER.error("befor call: (points, 0, 4, 1)");
 					face1 = triangleStore.newTriangle(points, 0, 4, 1);
-					////Log.error("befor call: (points, 0, 2, 4)");
+					////LOGGER.error("befor call: (points, 0, 2, 4)");
 					face2 = triangleStore.newTriangle(points, 0, 2, 4);
-					////Log.error("befor call: (points, 1, 4, 2)");
+					////LOGGER.error("befor call: (points, 1, 4, 2)");
 					face3 = triangleStore.newTriangle(points, 1, 4, 2);
 				} else {
-					return;
+					return vector;
 				}
 				// If the ructed tetrahedron is not correct
 				if (!(face0 != null && face1 != null && face2 != null && face3 != null && face0.getDistSquare() > 0.0f && face1.getDistSquare() > 0.0f && face2.getDistSquare() > 0.0f
 						&& face3.getDistSquare() > 0.0f)) {
-					return;
+					return vector;
 				}
 				// Associate the edges of neighbouring triangle faces
 				TriangleEPA.link(new EdgeEPA(face0, 0), new EdgeEPA(face1, 2));
@@ -296,21 +296,23 @@ public class EPAAlgorithm {
 				nbVertices = 4;
 			}
 				break;
+			default:
+				break;
 		}
 		// At this point, we have a polytope that contains the origin. Therefore, we
 		// can run the EPA algorithm.
 		if (triangleHeap.size() == 0) {
-			return;
+			return vector;
 		}
 		TriangleEPA triangle = null;
 		float upperBoundSquarePenDepth = Float.MAX_VALUE;
 		do {
 			triangle = triangleHeap.first();
 			triangleHeap.remove(triangle);
-			////Log.info("rm from heap:");
+			////LOGGER.info("rm from heap:");
 			int iii = 0;
 			for (final TriangleEPA elem : triangleHeap) {
-				////Log.info("    [" + iii + "] " + elem.getDistSquare());
+				////LOGGER.info("    [" + iii + "] " + elem.getDistSquare());
 				++iii;
 			}
 			// If the candidate face in the heap is not obsolete
@@ -323,18 +325,17 @@ public class EPAAlgorithm {
 				// Compute the support point of the Minkowski
 				// difference (A-B) in the closest point direction
 				suppPointsA[nbVertices] = shape1.getLocalSupportPointWithMargin(triangle.getClosestPoint(), shape1CachedCollisionData);
-				suppPointsB[nbVertices] = body2Tobody1
-						.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(triangle.getClosestPoint().multiplyNew(-1)), shape2CachedCollisionData));
-				points[nbVertices] = suppPointsA[nbVertices].lessNew(suppPointsB[nbVertices]);
+				suppPointsB[nbVertices] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(triangle.getClosestPoint().multiply(-1)), shape2CachedCollisionData));
+				points[nbVertices] = suppPointsA[nbVertices].less(suppPointsB[nbVertices]);
 				final int indexNewVertex = nbVertices;
 				nbVertices++;
 				// Update the upper bound of the penetration depth
 				final float wDotv = points[indexNewVertex].dot(triangle.getClosestPoint());
-				////Log.info("      point=" + points[indexNewVertex]);
-				////Log.info("close point=" + triangle.getClosestPoint());
-				////Log.info("         ==>" + wDotv);
+				////LOGGER.info("      point=" + points[indexNewVertex]);
+				////LOGGER.info("close point=" + triangle.getClosestPoint());
+				////LOGGER.info("         ==>" + wDotv);
 				if (wDotv < 0.0f) {
-					////Log.error("depth penetration error " + wDotv);
+					////LOGGER.error("depth penetration error " + wDotv);
 					continue;
 				}
 				assert (wDotv >= 0.0f);
@@ -365,20 +366,21 @@ public class EPAAlgorithm {
 			}
 		} while (triangleHeap.size() > 0 && triangleHeap.first().getDistSquare() <= upperBoundSquarePenDepth);
 		// Compute the contact info
-		final Vector3f tmp = _transform1.getOrientation().multiply(triangle.getClosestPoint());
-		_vector.set(tmp);
+		final Vector3f tmp = transform1.getOrientation().multiply(triangle.getClosestPoint());
+		vector = tmp;
 		final Vector3f pALocal = triangle.computeClosestPointOfObject(suppPointsA);
 		final Vector3f pBLocal = body2Tobody1.inverseNew().multiply(triangle.computeClosestPointOfObject(suppPointsB));
-		final Vector3f normal = _vector.safeNormalizeNew();
-		final float penetrationDepth = _vector.length();
+		final Vector3f normal = vector.safeNormalize();
+		final float penetrationDepth = vector.length();
 		assert (penetrationDepth >= 0.0f);
 		if (normal.length2() < Constant.FLOAT_EPSILON) {
-			return;
+			return vector;
 		}
 		// Create the contact info object
-		final ContactPointInfo contactInfo = new ContactPointInfo(_shape1Info.proxyShape, _shape2Info.proxyShape, _shape1Info.collisionShape, _shape2Info.collisionShape, normal, penetrationDepth,
+		final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape(), shape2Info.proxyShape(), shape1Info.collisionShape(), shape2Info.collisionShape(), normal, penetrationDepth,
 				pALocal, pBLocal);
-		_narrowPhaseCallback.notifyContact(_shape1Info.overlappingPair, contactInfo);
+		narrowPhaseCallback.notifyContact(shape1Info.overlappingPair(), contactInfo);
+		return vector;
 	}
 	
 	/// Initalize the algorithm
@@ -389,33 +391,33 @@ public class EPAAlgorithm {
 	// Decide if the origin is in the tetrahedron.
 	/// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
 	/// the vertex that is wrong if the origin is not in the tetrahedron
-	private int isOriginInTetrahedron(final Vector3f _p1, final Vector3f _p2, final Vector3f _p3, final Vector3f _p4) {
-		////Log.error("isOriginInTetrahedron(" + _p1 + ", " + _p2 + ", " + _p3 + ", " + _p4 + ")");
+	private int isOriginInTetrahedron(final Vector3f p1, final Vector3f p2, final Vector3f p3, final Vector3f p4) {
+		////LOGGER.error("isOriginInTetrahedron(" + p1 + ", " + p2 + ", " + p3 + ", " + p4 + ")");
 		// Check vertex 1
-		final Vector3f normal1 = _p2.lessNew(_p1).cross(_p3.lessNew(_p1));
-		if ((normal1.dot(_p1) > 0.0f) == (normal1.dot(_p4) > 0.0)) {
-			////Log.error("    ==> 4");
+		final Vector3f normal1 = p2.less(p1).cross(p3.less(p1));
+		if ((normal1.dot(p1) > 0.0f) == (normal1.dot(p4) > 0.0)) {
+			////LOGGER.error("    ==> 4");
 			return 4;
 		}
 		// Check vertex 2
-		final Vector3f normal2 = _p4.lessNew(_p2).cross(_p3.lessNew(_p2));
-		if ((normal2.dot(_p2) > 0.0f) == (normal2.dot(_p1) > 0.0)) {
-			////Log.error("    ==> 1");
+		final Vector3f normal2 = p4.less(p2).cross(p3.less(p2));
+		if ((normal2.dot(p2) > 0.0f) == (normal2.dot(p1) > 0.0)) {
+			////LOGGER.error("    ==> 1");
 			return 1;
 		}
 		// Check vertex 3
-		final Vector3f normal3 = _p4.lessNew(_p3).cross(_p1.lessNew(_p3));
-		if ((normal3.dot(_p3) > 0.0f) == (normal3.dot(_p2) > 0.0)) {
-			////Log.error("    ==> 2");
+		final Vector3f normal3 = p4.less(p3).cross(p1.less(p3));
+		if ((normal3.dot(p3) > 0.0f) == (normal3.dot(p2) > 0.0)) {
+			////LOGGER.error("    ==> 2");
 			return 2;
 		}
 		// Check vertex 4
-		final Vector3f normal4 = _p2.lessNew(_p4).cross(_p1.lessNew(_p4));
-		if ((normal4.dot(_p4) > 0.0f) == (normal4.dot(_p3) > 0.0)) {
-			////Log.error("    ==> 3");
+		final Vector3f normal4 = p2.less(p4).cross(p1.less(p4));
+		if ((normal4.dot(p4) > 0.0f) == (normal4.dot(p3) > 0.0)) {
+			////LOGGER.error("    ==> 3");
 			return 3;
 		}
-		////Log.error("    ==> 0");
+		////LOGGER.error("    ==> 0");
 		// The origin is in the tetrahedron, we return 0
 		return 0;
 	}

src/org/atriasoft/ephysics/collision/narrowphase/EPA/EdgeEPA.java

@@ -17,31 +17,31 @@ import org.atriasoft.etk.math.Vector3f;
  * algorithm is based on the book "Collision Detection in 3D Environments".
  */
 public class EdgeEPA implements Cloneable {
-	/// Maximum number of support points of the polytope
-	static public final int MAX_SUPPORT_POINTS = 100;
 	/// Maximum number of facets of the polytope
-	static public final int MAX_FACETS = 200;
+	public static final int MAX_FACETS = 200;
+	/// Maximum number of support points of the polytope
+	public static final int MAX_SUPPORT_POINTS = 100;
 	
 	// Return the index of the next counter-clockwise edge of the ownver triangle
-	static public int indexOfNextCounterClockwiseEdge(final int iii) {
+	public static int indexOfNextCounterClockwiseEdge(final int iii) {
 		return (iii + 1) % 3;
 	}
 	
 	// Return the index of the previous counter-clockwise edge of the ownver triangle
-	static public int indexOfPreviousCounterClockwiseEdge(final int iii) {
+	public static int indexOfPreviousCounterClockwiseEdge(final int iii) {
 		return (iii + 2) % 3;
 	}
 	
-	/// Pointer to the triangle that contains this edge
-	private TriangleEPA ownerTriangle;
 	/// Index of the edge in the triangle (between 0 and 2).
 	/// The edge with index i connect triangle vertices i and (i+1 % 3)
 	private int index;
+	/// Pointer to the triangle that contains this edge
+	private TriangleEPA ownerTriangle;
 	
 	/// Constructor
 	public EdgeEPA() {
 		this.ownerTriangle = null;
-	};
+	}
 	
 	/// Copy-ructor
 	public EdgeEPA(final EdgeEPA obj) {
@@ -63,12 +63,12 @@ public class EdgeEPA implements Cloneable {
 	
 	/// Execute the recursive silhouette algorithm from this edge
 	public boolean computeSilhouette(final Vector3f[] vertices, final int indexNewVertex, final TrianglesStore triangleStore) {
-		//Log.error("EdgeEPA computeSilhouette ...");
+		//LOGGER.error("EdgeEPA computeSilhouette ...");
 		// If the edge has not already been visited
 		if (!this.ownerTriangle.getIsObsolete()) {
 			// If the triangle of this edge is not visible from the given point
 			if (!this.ownerTriangle.isVisibleFromVertex(vertices, indexNewVertex)) {
-				//Log.error("EdgeEPA 1 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex());
+				//LOGGER.error("EdgeEPA 1 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex());
 				final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
 				// If the triangle has been created
 				if (triangle != null) {
@@ -76,31 +76,30 @@ public class EdgeEPA implements Cloneable {
 					return true;
 				}
 				return false;
-			} else {
-				// The current triangle is visible and therefore obsolete
-				this.ownerTriangle.setIsObsolete(true);
-				final int backup = triangleStore.getNbTriangles();
-				if (!this.ownerTriangle.getAdjacentEdge(indexOfNextCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
-					this.ownerTriangle.setIsObsolete(false);
-					//Log.error("EdgeEPA 2 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex());
-					final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
-					// If the triangle has been created
-					if (triangle != null) {
-						TriangleEPA.halfLink(new EdgeEPA(triangle, 1), this);
-						return true;
-					}
-					return false;
-				} else if (!this.ownerTriangle.getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
-					this.ownerTriangle.setIsObsolete(false);
-					triangleStore.resize(backup);
-					//Log.error("EdgeEPA 3 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex());
-					final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
-					if (triangle != null) {
-						TriangleEPA.halfLink(new EdgeEPA(triangle, 1), this);
-						return true;
-					}
-					return false;
+			}
+			// The current triangle is visible and therefore obsolete
+			this.ownerTriangle.setIsObsolete(true);
+			final int backup = triangleStore.getNbTriangles();
+			if (!this.ownerTriangle.getAdjacentEdge(EdgeEPA.indexOfNextCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
+				this.ownerTriangle.setIsObsolete(false);
+				//LOGGER.error("EdgeEPA 2 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex());
+				final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
+				// If the triangle has been created
+				if (triangle != null) {
+					TriangleEPA.halfLink(new EdgeEPA(triangle, 1), this);
+					return true;
 				}
+				return false;
+			} else if (!this.ownerTriangle.getAdjacentEdge(EdgeEPA.indexOfPreviousCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
+				this.ownerTriangle.setIsObsolete(false);
+				triangleStore.resize(backup);
+				//LOGGER.error("EdgeEPA 3 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex());
+				final TriangleEPA triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
+				if (triangle != null) {
+					TriangleEPA.halfLink(new EdgeEPA(triangle, 1), this);
+					return true;
+				}
+				return false;
 			}
 		}
 		return true;
@@ -124,7 +123,7 @@ public class EdgeEPA implements Cloneable {
 	
 	/// Return the index of the target vertex of the edge
 	public int getTargetVertexIndex() {
-		return this.ownerTriangle.get(indexOfNextCounterClockwiseEdge(this.index));
+		return this.ownerTriangle.get(EdgeEPA.indexOfNextCounterClockwiseEdge(this.index));
 		//return this.ownerTriangle[indexOfNextCounterClockwiseEdge(this.index)];
 	}
 	

src/org/atriasoft/ephysics/collision/narrowphase/EPA/TriangleEPA.java

@@ -11,30 +11,30 @@ public class TriangleEPA {
 	/// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an
 	/// adjacent edge of "edge0" but not the opposite. The opposite edge connection will
 	/// be made later.
-	public static void halfLink(final EdgeEPA _edge0, final EdgeEPA _edge1) {
-		assert (_edge0.getSourceVertexIndex() == _edge1.getTargetVertexIndex() && _edge0.getTargetVertexIndex() == _edge1.getSourceVertexIndex());
-		_edge0.getOwnerTriangle().adjacentEdges[_edge0.getIndex()] = _edge1;
+	public static void halfLink(final EdgeEPA edge0, final EdgeEPA edge1) {
+		assert (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
+		edge0.getOwnerTriangle().adjacentEdges[edge0.getIndex()] = edge1;
 	}
 	
-	public static boolean link(final EdgeEPA _edge0, final EdgeEPA _edge1) {
-		if (_edge0.getSourceVertexIndex() == _edge1.getTargetVertexIndex() && _edge0.getTargetVertexIndex() == _edge1.getSourceVertexIndex()) {
-			_edge0.getOwnerTriangle().adjacentEdges[_edge0.getIndex()] = _edge1;
-			_edge1.getOwnerTriangle().adjacentEdges[_edge1.getIndex()] = _edge0;
+	public static boolean link(final EdgeEPA edge0, final EdgeEPA edge1) {
+		if (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()) {
+			edge0.getOwnerTriangle().adjacentEdges[edge0.getIndex()] = edge1;
+			edge1.getOwnerTriangle().adjacentEdges[edge1.getIndex()] = edge0;
 			return true;
 		}
 		return false;
 	}
 	
-	private final int[] indicesVertices = new int[3]; //!< Indices of the vertices y_i of the triangle
 	private final EdgeEPA[] adjacentEdges = new EdgeEPA[3]; //!< Three adjacent edges of the triangle (edges of other triangles)
-	private boolean isObsolete; //!< True if the triangle face is visible from the new support point
-	private float determinant; //!< Determinant
 	private Vector3f closestPoint; //!< Point v closest to the origin on the affine hull of the triangle
-	private float lambda1; //!< Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
-	
-	private float lambda2; //!< Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
-	
+	private float determinant; //!< Determinant
 	private float distSquare; //!< Square distance of the point closest point v to the origin
+	private final int[] indicesVertices = new int[3]; //!< Indices of the vertices yi of the triangle
+	private boolean isObsolete; //!< True if the triangle face is visible from the new support point
+	
+	private float lambda1; //!< Lambda1 value such that v = lambda0 * y0 + lambda1 * y1 + lambda2 * y2
+	
+	private float lambda2; //!< Lambda1 value such that v = lambda0 * y0 + lambda1 * y1 + lambda2 * y2
 	
 	/// Constructor
 	/*
@@ -47,40 +47,40 @@ public class TriangleEPA {
 	*/
 	
 	/// Constructor
-	public TriangleEPA(final int _indexVertex1, final int _indexVertex2, final int _indexVertex3) {
+	public TriangleEPA(final int indexVertex1, final int indexVertex2, final int indexVertex3) {
 		this.isObsolete = false;
-		this.indicesVertices[0] = _indexVertex1;
-		this.indicesVertices[1] = _indexVertex2;
-		this.indicesVertices[2] = _indexVertex3;
+		this.indicesVertices[0] = indexVertex1;
+		this.indicesVertices[1] = indexVertex2;
+		this.indicesVertices[2] = indexVertex3;
 		this.adjacentEdges[0] = new EdgeEPA();
 		this.adjacentEdges[1] = new EdgeEPA();
 		this.adjacentEdges[2] = new EdgeEPA();
-		this.closestPoint = new Vector3f();
+		this.closestPoint = Vector3f.ZERO;
 	}
 	
 	/// Private copy-ructor
 	/*
-	public TriangleEPA(final TriangleEPA _triangle) {
-		this.indicesVertices[0] = _triangle.indicesVertices[0];
-		this.indicesVertices[1] = _triangle.indicesVertices[1];
-		this.indicesVertices[2] = _triangle.indicesVertices[2];
-		this.adjacentEdges[0] = _triangle.adjacentEdges[0];
-		this.adjacentEdges[1] = _triangle.adjacentEdges[1];
-		this.adjacentEdges[2] = _triangle.adjacentEdges[2];
-		this.isObsolete = _triangle.isObsolete;
-		this.determinant = _triangle.determinant;
-		this.closestPoint = _triangle.closestPoint;
-		this.lambda1 = _triangle.lambda1;
-		this.lambda2 = _triangle.lambda2;
-		this.distSquare = _triangle.distSquare;
+	public TriangleEPA(final TriangleEPA triangle) {
+		this.indicesVertices[0] = triangle.indicesVertices[0];
+		this.indicesVertices[1] = triangle.indicesVertices[1];
+		this.indicesVertices[2] = triangle.indicesVertices[2];
+		this.adjacentEdges[0] = triangle.adjacentEdges[0];
+		this.adjacentEdges[1] = triangle.adjacentEdges[1];
+		this.adjacentEdges[2] = triangle.adjacentEdges[2];
+		this.isObsolete = triangle.isObsolete;
+		this.determinant = triangle.determinant;
+		this.closestPoint = triangle.closestPoint;
+		this.lambda1 = triangle.lambda1;
+		this.lambda2 = triangle.lambda2;
+		this.distSquare = triangle.distSquare;
 	}
 	*/
 	
 	/// Compute the point v closest to the origin of this triangle
-	public boolean computeClosestPoint(final Vector3f[] _vertices) {
-		final Vector3f p0 = _vertices[this.indicesVertices[0]];
-		final Vector3f v1 = _vertices[this.indicesVertices[1]].lessNew(p0);
-		final Vector3f v2 = _vertices[this.indicesVertices[2]].lessNew(p0);
+	public boolean computeClosestPoint(final Vector3f[] vertices) {
+		final Vector3f p0 = vertices[this.indicesVertices[0]];
+		final Vector3f v1 = vertices[this.indicesVertices[1]].less(p0);
+		final Vector3f v2 = vertices[this.indicesVertices[2]].less(p0);
 		final float v1Dotv1 = v1.dot(v1);
 		final float v1Dotv2 = v1.dot(v2);
 		final float v2Dotv2 = v2.dot(v2);
@@ -94,7 +94,7 @@ public class TriangleEPA {
 		// If the determinant is positive
 		if (this.determinant > 0.0f) {
 			// Compute the closest point v
-			this.closestPoint = v1.multiplyNew(this.lambda1).add(v2.multiplyNew(this.lambda2)).multiply(1.0f / this.determinant).add(p0);
+			this.closestPoint = v1.multiply(this.lambda1).add(v2.multiply(this.lambda2)).multiply(1.0f / this.determinant).add(p0);
 			// Compute the square distance of closest point to the origin
 			this.distSquare = this.closestPoint.dot(this.closestPoint);
 			return true;
@@ -103,10 +103,10 @@ public class TriangleEPA {
 	}
 	
 	/// Compute the point of an object closest to the origin
-	public Vector3f computeClosestPointOfObject(final Vector3f[] _supportPointsOfObject) {
-		final Vector3f p0 = _supportPointsOfObject[this.indicesVertices[0]].clone();
-		final Vector3f tmp1 = _supportPointsOfObject[this.indicesVertices[1]].lessNew(p0).multiply(this.lambda1);
-		final Vector3f tmp2 = _supportPointsOfObject[this.indicesVertices[2]].lessNew(p0).multiply(this.lambda2);
+	public Vector3f computeClosestPointOfObject(final Vector3f[] supportPointsOfObject) {
+		final Vector3f p0 = supportPointsOfObject[this.indicesVertices[0]];
+		final Vector3f tmp1 = supportPointsOfObject[this.indicesVertices[1]].less(p0).multiply(this.lambda1);
+		final Vector3f tmp2 = supportPointsOfObject[this.indicesVertices[2]].less(p0).multiply(this.lambda2);
 		return p0.add(tmp1.add(tmp2).multiply(1.0f / this.determinant));
 	}
 	
@@ -121,21 +121,21 @@ public class TriangleEPA {
 	/// face from the new vertex, computes the silhouette and create the new faces from the new vertex in
 	/// order that we always have a convex polytope. The faces visible from the new vertex are set
 	/// obselete and will not be idered as being a candidate face in the future.
-	public boolean computeSilhouette(final Vector3f[] _vertices, final int _indexNewVertex, final TrianglesStore _triangleStore) {
-		final int first = _triangleStore.getNbTriangles();
+	public boolean computeSilhouette(final Vector3f[] vertices, final int indexNewVertex, final TrianglesStore triangleStore) {
+		final int first = triangleStore.getNbTriangles();
 		// Mark the current triangle as obsolete because it
 		setIsObsolete(true);
 		// Execute recursively the silhouette algorithm for the adjacent edges of neighboring
 		// triangles of the current triangle
-		final boolean result = this.adjacentEdges[0].computeSilhouette(_vertices, _indexNewVertex, _triangleStore)
-				&& this.adjacentEdges[1].computeSilhouette(_vertices, _indexNewVertex, _triangleStore) && this.adjacentEdges[2].computeSilhouette(_vertices, _indexNewVertex, _triangleStore);
+		final boolean result = this.adjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) && this.adjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore)
+				&& this.adjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore);
 		if (result) {
 			int i, j;
 			// For each triangle face that contains the new vertex and an edge of the silhouette
-			for (i = first, j = _triangleStore.getNbTriangles() - 1; i != _triangleStore.getNbTriangles(); j = i++) {
-				final TriangleEPA triangle = _triangleStore.get(i);
-				halfLink(triangle.getAdjacentEdge(1), new EdgeEPA(triangle, 1));
-				if (!link(new EdgeEPA(triangle, 0), new EdgeEPA(_triangleStore.get(j), 2))) {
+			for (i = first, j = triangleStore.getNbTriangles() - 1; i != triangleStore.getNbTriangles(); j = i++) {
+				final TriangleEPA triangle = triangleStore.get(i);
+				TriangleEPA.halfLink(triangle.getAdjacentEdge(1), new EdgeEPA(triangle, 1));
+				if (!TriangleEPA.link(new EdgeEPA(triangle, 0), new EdgeEPA(triangleStore.get(j), 2))) {
 					return false;
 				}
 			}
@@ -144,18 +144,18 @@ public class TriangleEPA {
 	}
 	
 	/// Access operator
-	public int get(final int _pos) {
-		assert (_pos >= 0 && _pos < 3);
-		return this.indicesVertices[_pos];
+	public int get(final int pos) {
+		assert (pos >= 0 && pos < 3);
+		return this.indicesVertices[pos];
 	}
 	/// Link an edge with another one. It means that the current edge of a triangle will
 	/// be associated with the edge of another triangle in order that both triangles
 	/// are neighbour aint both edges).
 	
 	/// Return an adjacent edge of the triangle
-	public EdgeEPA getAdjacentEdge(final int _index) {
-		assert (_index >= 0 && _index < 3);
-		return this.adjacentEdges[_index];
+	public EdgeEPA getAdjacentEdge(final int index) {
+		assert (index >= 0 && index < 3);
+		return this.adjacentEdges[index];
 	}
 	
 	/// Return the point closest to the origin
@@ -179,45 +179,45 @@ public class TriangleEPA {
 	}
 	
 	/// Return true if the triangle is visible from a given vertex
-	public boolean isVisibleFromVertex(final Vector3f[] _vertices, final int _index) {
-		final Vector3f closestToVert = _vertices[_index].lessNew(this.closestPoint);
+	public boolean isVisibleFromVertex(final Vector3f[] vertices, final int index) {
+		final Vector3f closestToVert = vertices[index].less(this.closestPoint);
 		return (this.closestPoint.dot(closestToVert) > 0.0f);
 	}
 	
 	/// Constructor
-	public void set(final int _indexVertex1, final int _indexVertex2, final int _indexVertex3) {
+	public void set(final int indexVertex1, final int indexVertex2, final int indexVertex3) {
 		this.isObsolete = false;
-		this.indicesVertices[0] = _indexVertex1;
-		this.indicesVertices[1] = _indexVertex2;
-		this.indicesVertices[2] = _indexVertex3;
+		this.indicesVertices[0] = indexVertex1;
+		this.indicesVertices[1] = indexVertex2;
+		this.indicesVertices[2] = indexVertex3;
 	}
 	
 	/// Private assignment operator
-	public TriangleEPA set(final TriangleEPA _triangle) {
-		this.indicesVertices[0] = _triangle.indicesVertices[0];
-		this.indicesVertices[1] = _triangle.indicesVertices[1];
-		this.indicesVertices[2] = _triangle.indicesVertices[2];
-		this.adjacentEdges[0] = _triangle.adjacentEdges[0];
-		this.adjacentEdges[1] = _triangle.adjacentEdges[1];
-		this.adjacentEdges[2] = _triangle.adjacentEdges[2];
-		this.isObsolete = _triangle.isObsolete;
-		this.determinant = _triangle.determinant;
-		this.closestPoint = _triangle.closestPoint;
-		this.lambda1 = _triangle.lambda1;
-		this.lambda2 = _triangle.lambda2;
-		this.distSquare = _triangle.distSquare;
+	public TriangleEPA set(final TriangleEPA triangle) {
+		this.indicesVertices[0] = triangle.indicesVertices[0];
+		this.indicesVertices[1] = triangle.indicesVertices[1];
+		this.indicesVertices[2] = triangle.indicesVertices[2];
+		this.adjacentEdges[0] = triangle.adjacentEdges[0];
+		this.adjacentEdges[1] = triangle.adjacentEdges[1];
+		this.adjacentEdges[2] = triangle.adjacentEdges[2];
+		this.isObsolete = triangle.isObsolete;
+		this.determinant = triangle.determinant;
+		this.closestPoint = triangle.closestPoint;
+		this.lambda1 = triangle.lambda1;
+		this.lambda2 = triangle.lambda2;
+		this.distSquare = triangle.distSquare;
 		return this;
 	}
 	
 	/// Set an adjacent edge of the triangle
-	public void setAdjacentEdge(final int _index, final EdgeEPA _edge) {
-		assert (_index >= 0 && _index < 3);
-		this.adjacentEdges[_index] = _edge;
+	public void setAdjacentEdge(final int index, final EdgeEPA edge) {
+		assert (index >= 0 && index < 3);
+		this.adjacentEdges[index] = edge;
 	}
 	
 	/// Set the isObsolete value
-	public void setIsObsolete(final boolean _isObsolete) {
-		this.isObsolete = _isObsolete;
+	public void setIsObsolete(final boolean isObsolete) {
+		this.isObsolete = isObsolete;
 	}
 	
 }

src/org/atriasoft/ephysics/collision/narrowphase/EPA/TrianglesStore.java

@@ -3,12 +3,14 @@ package org.atriasoft.ephysics.collision.narrowphase.EPA;
 import java.util.ArrayList;
 import java.util.List;
 
-import org.atriasoft.ephysics.internal.Log;
 import org.atriasoft.etk.math.Vector3f;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 public class TrianglesStore {
-	private static int MAX_TRIANGLES = 200;
-	
+	static final Logger LOGGER = LoggerFactory.getLogger(TrianglesStore.class);
+	private static final int MAX_TRIANGLES = 200;
+
 	public static void shrinkTo(final List list, final int newSize) {
 		final int size = list.size();
 		if (newSize >= size) {
@@ -18,53 +20,53 @@ public class TrianglesStore {
 			list.remove(list.size() - 1);
 		}
 	}
-	
+
 	private final List<TriangleEPA> triangles = new ArrayList<>();
-	
+
 	/// Clear all the storage
 	public void clear() {
 		this.triangles.clear();
 	}
-	
+
 	/// Access operator
-	public TriangleEPA get(final int _id) {
-		return this.triangles.get(_id);
+	public TriangleEPA get(final int id) {
+		return this.triangles.get(id);
 	}
-	
+
 	/// Return the number of triangles
 	public int getNbTriangles() {
 		return this.triangles.size();
 	}
-	
+
 	/// Return the last triangle
 	public TriangleEPA last() {
 		return this.triangles.get(this.triangles.size() - 1);
 	}
-	
+
 	/// Create a new triangle
-	public TriangleEPA newTriangle(final Vector3f[] _vertices, final int _v0, final int _v1, final int _v2) {
-		//Log.info("newTriangle: " + _v0 + ", " + _v1 + ", " + _v2);
+	public TriangleEPA newTriangle(final Vector3f[] vertices, final int v0, final int v1, final int v2) {
+		//LOGGER.info("newTriangle: " + v0 + ", " + v1 + ", " + v2);
 		// If we have not reached the maximum number of triangles
-		if (this.triangles.size() < MAX_TRIANGLES) {
-			
-			final TriangleEPA tmp = new TriangleEPA(_v0, _v1, _v2);
-			if (!tmp.computeClosestPoint(_vertices)) {
+		if (this.triangles.size() < TrianglesStore.MAX_TRIANGLES) {
+
+			final TriangleEPA tmp = new TriangleEPA(v0, v1, v2);
+			if (!tmp.computeClosestPoint(vertices)) {
 				return null;
 			}
 			this.triangles.add(tmp);
-			//Log.info(" ==> retrurn Triangle: " + tmp.get(0) + ", " + tmp.get(1) + ", " + tmp.get(2));
+			//LOGGER.info(" ==> retrurn Triangle: " + tmp.get(0) + ", " + tmp.get(1) + ", " + tmp.get(2));
 			return tmp;
 		}
 		// We are at the limit (internal)
 		return null;
-		
+
 	}
-	
+
 	/// Set the number of triangles
-	public void resize(final int _backup) {
-		if (_backup > this.triangles.size()) {
-			Log.error("RESIZE BIGGER : " + _backup + " > " + this.triangles.size());
+	public void resize(final int backup) {
+		if (backup > this.triangles.size()) {
+			LOGGER.error("RESIZE BIGGER : " + backup + " > " + this.triangles.size());
 		}
-		shrinkTo(this.triangles, _backup);
+		TrianglesStore.shrinkTo(this.triangles, backup);
 	}
 }

src/org/atriasoft/ephysics/collision/narrowphase/GJK/GJKAlgorithm.java

@@ -33,10 +33,10 @@ import org.atriasoft.etk.math.Vector3f;
  * enlarged objects.
  */
 public class GJKAlgorithm extends NarrowPhaseAlgorithm {
-	public static final float REL_ERROR = 0.001f;
-	
-	public static final float REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
 	public static final int MAX_ITERATIONS_GJK_RAYCAST = 32;
+	
+	public static final float REL_ERROR = 0.001f;
+	public static final float REL_ERROR_SQUARE = GJKAlgorithm.REL_ERROR * GJKAlgorithm.REL_ERROR;
 	private final EPAAlgorithm algoEPA; //!< EPA Algorithm
 	/// This method runs the GJK algorithm on the two enlarged objects (with margin)
 	/// to compute a simplex polytope that contains the origin. The two objects are
@@ -50,70 +50,73 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 		this.algoEPA.init();
 	}
 	
-	private void computePenetrationDepthForEnlargedObjects(final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info, final Transform3D transform2,
-			final NarrowPhaseCallback narrowPhaseCallback, final Vector3f v) {
-		//Log.info("computePenetrationDepthForEnlargedObjects...");
+	private Vector3f computePenetrationDepthForEnlargedObjects(final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info, final Transform3D transform2,
+			final NarrowPhaseCallback narrowPhaseCallback, Vector3f v) {
+		//LOGGER.info("computePenetrationDepthForEnlargedObjects...");
 		final Simplex simplex = new Simplex();
 		float distSquare = Float.MAX_VALUE;
 		float prevDistSquare;
-		assert (shape1Info.collisionShape.isConvex());
-		assert (shape2Info.collisionShape.isConvex());
-		final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape);
-		final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape);
-		final Object shape1CachedCollisionData = shape1Info.cachedCollisionData;
-		final Object shape2CachedCollisionData = shape2Info.cachedCollisionData;
-		//Log.info("    transform1=" + transform1);
-		//Log.info("    transform2=" + transform2);
+		assert (shape1Info.collisionShape().isConvex());
+		assert (shape2Info.collisionShape().isConvex());
+		final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape());
+		final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape());
+		final Object shape1CachedCollisionData = shape1Info.cachedCollisionData();
+		final Object shape2CachedCollisionData = shape2Info.cachedCollisionData();
+		//LOGGER.info("    transform1=" + transform1);
+		//LOGGER.info("    transform2=" + transform2);
 		
 		// Transform3D a point from local space of body 2 to local space
 		// of body 1 (the GJK algorithm is done in local space of body 1)
-		final Transform3D body2ToBody1 = transform1.inverseNew().multiplyNew(transform2);
+		final Transform3D body2ToBody1 = transform1.inverseNew().multiply(transform2);
 		// Matrix that transform a direction from local space of body 1 into local space of body 2
-		final Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().transposeNew().multiply(transform1.getOrientation().getMatrix());
-		//Log.info("    body2ToBody1=" + body2ToBody1);
-		//Log.info("    rotateToBody2=" + rotateToBody2);
-		//Log.info("    v=" + v);
+		final Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().transpose().multiply(transform1.getOrientation().getMatrix());
+		//LOGGER.info("    body2ToBody1=" + body2ToBody1);
+		//LOGGER.info("    rotateToBody2=" + rotateToBody2);
+		//LOGGER.info("    v=" + v);
 		do {
 			// Compute the support points for the enlarged object A and B
-			final Vector3f suppA = shape1.getLocalSupportPointWithMargin(v.multiplyNew(-1), (CacheData) shape1CachedCollisionData);
-			//Log.info("    suppA=" + suppA);
-			final Vector3f suppB = body2ToBody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiplyNew(v), (CacheData) shape2CachedCollisionData));
-			//Log.info("    suppB=" + suppB);
+			final Vector3f suppA = shape1.getLocalSupportPointWithMargin(v.multiply(-1), (CacheData) shape1CachedCollisionData);
+			//LOGGER.info("    suppA=" + suppA);
+			final Vector3f suppB = body2ToBody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v), (CacheData) shape2CachedCollisionData));
+			//LOGGER.info("    suppB=" + suppB);
 			// Compute the support point for the Minkowski difference A-B
-			final Vector3f w = suppA.lessNew(suppB);
+			final Vector3f w = suppA.less(suppB);
 			final float vDotw = v.dot(w);
-			//Log.info("    vDotw=" + vDotw);
+			//LOGGER.info("    vDotw=" + vDotw);
 			// If the enlarge objects do not intersect
 			if (vDotw > 0.0f) {
-				//Log.info("        ==> ret 1");
+				//LOGGER.info("        ==> ret 1");
 				// No intersection, we return
-				return;
+				return v;
 			}
 			// Add the new support point to the simplex
 			simplex.addPoint(w, suppA, suppB);
 			if (simplex.isAffinelyDependent()) {
-				//Log.info("        ==> ret 2");
-				return;
+				//LOGGER.info("        ==> ret 2");
+				return v;
 			}
-			if (!simplex.computeClosestPoint(v)) {
-				//Log.info("        ==> ret 3");
-				return;
+			Vector3f v2 = simplex.computeClosestPoint(v);
+			if (v2 == null) {
+				//LOGGER.info("        ==> ret 3");
+				return v;
 			}
+			v = v2;
 			// Store and update the square distance
 			prevDistSquare = distSquare;
-			//Log.info("    distSquare=" + distSquare);
+			//LOGGER.info("    distSquare=" + distSquare);
 			distSquare = v.length2();
-			//Log.info("    distSquare=" + distSquare);
+			//LOGGER.info("    distSquare=" + distSquare);
 			if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) {
-				//Log.info("        ==> ret 4");
-				return;
+				//LOGGER.info("        ==> ret 4");
+				return v;
 			}
 		} while (!simplex.isFull() && distSquare > Constant.FLOAT_EPSILON * simplex.getMaxLengthSquareOfAPoint());
 		// Give the simplex computed with GJK algorithm to the EPA algorithm
 		// which will compute the correct penetration depth and contact points
 		// between the two enlarged objects
-		//Log.info("        ==> ret 5");
-		this.algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, transform1, shape2Info, transform2, v, narrowPhaseCallback);
+		//LOGGER.info("        ==> ret 5");
+		v = this.algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info, transform1, shape2Info, transform2, v, narrowPhaseCallback);
+		return v;
 	}
 	
 	/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
@@ -128,7 +131,7 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 		final float machineEpsilonSquare = Constant.FLOAT_EPSILON * Constant.FLOAT_EPSILON;
 		final float epsilon = 0.0001f;
 		// Convert the ray origin and direction into the local-space of the collision shape
-		final Vector3f rayDirection = ray.point2.lessNew(ray.point1);
+		final Vector3f rayDirection = ray.point2.less(ray.point1);
 		// If the points of the segment are two close, return no hit
 		if (rayDirection.length2() < machineEpsilonSquare) {
 			return false;
@@ -141,37 +144,42 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 		float lambda = 0.0f;
 		suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
 		suppB = shape.getLocalSupportPointWithoutMargin(rayDirection, (CacheData) shapeCachedCollisionData);
-		final Vector3f v = suppA.lessNew(suppB);
+		//Vector3f v = suppA.less(suppB);
+		var v = new Object() {
+			Vector3f value;
+		};
+		v.value = suppA.less(suppB);
 		float vDotW, vDotR;
-		float distSquare = v.length2();
+		float distSquare = v.value.length2();
 		int nbIterations = 0;
 		// GJK Algorithm loop
-		while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) {
+		while (distSquare > epsilon && nbIterations < GJKAlgorithm.MAX_ITERATIONS_GJK_RAYCAST) {
 			// Compute the support points
-			suppB = shape.getLocalSupportPointWithoutMargin(v, (CacheData) shapeCachedCollisionData);
-			w = suppA.lessNew(suppB);
-			vDotW = v.dot(w);
+			suppB = shape.getLocalSupportPointWithoutMargin(v.value, (CacheData) shapeCachedCollisionData);
+			w = suppA.less(suppB);
+			vDotW = v.value.dot(w);
 			if (vDotW > 0.0f) {
-				vDotR = v.dot(rayDirection);
+				vDotR = v.value.dot(rayDirection);
 				if (vDotR >= -machineEpsilonSquare) {
 					return false;
-				} else {
-					// We have found a better lower bound for the hit point aint the ray
-					lambda = lambda - vDotW / vDotR;
-					suppA = rayDirection.multiplyNew(lambda).add(ray.point1);
-					w = suppA.lessNew(suppB);
-					n = v;
 				}
+				// We have found a better lower bound for the hit point aint the ray
+				lambda = lambda - vDotW / vDotR;
+				suppA = rayDirection.multiply(lambda).add(ray.point1);
+				w = suppA.less(suppB);
+				n = v.value;
 			}
 			// Add the new support point to the simplex
 			if (!simplex.isPointInSimplex(w)) {
 				simplex.addPoint(w, suppA, suppB);
 			}
 			// Compute the closest point
-			if (simplex.computeClosestPoint(v)) {
-				distSquare = v.length2();
+			Vector3f v2 = simplex.computeClosestPoint(v.value);
+			if (v2 == null) {
+				distSquare = v.value.length2();
 			} else {
 				distSquare = 0.0f;
+				v.value = v2;
 			}
 			// If the current lower bound distance is larger than the maximum raycasting distance
 			if (lambda > ray.maxFraction) {
@@ -184,12 +192,16 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 			return false;
 		}
 		// Compute the closet points of both objects (without the margins)
-		final Vector3f pointA = new Vector3f();
-		final Vector3f pointB = new Vector3f();
-		simplex.computeClosestPointsOfAandB(pointA, pointB);
+		var pointA = new Object() {
+			Vector3f value = Vector3f.ZERO;
+		};
+		var pointB = new Object() {
+			Vector3f value = Vector3f.ZERO;
+		};
+		simplex.computeClosestPointsOfAandB(o -> pointA.value = o, o -> pointB.value = o);
 		// A raycast hit has been found, we fill in the raycast info
 		raycastInfo.hitFraction = lambda;
-		raycastInfo.worldPoint = pointB;
+		raycastInfo.worldPoint = pointB.value;
 		raycastInfo.body = proxyShape.getBody();
 		raycastInfo.proxyShape = proxyShape;
 		if (n.length2() >= machineEpsilonSquare) {
@@ -204,31 +216,37 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 	
 	@Override
 	public void testCollision(final CollisionShapeInfo shape1Info, final CollisionShapeInfo shape2Info, final NarrowPhaseCallback callback) {
-		//Log.error("=================================================");
-		//Log.error(" shape1Info=" + shape1Info.shapeToWorldTransform);
-		//Log.error(" shape2Info=" + shape2Info.shapeToWorldTransform);
-		Vector3f suppA = new Vector3f(); // Support point of object A
-		Vector3f suppB = new Vector3f(); // Support point of object B
-		Vector3f w = new Vector3f(); // Support point of Minkowski difference A-B
-		Vector3f pA = new Vector3f(); // Closest point of object A
-		Vector3f pB = new Vector3f(); // Closest point of object B
+		//LOGGER.error("=================================================");
+		//LOGGER.error(" shape1Info=" + shape1Info.shapeToWorldTransform);
+		//LOGGER.error(" shape2Info=" + shape2Info.shapeToWorldTransform);
+		Vector3f suppA = Vector3f.ZERO; // Support point of object A
+		Vector3f suppB = Vector3f.ZERO; // Support point of object B
+		Vector3f w = Vector3f.ZERO; // Support point of Minkowski difference A-B
+		var pA = new Object() {
+			Vector3f value;
+		};
+		var pB = new Object() {
+			Vector3f value;
+		};
+		//		Vector3f pA = Vector3f.ZERO; // Closest point of object A
+		//		Vector3f pB = Vector3f.ZERO; // Closest point of object B
 		float vDotw;
 		float prevDistSquare;
-		assert (shape1Info.collisionShape.isConvex());
-		assert (shape2Info.collisionShape.isConvex());
-		final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape);
-		final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape);
-		final CacheData shape1CachedCollisionData = (CacheData) shape1Info.cachedCollisionData;
-		final CacheData shape2CachedCollisionData = (CacheData) shape2Info.cachedCollisionData;
+		assert (shape1Info.collisionShape().isConvex());
+		assert (shape2Info.collisionShape().isConvex());
+		final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape());
+		final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape());
+		final CacheData shape1CachedCollisionData = (CacheData) shape1Info.cachedCollisionData();
+		final CacheData shape2CachedCollisionData = (CacheData) shape2Info.cachedCollisionData();
 		// Get the local-space to world-space transforms
-		final Transform3D transform1 = shape1Info.shapeToWorldTransform.clone();
-		final Transform3D transform2 = shape2Info.shapeToWorldTransform.clone();
+		final Transform3D transform1 = shape1Info.shapeToWorldTransform();
+		final Transform3D transform2 = shape2Info.shapeToWorldTransform();
 		// Transform3D a point from local space of body 2 to local
 		// space of body 1 (the GJK algorithm is done in local space of body 1)
-		final Transform3D body2Tobody1 = transform1.inverseNew().multiplyNew(transform2);
+		final Transform3D body2Tobody1 = transform1.inverseNew().multiply(transform2);
 		// Matrix that transform a direction from local
 		// space of body 1 into local space of body 2
-		final Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().transposeNew().multiplyNew(transform1.getOrientation().getMatrix());
+		final Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().transpose().multiply(transform1.getOrientation().getMatrix());
 		// Initialize the margin (sum of margins of both objects)
 		final float margin = shape1.getMargin() + shape2.getMargin();
 		final float marginSquare = margin * margin;
@@ -236,55 +254,57 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 		// Create a simplex set
 		final Simplex simplex = new Simplex();
 		// Get the previous point V (last cached separating axis)
-		final Vector3f cacheSeparatingAxis = this.currentOverlappingPair.getCachedSeparatingAxis().clone();
+		var cacheSeparatingAxis = new Object() {
+			Vector3f value = GJKAlgorithm.this.currentOverlappingPair.getCachedSeparatingAxis();
+		};
 		// Initialize the upper bound for the square distance
 		float distSquare = Float.MAX_VALUE;
 		
-		//Log.error(" T1=" + transform1 + "       T2=" + transform2);
-		//Log.error(" BT1=" + body2Tobody1 + "    RT2=" + rotateToBody2);
-		//Log.error(" M=" + FMath.floatToString(margin) + "             M2=" + FMath.floatToString(marginSquare));
-		//Log.error(" v=" + cacheSeparatingAxis);
+		//LOGGER.error(" T1=" + transform1 + "       T2=" + transform2);
+		//LOGGER.error(" BT1=" + body2Tobody1 + "    RT2=" + rotateToBody2);
+		//LOGGER.error(" M=" + FMath.floatToString(margin) + "             M2=" + FMath.floatToString(marginSquare));
+		//LOGGER.error(" v=" + cacheSeparatingAxis);
 		
 		do {
-			//Log.error("------------------");
+			//LOGGER.error("------------------");
 			// Compute the support points for original objects (without margins) A and B
-			suppA = shape1.getLocalSupportPointWithoutMargin(cacheSeparatingAxis.multiplyNew(-1.0f), shape1CachedCollisionData);
-			suppB = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithoutMargin(rotateToBody2.multiplyNew(cacheSeparatingAxis), shape2CachedCollisionData));
+			suppA = shape1.getLocalSupportPointWithoutMargin(cacheSeparatingAxis.value.multiply(-1.0f), shape1CachedCollisionData);
+			suppB = body2Tobody1.multiply(shape2.getLocalSupportPointWithoutMargin(rotateToBody2.multiply(cacheSeparatingAxis.value), shape2CachedCollisionData));
 			// Compute the support point for the Minkowski difference A-B
-			w = suppA.lessNew(suppB);
-			vDotw = cacheSeparatingAxis.dot(w);
-			//Log.error(" suppA=" + suppA);
-			//Log.error(" suppB=" + suppB);
-			//Log.error(" w=" + w);
+			w = suppA.less(suppB);
+			vDotw = cacheSeparatingAxis.value.dot(w);
+			//LOGGER.error(" suppA=" + suppA);
+			//LOGGER.error(" suppB=" + suppB);
+			//LOGGER.error(" w=" + w);
 			// If the enlarge objects (with margins) do not intersect
 			if (vDotw > 0.0f && vDotw * vDotw > distSquare * marginSquare) {
 				// Cache the current separating axis for frame coherence
-				this.currentOverlappingPair.setCachedSeparatingAxis(cacheSeparatingAxis);
+				this.currentOverlappingPair.setCachedSeparatingAxis(cacheSeparatingAxis.value);
 				// No intersection, we return
 				return;
 			}
 			// If the objects intersect only in the margins
-			if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
-				//Log.error("11111111 ");
+			if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * GJKAlgorithm.REL_ERROR_SQUARE) {
+				//LOGGER.error("11111111 ");
 				// Compute the closet points of both objects (without the margins)
-				simplex.computeClosestPointsOfAandB(pA, pB);
+				simplex.computeClosestPointsOfAandB(o -> pA.value = o, o -> pB.value = o);
 				// Project those two points on the margins to have the closest points of both
 				// object with the margins	
 				final float dist = FMath.sqrt(distSquare);
 				assert (dist > 0.0f);
-				pA = pA.lessNew(cacheSeparatingAxis.multiplyNew(shape1.getMargin() / dist));
-				pB = body2Tobody1.inverseNew().multiplyNew(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB));
+				pA.value = pA.value.less(cacheSeparatingAxis.value.multiply(shape1.getMargin() / dist));
+				pB.value = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.value.multiply(shape2.getMargin() / dist).add(pB.value));
 				// Compute the contact info
-				final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.safeNormalizeNew().multiply(-1));
+				final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.value.safeNormalize().multiply(-1));
 				final float penetrationDepth = margin - dist;
 				// Reject the contact if the penetration depth is negative (due too numerical errors)
 				if (penetrationDepth <= 0.0f) {
 					return;
 				}
 				// Create the contact info object
-				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth,
-						pA, pB);
-				callback.notifyContact(shape1Info.overlappingPair, contactInfo);
+				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape(), shape2Info.proxyShape(), shape1Info.collisionShape(), shape2Info.collisionShape(), normal,
+						penetrationDepth, pA.value, pB.value);
+				callback.notifyContact(shape1Info.overlappingPair(), contactInfo);
 				// There is an intersection, therefore we return
 				return;
 			}
@@ -292,46 +312,47 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 			simplex.addPoint(w, suppA, suppB);
 			// If the simplex is affinely dependent
 			if (simplex.isAffinelyDependent()) {
-				//Log.error("222222 ");
+				//LOGGER.error("222222 ");
 				// Compute the closet points of both objects (without the margins)
-				simplex.computeClosestPointsOfAandB(pA, pB);
+				simplex.computeClosestPointsOfAandB(o -> pA.value = o, o -> pB.value = o);
 				// Project those two points on the margins to have the closest points of both
 				// object with the margins
 				final float dist = FMath.sqrt(distSquare);
 				assert (dist > 0.0f);
-				pA = pA.lessNew(cacheSeparatingAxis.multiplyNew(shape1.getMargin() / dist));
-				pB = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB));
+				pA.value = pA.value.less(cacheSeparatingAxis.value.multiply(shape1.getMargin() / dist));
+				pB.value = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.value.multiply(shape2.getMargin() / dist).add(pB.value));
 				// Compute the contact info
-				final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.safeNormalizeNew().multiply(-1));
+				final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.value.safeNormalize().multiply(-1));
 				final float penetrationDepth = margin - dist;
 				
 				// Reject the contact if the penetration depth is negative (due too numerical errors)
 				if (penetrationDepth <= 0.0f) {
-					//Log.info("penetration depth " + penetrationDepth);
+					//LOGGER.info("penetration depth " + penetrationDepth);
 					return;
 				}
 				
 				// Create the contact info object
-				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth,
-						pA, pB);
-				callback.notifyContact(shape1Info.overlappingPair, contactInfo);
+				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape(), shape2Info.proxyShape(), shape1Info.collisionShape(), shape2Info.collisionShape(), normal,
+						penetrationDepth, pA.value, pB.value);
+				callback.notifyContact(shape1Info.overlappingPair(), contactInfo);
 				// There is an intersection, therefore we return
 				return;
 			}
 			// Compute the point of the simplex closest to the origin
 			// If the computation of the closest point fail
-			if (!simplex.computeClosestPoint(cacheSeparatingAxis)) {
-				//Log.error("3333333333 ");
+			Vector3f tmp = simplex.computeClosestPoint(cacheSeparatingAxis.value);
+			if (tmp == null) {
+				//LOGGER.error("3333333333 ");
 				// Compute the closet points of both objects (without the margins)
-				simplex.computeClosestPointsOfAandB(pA, pB);
+				simplex.computeClosestPointsOfAandB(o -> pA.value = o, o -> pB.value = o);
 				// Project those two points on the margins to have the closest points of both
 				// object with the margins
 				final float dist = FMath.sqrt(distSquare);
 				assert (dist > 0.0f);
-				pA = pA.lessNew(cacheSeparatingAxis.multiplyNew(shape1.getMargin() / dist));
-				pB = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB));
+				pA.value = pA.value.less(cacheSeparatingAxis.value.multiply(shape1.getMargin() / dist));
+				pB.value = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.value.multiply(shape2.getMargin() / dist).add(pB.value));
 				// Compute the contact info
-				final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.safeNormalizeNew().multiply(-1));
+				final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.value.safeNormalize().multiply(-1));
 				final float penetrationDepth = margin - dist;
 				
 				// Reject the contact if the penetration depth is negative (due too numerical errors)
@@ -340,32 +361,33 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 				}
 				
 				// Create the contact info object
-				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth,
-						pA, pB);
-				callback.notifyContact(shape1Info.overlappingPair, contactInfo);
+				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape(), shape2Info.proxyShape(), shape1Info.collisionShape(), shape2Info.collisionShape(), normal,
+						penetrationDepth, pA.value, pB.value);
+				callback.notifyContact(shape1Info.overlappingPair(), contactInfo);
 				// There is an intersection, therefore we return
 				return;
 			}
+			cacheSeparatingAxis.value = tmp;
 			// Store and update the squared distance of the closest point
 			prevDistSquare = distSquare;
-			distSquare = cacheSeparatingAxis.length2();
+			distSquare = cacheSeparatingAxis.value.length2();
 			// If the distance to the closest point doesn't improve a lot
 			if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) {
-				//Log.error("444444444 ");
-				simplex.backupClosestPointInSimplex(cacheSeparatingAxis);
+				//LOGGER.error("444444444 ");
+				cacheSeparatingAxis.value = simplex.backupClosestPointInSimplex();
 				
 				// Get the new squared distance
-				distSquare = cacheSeparatingAxis.length2();
+				distSquare = cacheSeparatingAxis.value.length2();
 				// Compute the closet points of both objects (without the margins)
-				simplex.computeClosestPointsOfAandB(pA, pB);
+				simplex.computeClosestPointsOfAandB(o -> pA.value = o, o -> pB.value = o);
 				// Project those two points on the margins to have the closest points of both
 				// object with the margins
 				final float dist = FMath.sqrt(distSquare);
 				assert (dist > 0.0f);
-				pA = pA.lessNew(cacheSeparatingAxis.multiplyNew(shape1.getMargin() / dist));
-				pB = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB));
+				pA.value = pA.value.less(cacheSeparatingAxis.value.multiply(shape1.getMargin() / dist));
+				pB.value = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.value.multiply(shape2.getMargin() / dist).add(pB.value));
 				// Compute the contact info
-				final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.safeNormalizeNew().multiply(-1));
+				final Vector3f normal = transform1.getOrientation().multiply(cacheSeparatingAxis.value.safeNormalize().multiply(-1));
 				final float penetrationDepth = margin - dist;
 				
 				// Reject the contact if the penetration depth is negative (due too numerical errors)
@@ -374,9 +396,9 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 				}
 				
 				// Create the contact info object
-				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, shape2Info.collisionShape, normal, penetrationDepth,
-						pA, pB);
-				callback.notifyContact(shape1Info.overlappingPair, contactInfo);
+				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape(), shape2Info.proxyShape(), shape1Info.collisionShape(), shape2Info.collisionShape(), normal,
+						penetrationDepth, pA.value, pB.value);
+				callback.notifyContact(shape1Info.overlappingPair(), contactInfo);
 				// There is an intersection, therefore we return
 				return;
 			}
@@ -385,31 +407,33 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 		// again but on the enlarged objects to compute a simplex polytope that contains
 		// the origin. Then, we give that simplex polytope to the EPA algorithm to compute
 		// the correct penetration depth and contact points between the enlarged objects.
-		computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info, transform2, callback, cacheSeparatingAxis);
+		cacheSeparatingAxis.value = computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info, transform2, callback, cacheSeparatingAxis.value);
 	}
 	
 	/// Use the GJK Algorithm to find if a point is inside a convex collision shape
 	public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) {
-		Vector3f suppA = new Vector3f(); // Support point of object A
-		Vector3f w = new Vector3f(); // Support point of Minkowski difference A-B
+		Vector3f suppA = Vector3f.ZERO; // Support point of object A
+		Vector3f w = Vector3f.ZERO; // Support point of Minkowski difference A-B
 		float prevDistSquare;
 		assert (proxyShape.getCollisionShape().isConvex());
 		final ConvexShape shape = (ConvexShape) (proxyShape.getCollisionShape());
 		final CacheData shapeCachedCollisionData = (CacheData) proxyShape.getCachedCollisionData();
 		// Support point of object B (object B is a single point)
-		final Vector3f suppB = new Vector3f(localPoint);
+		final Vector3f suppB = localPoint;
 		// Create a simplex set
 		final Simplex simplex = new Simplex();
 		
 		// Initial supporting direction
-		final Vector3f v = new Vector3f(1, 1, 1);
+		var v = new Object() {
+			Vector3f value = Vector3f.ZERO;
+		};
 		// Initialize the upper bound for the square distance
 		float distSquare = Float.MAX_VALUE;
 		do {
 			// Compute the support points for original objects (without margins) A and B
-			suppA = shape.getLocalSupportPointWithoutMargin(v.multiplyNew(-1), shapeCachedCollisionData);
+			suppA = shape.getLocalSupportPointWithoutMargin(v.value.multiply(-1), shapeCachedCollisionData);
 			// Compute the support point for the Minkowski difference A-B
-			w = suppA.lessNew(suppB);
+			w = suppA.less(suppB);
 			// Add the new support point to the simplex
 			simplex.addPoint(w, suppA, suppB);
 			// If the simplex is affinely dependent
@@ -418,12 +442,14 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 			}
 			// Compute the point of the simplex closest to the origin
 			// If the computation of the closest point fail
-			if (!simplex.computeClosestPoint(v)) {
+			Vector3f tmpV = simplex.computeClosestPoint(v.value);
+			if (tmpV == null) {
 				return false;
 			}
+			v.value = tmpV;
 			// Store and update the squared distance of the closest point
 			prevDistSquare = distSquare;
-			distSquare = v.length2();
+			distSquare = v.value.length2();
 			// If the distance to the closest point doesn't improve a lot
 			if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) {
 				return false;

src/org/atriasoft/ephysics/collision/narrowphase/GJK/Simplex.java

@@ -1,5 +1,7 @@
 package org.atriasoft.ephysics.collision.narrowphase.GJK;
 
+import java.util.function.Consumer;
+
 import org.atriasoft.etk.math.Vector3f;
 
 /**
@@ -118,7 +120,7 @@ public class Simplex {
 	/// point	  : support point of object (A-B) => point = suppPointA - suppPointB
 	public void addPoint(final Vector3f point, final Vector3f suppPointA, final Vector3f suppPointB) {
 		assert (!isFull());
-		//Log.warning("simplex: addPoint(" + point + ", " + suppPointA + ", " + suppPointA + ")");
+		//LOGGER.warn("simplex: addPoint(" + point + ", " + suppPointA + ", " + suppPointA + ")");
 		this.lastFound = 0;
 		this.lastFoundBit = 0x1;
 		
@@ -128,8 +130,8 @@ public class Simplex {
 			this.lastFound++;
 			this.lastFoundBit <<= 1;
 		}
-		//Log.warning("     this.lastFound " + this.lastFound);
-		//Log.warning("     this.lastFoundBit " + this.lastFoundBit);
+		//LOGGER.warn("     this.lastFound " + this.lastFound);
+		//LOGGER.warn("     this.lastFoundBit " + this.lastFoundBit);
 		
 		assert (this.lastFound < 4);
 		
@@ -137,7 +139,7 @@ public class Simplex {
 		this.points[this.lastFound] = point;
 		this.pointsLengthSquare[this.lastFound] = point.dot(point);
 		this.allBits = this.bitsCurrentSimplex | this.lastFoundBit;
-		//Log.warning("     this.allBits " + this.allBits);
+		//LOGGER.warn("     this.allBits " + this.allBits);
 		
 		// Update the cached values
 		updateCache();
@@ -151,8 +153,9 @@ public class Simplex {
 	}
 	
 	/// Backup the closest point
-	public void backupClosestPointInSimplex(final Vector3f point) {
+	public Vector3f backupClosestPointInSimplex() {
 		float minDistSquare = Float.MAX_VALUE;
+		Vector3f out = Vector3f.ZERO;
 		for (int bit = this.allBits; bit != 0x0; bit--) {
 			if (isSubset(bit, this.allBits) && isProperSubset(bit)) {
 				final Vector3f u = computeClosestPointForSubset(bit);
@@ -160,25 +163,25 @@ public class Simplex {
 				if (distSquare < minDistSquare) {
 					minDistSquare = distSquare;
 					this.bitsCurrentSimplex = bit;
-					point.set(u);
+					out = u;
 				}
 			}
 		}
+		return out;
 	}
 	
 	/// Compute the closest point to the origin of the current simplex.
 	/// This method executes the Jonhnson's algorithm for computing the point
 	/// "v" of simplex that is closest to the origin. The method returns true
 	/// if a closest point has been found.
-	public boolean computeClosestPoint(final Vector3f vvv) {
+	public Vector3f computeClosestPoint(final Vector3f vvv) {
 		// For each possible simplex set
 		for (int subset = this.bitsCurrentSimplex; subset != 0x0; subset--) {
 			// If the simplex is a subset of the current simplex and is valid for the Johnson's
 			// algorithm test
 			if (isSubset(subset, this.bitsCurrentSimplex) && isValidSubset(subset | this.lastFoundBit)) {
 				this.bitsCurrentSimplex = subset | this.lastFoundBit; // Add the last added point to the current simplex
-				vvv.set(computeClosestPointForSubset(this.bitsCurrentSimplex)); // Compute the closest point in the simplex
-				return true;
+				return computeClosestPointForSubset(this.bitsCurrentSimplex); // Compute the closest point in the simplex
 			}
 		}
 		
@@ -186,17 +189,16 @@ public class Simplex {
 		if (isValidSubset(this.lastFoundBit)) {
 			this.bitsCurrentSimplex = this.lastFoundBit; // Set the current simplex to the set that contains only the last added point
 			this.maxLengthSquare = this.pointsLengthSquare[this.lastFound]; // Update the maximum square length
-			vvv.set(this.points[this.lastFound]); // The closest point of the simplex "v" is the last added point
-			return true;
+			return this.points[this.lastFound]; // The closest point of the simplex "v" is the last added point
 		}
 		
 		// The algorithm failed to found a point
-		return false;
+		return null;
 	}
 	
 	/// Return the closest point "v" in the convex hull of a subset of points
 	private Vector3f computeClosestPointForSubset(final int subset) {
-		final Vector3f vvv = new Vector3f(0.0f, 0.0f, 0.0f); // Closet point v = sum(lambda_i * points[i])
+		Vector3f vvv = new Vector3f(0.0f, 0.0f, 0.0f); // Closet point v = sum(lambda_i * points[i])
 		this.maxLengthSquare = 0.0f;
 		float deltaX = 0.0f; // deltaX = sum of all det[subset][i]
 		// For each four point in the possible simplex set
@@ -209,7 +211,7 @@ public class Simplex {
 					this.maxLengthSquare = this.pointsLengthSquare[iii];
 				}
 				// Closest point v = sum(lambda_i * points[i])
-				vvv.add(this.points[iii].multiplyNew(this.det.get(subset, iii)));
+				vvv = vvv.add(this.points[iii].multiply(this.det.get(subset, iii)));
 			}
 		}
 		assert (deltaX > 0.0f);
@@ -222,30 +224,32 @@ public class Simplex {
 	///	  pA = sum(lambda_i * a_i)	where "a_i" are the support points of object A
 	///	  pB = sum(lambda_i * b_i)	where "b_i" are the support points of object B
 	///	  with lambda_i = deltaX_i / deltaX
-	public void computeClosestPointsOfAandB(final Vector3f pA, final Vector3f pB) {
+	public void computeClosestPointsOfAandB(final Consumer<Vector3f> outA, final Consumer<Vector3f> outB) {
 		float deltaX = 0.0f;
-		pA.set(0.0f, 0.0f, 0.0f);
-		pB.set(0.0f, 0.0f, 0.0f);
+		Vector3f pA = Vector3f.ZERO;
+		Vector3f pB = Vector3f.ZERO;
 		// For each four points in the possible simplex set
 		for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) {
 			// If the current point is part of the simplex
 			if (overlap(this.bitsCurrentSimplex, bit)) {
 				deltaX += this.det.get(this.bitsCurrentSimplex, iii);
-				pA.add(this.suppPointsA[iii].multiplyNew(this.det.get(this.bitsCurrentSimplex, iii)));
-				pB.add(this.suppPointsB[iii].multiplyNew(this.det.get(this.bitsCurrentSimplex, iii)));
+				pA = pA.add(this.suppPointsA[iii].multiply(this.det.get(this.bitsCurrentSimplex, iii)));
+				pB = pB.add(this.suppPointsB[iii].multiply(this.det.get(this.bitsCurrentSimplex, iii)));
 			}
 		}
 		
 		assert (deltaX > 0.0f);
 		final float factor = 1.0f / deltaX;
-		pA.multiply(factor);
-		pB.multiply(factor);
+		pA = pA.multiply(factor);
+		pB = pB.multiply(factor);
+		outA.accept(pA);
+		outB.accept(pB);
 	}
 	
 	/// Compute the cached determinant values
 	private void computeDeterminants() {
 		this.det.set(this.lastFoundBit, this.lastFound, 1.0f);
-		//Log.warning("simplex:     computeDeterminants() " + this.det.get(this.lastFoundBit, this.lastFound));
+		//LOGGER.warn("simplex:     computeDeterminants() " + this.det.get(this.lastFoundBit, this.lastFound));
 		// If the current simplex is not empty
 		if (!isEmpty()) {
 			// For each possible four points in the simplex set
@@ -320,9 +324,9 @@ public class Simplex {
 			// If the current point is in the simplex
 			if (overlap(this.bitsCurrentSimplex, bit)) {
 				// Store the points
-				suppPointsA[nbVertices] = this.suppPointsA[nbVertices].clone();
-				suppPointsB[nbVertices] = this.suppPointsB[nbVertices].clone();
-				points[nbVertices] = this.points[nbVertices].clone();
+				suppPointsA[nbVertices] = this.suppPointsA[nbVertices];
+				suppPointsB[nbVertices] = this.suppPointsB[nbVertices];
+				points[nbVertices] = this.points[nbVertices];
 				nbVertices++;
 			}
 		}
@@ -418,19 +422,19 @@ public class Simplex {
 	
 	/// Update the cached values used during the GJK algorithm
 	private void updateCache() {
-		//Log.warning("simplex:     update Cache()");
+		//LOGGER.warn("simplex:     update Cache()");
 		// For each of the four possible points of the simplex
 		for (int iii = 0, bit = 0x1; iii < 4; iii++, bit <<= 1) {
-			//Log.warning("simplex:         iii=" + iii);
-			//Log.warning("simplex:         bit=" + bit);
+			//LOGGER.warn("simplex:         iii=" + iii);
+			//LOGGER.warn("simplex:         bit=" + bit);
 			
 			// If the current points is in the simplex
 			if (overlap(this.bitsCurrentSimplex, bit)) {
-				//Log.warning("simplex:         ==> overlap");
+				//LOGGER.warn("simplex:         ==> overlap");
 				// Compute the distance between two points in the possible simplex set
-				final Vector3f tmp = this.points[iii].lessNew(this.points[this.lastFound]);
+				final Vector3f tmp = this.points[iii].less(this.points[this.lastFound]);
 				this.diffLength.set(iii, this.lastFound, tmp);
-				final Vector3f tmp2 = tmp.multiplyNew(-1);
+				final Vector3f tmp2 = tmp.multiply(-1);
 				this.diffLength.set(this.lastFound, iii, tmp2);
 				// Compute the squared length of the vector
 				// distances from points in the possible simplex set

src/org/atriasoft/ephysics/collision/narrowphase/NarrowPhaseAlgorithm.java

@@ -27,6 +27,6 @@ public abstract class NarrowPhaseAlgorithm {
 	}
 	
 	/// Compute a contact info if the two bounding volume collide
-	public abstract void testCollision(final CollisionShapeInfo _shape1Info, CollisionShapeInfo _shape2Info, NarrowPhaseCallback _narrowPhaseCallback);
+	public abstract void testCollision(final CollisionShapeInfo shape1Info, CollisionShapeInfo shape2Info, NarrowPhaseCallback narrowPhaseCallback);
 	
 }

src/org/atriasoft/ephysics/collision/narrowphase/NarrowPhaseCallback.java

@@ -9,6 +9,6 @@ import org.atriasoft.ephysics.engine.OverlappingPair;
  */
 public interface NarrowPhaseCallback {
 	/// Called by a narrow-phase collision algorithm when a new contact has been found
-	void notifyContact(OverlappingPair _overlappingPair, ContactPointInfo _contactInfo);
+	void notifyContact(OverlappingPair overlappingPair, ContactPointInfo contactInfo);
 	
-};
+}

src/org/atriasoft/ephysics/collision/narrowphase/SphereVsSphereAlgorithm.java

@@ -1,13 +1,12 @@
 package org.atriasoft.ephysics.collision.narrowphase;
 
-import org.atriasoft.etk.math.FMath;
-import org.atriasoft.etk.math.Transform3D;
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.collision.CollisionDetection;
 import org.atriasoft.ephysics.collision.CollisionShapeInfo;
 import org.atriasoft.ephysics.collision.shapes.SphereShape;
 import org.atriasoft.ephysics.constraint.ContactPointInfo;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
 
 /**
  *  It is used to compute the narrow-phase collision detection
@@ -20,15 +19,15 @@ public class SphereVsSphereAlgorithm extends NarrowPhaseAlgorithm {
 	}
 	
 	@Override
-	public void testCollision(final CollisionShapeInfo _shape1Info, final CollisionShapeInfo _shape2Info, final NarrowPhaseCallback _narrowPhaseCallback) {
+	public void testCollision(final CollisionShapeInfo shape1Info, final CollisionShapeInfo shape2Info, final NarrowPhaseCallback narrowPhaseCallback) {
 		// Get the sphere collision shapes
-		final SphereShape sphereShape1 = (SphereShape) _shape1Info.collisionShape;
-		final SphereShape sphereShape2 = (SphereShape) _shape2Info.collisionShape;
+		final SphereShape sphereShape1 = (SphereShape) shape1Info.collisionShape();
+		final SphereShape sphereShape2 = (SphereShape) shape2Info.collisionShape();
 		// Get the local-space to world-space transforms
-		final Transform3D transform1 = _shape1Info.shapeToWorldTransform;
-		final Transform3D transform2 = _shape2Info.shapeToWorldTransform;
+		final Transform3D transform1 = shape1Info.shapeToWorldTransform();
+		final Transform3D transform2 = shape2Info.shapeToWorldTransform();
 		// Compute the distance between the centers
-		final Vector3f vectorBetweenCenters = transform2.getPosition().lessNew(transform1.getPosition());
+		final Vector3f vectorBetweenCenters = transform2.getPosition().less(transform1.getPosition());
 		final float squaredDistanceBetweenCenters = vectorBetweenCenters.length2();
 		// Compute the sum of the radius
 		final float sumRadius = sphereShape1.getRadius() + sphereShape2.getRadius();
@@ -36,15 +35,15 @@ public class SphereVsSphereAlgorithm extends NarrowPhaseAlgorithm {
 		if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
 			final Vector3f centerSphere2InBody1LocalSpace = transform1.inverseNew().multiply(transform2.getPosition());
 			final Vector3f centerSphere1InBody2LocalSpace = transform2.inverseNew().multiply(transform1.getPosition());
-			final Vector3f intersectionOnBody1 = centerSphere2InBody1LocalSpace.safeNormalizeNew().multiply(sphereShape1.getRadius());
-			final Vector3f intersectionOnBody2 = centerSphere1InBody2LocalSpace.safeNormalizeNew().multiply(sphereShape2.getRadius());
+			final Vector3f intersectionOnBody1 = centerSphere2InBody1LocalSpace.safeNormalize().multiply(sphereShape1.getRadius());
+			final Vector3f intersectionOnBody2 = centerSphere1InBody2LocalSpace.safeNormalize().multiply(sphereShape2.getRadius());
 			final float penetrationDepth = sumRadius - FMath.sqrt(squaredDistanceBetweenCenters);
 			
 			// Create the contact info object
-			final ContactPointInfo contactInfo = new ContactPointInfo(_shape1Info.proxyShape, _shape2Info.proxyShape, _shape1Info.collisionShape, _shape2Info.collisionShape,
-					vectorBetweenCenters.safeNormalizeNew(), penetrationDepth, intersectionOnBody1, intersectionOnBody2);
+			final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape(), shape2Info.proxyShape(), shape1Info.collisionShape(), shape2Info.collisionShape(),
+					vectorBetweenCenters.safeNormalize(), penetrationDepth, intersectionOnBody1, intersectionOnBody2);
 			// Notify about the new contact
-			_narrowPhaseCallback.notifyContact(_shape1Info.overlappingPair, contactInfo);
+			narrowPhaseCallback.notifyContact(shape1Info.overlappingPair(), contactInfo);
 		}
 	}
 }

src/org/atriasoft/ephysics/collision/shapes/AABB.java

@@ -1,11 +1,3 @@
-/** @file
- * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
- * @author Daniel CHAPPUIS
- * @author Edouard DUPIN
- * @copyright 2010-2016, Daniel Chappuis
- * @copyright 2017-now, Edouard DUPIN
- * @license MPL v2.0 (see license file)
- */
 package org.atriasoft.ephysics.collision.shapes;
 
 import org.atriasoft.ephysics.mathematics.Ray;
@@ -18,75 +10,38 @@ import org.atriasoft.etk.math.Vector3f;
  * always aligned with the world coordinate system. The AABB is defined by the  this.inimum and  this.aximum world coordinates of
  * the three axis.
  *
- * @author Jason Sorensen <sorensenj@smert.net>
  */
 public class AABB {
 	
 	/**
-	 * @brief Create and return an AABB for a triangle
-	 * @param[in] _trianglePoints List of 3 point od a triangle
+	 * Create and return an AABB for a triangle
+	 * @param trianglePoints List of 3 point od a triangle
 	 * @return An AABB box
 	 */
-	public static AABB createAABBForTriangle(final Vector3f[] _trianglePoints) {
-		final Vector3f minCoords = new Vector3f(_trianglePoints[0].x, _trianglePoints[0].y, _trianglePoints[0].z);
-		final Vector3f maxCoords = new Vector3f(_trianglePoints[0].x, _trianglePoints[0].y, _trianglePoints[0].z);
-		if (_trianglePoints[1].x < minCoords.x) {
-			minCoords.setX(_trianglePoints[1].x);
-		}
-		if (_trianglePoints[1].y < minCoords.y) {
-			minCoords.setY(_trianglePoints[1].y);
-		}
-		if (_trianglePoints[1].z < minCoords.z) {
-			minCoords.setZ(_trianglePoints[1].z);
-		}
-		if (_trianglePoints[2].x < minCoords.x) {
-			minCoords.setX(_trianglePoints[2].x);
-		}
-		if (_trianglePoints[2].y < minCoords.y) {
-			minCoords.setY(_trianglePoints[2].y);
-		}
-		if (_trianglePoints[2].z < minCoords.z) {
-			minCoords.setZ(_trianglePoints[2].z);
-		}
-		if (_trianglePoints[1].x > maxCoords.x) {
-			maxCoords.setX(_trianglePoints[1].x);
-		}
-		if (_trianglePoints[1].y > maxCoords.y) {
-			maxCoords.setY(_trianglePoints[1].y);
-		}
-		if (_trianglePoints[1].z > maxCoords.z) {
-			maxCoords.setZ(_trianglePoints[1].z);
-		}
-		if (_trianglePoints[2].x > maxCoords.x) {
-			maxCoords.setX(_trianglePoints[2].x);
-		}
-		if (_trianglePoints[2].y > maxCoords.y) {
-			maxCoords.setY(_trianglePoints[2].y);
-		}
-		if (_trianglePoints[2].z > maxCoords.z) {
-			maxCoords.setZ(_trianglePoints[2].z);
-		}
+	public static AABB createAABBForTriangle(final Vector3f[] trianglePoints) {
+		final Vector3f minCoords = trianglePoints[0].min(trianglePoints[1]).min(trianglePoints[2]);
+		final Vector3f maxCoords = trianglePoints[0].max(trianglePoints[1]).max(trianglePoints[2]);
 		return new AABB(minCoords, maxCoords);
 	}
 	
 	// Maximum world coordinates of the AABB on the x,y and z axis
-	private final Vector3f maxCoordinates;
+	private Vector3f maxCoordinates;
 	
 	// Minimum world coordinates of the AABB on the x,y and z axis
-	private final Vector3f minCoordinates;
+	private Vector3f minCoordinates;
 	
 	/**
-	 * @brief default contructor
+	 * default contructor
 	 */
 	public AABB() {
-		this.maxCoordinates = new Vector3f();
-		this.minCoordinates = new Vector3f();
+		this.maxCoordinates = Vector3f.ZERO;
+		this.minCoordinates = Vector3f.ZERO;
 	}
 	
 	/**
-	 * @brief contructor Whit sizes
-	 * @param[in] _minCoordinates Minimum coordinates
-	 * @param[in] _maxCoordinates Maximum coordinates
+	 * contructor Whit sizes
+	 * @param minCoordinates Minimum coordinates
+	 * @param maxCoordinates Maximum coordinates
 	 */
 	public AABB(final Vector3f minCoordinates, final Vector3f maxCoordinates) {
 		this.maxCoordinates = maxCoordinates;
@@ -94,50 +49,50 @@ public class AABB {
 	}
 	
 	/**
-	 * @brief Return true if the current AABB contains the AABB given in parameter
-	 * @param[in] _aabb AABB box that is contains in the current.
+	 * Return true if the current AABB contains the AABB given in parameter
+	 * @param aabb AABB box that is contains in the current.
 	 * @return true The parameter in contained inside
 	 */
-	public boolean contains(final AABB _aabb) {
+	public boolean contains(final AABB aabb) {
 		boolean isInside = true;
-		isInside = isInside && this.minCoordinates.x <= _aabb.minCoordinates.x;
-		isInside = isInside && this.minCoordinates.y <= _aabb.minCoordinates.y;
-		isInside = isInside && this.minCoordinates.z <= _aabb.minCoordinates.z;
-		isInside = isInside && this.maxCoordinates.x >= _aabb.maxCoordinates.x;
-		isInside = isInside && this.maxCoordinates.y >= _aabb.maxCoordinates.y;
-		isInside = isInside && this.maxCoordinates.z >= _aabb.maxCoordinates.z;
+		isInside = isInside && this.minCoordinates.x() <= aabb.minCoordinates.x();
+		isInside = isInside && this.minCoordinates.y() <= aabb.minCoordinates.y();
+		isInside = isInside && this.minCoordinates.z() <= aabb.minCoordinates.z();
+		isInside = isInside && this.maxCoordinates.x() >= aabb.maxCoordinates.x();
+		isInside = isInside && this.maxCoordinates.y() >= aabb.maxCoordinates.y();
+		isInside = isInside && this.maxCoordinates.z() >= aabb.maxCoordinates.z();
 		return isInside;
 	}
 	
 	/**
-	 * @brief Return true if a point is inside the AABB
-	 * @param[in] _point Point to check.
+	 * Return true if a point is inside the AABB
+	 * @param point Point to check.
 	 * @return true The point in contained inside
 	 */
-	public boolean contains(final Vector3f _point) {
-		return _point.x >= this.minCoordinates.x - Constant.FLOAT_EPSILON && _point.x <= this.maxCoordinates.x + Constant.FLOAT_EPSILON && _point.y >= this.minCoordinates.y - Constant.FLOAT_EPSILON
-				&& _point.y <= this.maxCoordinates.y + Constant.FLOAT_EPSILON && _point.z >= this.minCoordinates.z - Constant.FLOAT_EPSILON
-				&& _point.z <= this.maxCoordinates.z + Constant.FLOAT_EPSILON;
+	public boolean contains(final Vector3f point) {
+		return point.x() >= this.minCoordinates.x() - Constant.FLOAT_EPSILON && point.x() <= this.maxCoordinates.x() + Constant.FLOAT_EPSILON
+				&& point.y() >= this.minCoordinates.y() - Constant.FLOAT_EPSILON && point.y() <= this.maxCoordinates.y() + Constant.FLOAT_EPSILON
+				&& point.z() >= this.minCoordinates.z() - Constant.FLOAT_EPSILON && point.z() <= this.maxCoordinates.z() + Constant.FLOAT_EPSILON;
 	}
 	
 	/**
-	 * @brief Get the center point of the AABB box
+	 * Get the center point of the AABB box
 	 * @return The 3D position of the center
 	 */
 	public Vector3f getCenter() {
-		return new Vector3f(this.minCoordinates).add(this.maxCoordinates).multiply(0.5f);
+		return this.minCoordinates.add(this.maxCoordinates).multiply(0.5f);
 	}
 	
 	/**
-	 * @brief Get the size of the AABB in the three dimension x, y and z
+	 * Get the size of the AABB in the three dimension x, y and z
 	 * @return the AABB 3D size
 	 */
 	public Vector3f getExtent() {
-		return this.maxCoordinates.lessNew(this.minCoordinates);
+		return this.maxCoordinates.less(this.minCoordinates);
 	}
 	
 	/**
-	 * @brief Return the maximum coordinates of the AABB
+	 * Return the maximum coordinates of the AABB
 	 * @return The 3d maximum coordonates
 	 */
 	public Vector3f getMax() {
@@ -145,7 +100,7 @@ public class AABB {
 	}
 	
 	/**
-	 * @brief Get the minimum coordinates of the AABB
+	 * Get the minimum coordinates of the AABB
 	 * @return The 3d minimum coordonates
 	 */
 	public Vector3f getMin() {
@@ -153,144 +108,135 @@ public class AABB {
 	}
 	
 	/**
-	 * @brief Get the volume of the AABB
+	 * Get the volume of the AABB
 	 * @return The 3D volume.
 	 */
 	public float getVolume() {
-		final Vector3f diff = this.maxCoordinates.lessNew(this.minCoordinates);
-		return (diff.x * diff.y * diff.z);
+		final Vector3f diff = this.maxCoordinates.less(this.minCoordinates);
+		return (diff.x() * diff.y() * diff.z());
 	}
 	
 	/**
-	 * @brief Inflate each side of the AABB by a given size
-	 * @param[in] _dx Inflate X size
-	 * @param[in] _dy Inflate Y size
-	 * @param[in] _dz Inflate Z size
+	 * Inflate each side of the AABB by a given size
+	 * @param dx Inflate X size
+	 * @param dy Inflate Y size
+	 * @param dz Inflate Z size
 	 */
-	public void inflate(final float _dx, final float _dy, final float _dz) {
-		this.maxCoordinates.add(new Vector3f(_dx, _dy, _dz));
-		this.minCoordinates.less(new Vector3f(_dx, _dy, _dz));
+	public void inflate(final float dx, final float dy, final float dz) {
+		this.maxCoordinates = this.maxCoordinates.add(new Vector3f(dx, dy, dz));
+		this.minCoordinates = this.minCoordinates.less(new Vector3f(dx, dy, dz));
 	}
 	
 	/**
-	 * @brief Replace the current AABB with a new AABB that is the union of two AABBs in parameters
-	 * @param[in] _aabb1 first AABB box to merge with _aabb2.
-	 * @param[in] _aabb2 second AABB box to merge with _aabb1.
+	 * Replace the current AABB with a new AABB that is the union of two AABBs in parameters
+	 * @param aabb1 first AABB box to merge with aabb2.
+	 * @param aabb2 second AABB box to merge with aabb1.
 	 */
-	public void mergeTwoAABBs(final AABB _aabb1, final AABB _aabb2) {
-		this.minCoordinates.setX(FMath.min(_aabb1.minCoordinates.x, _aabb2.minCoordinates.x));
-		this.minCoordinates.setY(FMath.min(_aabb1.minCoordinates.y, _aabb2.minCoordinates.y));
-		this.minCoordinates.setZ(FMath.min(_aabb1.minCoordinates.z, _aabb2.minCoordinates.z));
-		this.maxCoordinates.setX(FMath.max(_aabb1.maxCoordinates.x, _aabb2.maxCoordinates.x));
-		this.maxCoordinates.setY(FMath.max(_aabb1.maxCoordinates.y, _aabb2.maxCoordinates.y));
-		this.maxCoordinates.setZ(FMath.max(_aabb1.maxCoordinates.z, _aabb2.maxCoordinates.z));
+	public void mergeTwoAABBs(final AABB aabb1, final AABB aabb2) {
+		this.minCoordinates = FMath.min(aabb1.minCoordinates, aabb2.minCoordinates);
+		this.maxCoordinates = FMath.max(aabb1.maxCoordinates, aabb2.maxCoordinates);
 	}
 	
 	/**
-	 * @brief Merge the AABB in parameter with the current one
-	 * @param[in] _aabb Other AABB box to merge.
+	 * Merge the AABB in parameter with the current one
+	 * @param aabb Other AABB box to merge.
 	 */
-	public void mergeWithAABB(final AABB _aabb) {
-		this.minCoordinates.setX(FMath.min(this.minCoordinates.x, _aabb.minCoordinates.x));
-		this.minCoordinates.setY(FMath.min(this.minCoordinates.y, _aabb.minCoordinates.y));
-		this.minCoordinates.setZ(FMath.min(this.minCoordinates.z, _aabb.minCoordinates.z));
-		this.maxCoordinates.setX(FMath.max(this.maxCoordinates.x, _aabb.maxCoordinates.x));
-		this.maxCoordinates.setY(FMath.max(this.maxCoordinates.y, _aabb.maxCoordinates.y));
-		this.maxCoordinates.setZ(FMath.max(this.maxCoordinates.z, _aabb.maxCoordinates.z));
+	public void mergeWithAABB(final AABB aabb) {
+		this.minCoordinates = FMath.min(this.minCoordinates, aabb.minCoordinates);
+		this.maxCoordinates = FMath.max(this.maxCoordinates, aabb.maxCoordinates);
 	}
 	
 	/**
-	 * @brief Set the maximum coordinates of the AABB
-	 * @param[in] _max The 3d maximum coordonates
+	 * Set the maximum coordinates of the AABB
+	 * @param max The 3d maximum coordonates
 	 */
 	public void setMax(final Vector3f max) {
-		this.maxCoordinates.set(max);
+		this.maxCoordinates = max;
 	}
 	
 	/**
-	 * @brief Set the minimum coordinates of the AABB
-	 * @param[in] _min The 3d minimum coordonates
+	 * Set the minimum coordinates of the AABB
+	 * @param min The 3d minimum coordonates
 	 */
 	public void setMin(final Vector3f min) {
-		this.minCoordinates.set(min);
+		this.minCoordinates = min;
 	}
 	
 	/**
-	 * @brief Return true if the current AABB is overlapping with the AABB in argument
+	 * Return true if the current AABB is overlapping with the AABB in argument
 	 * Two AABBs overlap if they overlap in the three x, y and z axis at the same time
-	 * @param[in] _aabb Other AABB box to check.
-	 * @return true Collision detected
-	 * @return false Not collide
+	 * @param aabb Other AABB box to check.
+	 * @return true Collision detected / false Not collide
 	 */
 	public boolean testCollision(final AABB aabb) {
 		if (this == aabb) {
-			///Log.info("test : AABB    ==> same object ");
+			///LOGGER.info("test : AABB    ==> same object ");
 			return true;
 		}
-		//Log.info("test : " + this + " && " + aabb);
-		if (this.maxCoordinates.getX() < aabb.minCoordinates.getX() || aabb.maxCoordinates.getX() < this.minCoordinates.getX()) {
+		//LOGGER.info("test : " + this + " && " + aabb);
+		if (this.maxCoordinates.x() < aabb.minCoordinates.x() || aabb.maxCoordinates.x() < this.minCoordinates.x()) {
 			return false;
 		}
-		if (this.maxCoordinates.getZ() < aabb.minCoordinates.getZ() || aabb.maxCoordinates.getZ() < this.minCoordinates.getZ()) {
+		if (this.maxCoordinates.z() < aabb.minCoordinates.z() || aabb.maxCoordinates.z() < this.minCoordinates.z()) {
 			return false;
 		}
-		if (this.maxCoordinates.getY() < aabb.minCoordinates.getY() || aabb.maxCoordinates.getY() < this.minCoordinates.getY()) {
+		if (this.maxCoordinates.y() < aabb.minCoordinates.y() || aabb.maxCoordinates.y() < this.minCoordinates.y()) {
 			return false;
 		}
-		//Log.info("detect collision ");
+		//LOGGER.info("detect collision ");
 		return true;
 	}
 	
 	/**
-	 * @brief check if the AABB of a triangle intersects the AABB
-	 * @param[in] _trianglePoints List of 3 point od a triangle
+	 * check if the AABB of a triangle intersects the AABB
+	 * @param trianglePoints List of 3 point od a triangle
 	 * @return true The triangle is contained in the Box
 	 */
-	public boolean testCollisionTriangleAABB(final Vector3f[] _trianglePoints) {
-		if (FMath.min(_trianglePoints[0].x, _trianglePoints[1].x, _trianglePoints[2].x) > this.maxCoordinates.x) {
+	public boolean testCollisionTriangleAABB(final Vector3f[] trianglePoints) {
+		if (FMath.min(trianglePoints[0].x(), trianglePoints[1].x(), trianglePoints[2].x()) > this.maxCoordinates.x()) {
 			return false;
 		}
-		if (FMath.min(_trianglePoints[0].y, _trianglePoints[1].y, _trianglePoints[2].y) > this.maxCoordinates.y) {
+		if (FMath.min(trianglePoints[0].y(), trianglePoints[1].y(), trianglePoints[2].y()) > this.maxCoordinates.y()) {
 			return false;
 		}
-		if (FMath.min(_trianglePoints[0].z, _trianglePoints[1].z, _trianglePoints[2].z) > this.maxCoordinates.z) {
+		if (FMath.min(trianglePoints[0].z(), trianglePoints[1].z(), trianglePoints[2].z()) > this.maxCoordinates.z()) {
 			return false;
 		}
-		if (FMath.max(_trianglePoints[0].x, _trianglePoints[1].x, _trianglePoints[2].x) < this.minCoordinates.x) {
+		if (FMath.max(trianglePoints[0].x(), trianglePoints[1].x(), trianglePoints[2].x()) < this.minCoordinates.x()) {
 			return false;
 		}
-		if (FMath.max(_trianglePoints[0].y, _trianglePoints[1].y, _trianglePoints[2].y) < this.minCoordinates.y) {
+		if (FMath.max(trianglePoints[0].y(), trianglePoints[1].y(), trianglePoints[2].y()) < this.minCoordinates.y()) {
 			return false;
 		}
-		if (FMath.max(_trianglePoints[0].z, _trianglePoints[1].z, _trianglePoints[2].z) < this.minCoordinates.z) {
+		if (FMath.max(trianglePoints[0].z(), trianglePoints[1].z(), trianglePoints[2].z()) < this.minCoordinates.z()) {
 			return false;
 		}
 		return true;
 	}
 	
 	/*
-	 * @brief check if the ray intersects the AABB
+	 * check if the ray intersects the AABB
 	 * This method use the line vs AABB raycasting technique described in
 	 * Real-time Collision Detection by Christer Ericson.
-	 * @param[in] _ray Ray to test
+	 * @param ray Ray to test
 	 * @return true The raytest intersect the AABB box
 	 */
 	public boolean testRayIntersect(final Ray ray) {
-		final Vector3f point2 = ray.point2.lessNew(ray.point1).multiply(ray.maxFraction).add(ray.point1);
-		final Vector3f e = this.maxCoordinates.lessNew(this.minCoordinates);
-		final Vector3f d = point2.lessNew(ray.point1);
-		final Vector3f m = ray.point1.addNew(point2).less(this.minCoordinates).less(this.maxCoordinates);
+		final Vector3f point2 = ray.point2.less(ray.point1).multiply(ray.maxFraction).add(ray.point1);
+		final Vector3f e = this.maxCoordinates.less(this.minCoordinates);
+		final Vector3f d = point2.less(ray.point1);
+		final Vector3f m = ray.point1.add(point2).less(this.minCoordinates).less(this.maxCoordinates);
 		// Test if the AABB face normals are separating axis
-		float adx = FMath.abs(d.x);
-		if (FMath.abs(m.x) > e.x + adx) {
+		float adx = FMath.abs(d.x());
+		if (FMath.abs(m.x()) > e.x() + adx) {
 			return false;
 		}
-		float ady = FMath.abs(d.y);
-		if (FMath.abs(m.y) > e.y + ady) {
+		float ady = FMath.abs(d.y());
+		if (FMath.abs(m.y()) > e.y() + ady) {
 			return false;
 		}
-		float adz = FMath.abs(d.z);
-		if (FMath.abs(m.z) > e.z + adz) {
+		float adz = FMath.abs(d.z());
+		if (FMath.abs(m.z()) > e.z() + adz) {
 			return false;
 		}
 		// Add in an epsilon term to counteract arithmetic errors when segment is
@@ -301,13 +247,13 @@ public class AABB {
 		adz += epsilon;
 		// Test if the cross products between face normals and ray direction are
 		// separating axis
-		if (FMath.abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady) {
+		if (FMath.abs(m.y() * d.z() - m.z() * d.y()) > e.y() * adz + e.z() * ady) {
 			return false;
 		}
-		if (FMath.abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx) {
+		if (FMath.abs(m.z() * d.x() - m.x() * d.z()) > e.x() * adz + e.z() * adx) {
 			return false;
 		}
-		if (FMath.abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx) {
+		if (FMath.abs(m.x() * d.y() - m.y() * d.x()) > e.x() * ady + e.y() * adx) {
 			return false;
 		}
 		// No separating axis has been found

src/org/atriasoft/ephysics/collision/shapes/Bounds.java

@@ -0,0 +1,10 @@
+package org.atriasoft.ephysics.collision.shapes;
+
+import org.atriasoft.etk.math.Vector3f;
+
+@SuppressWarnings("preview")
+public record Bounds(
+		Vector3f min,
+		Vector3f max) {
+	
+}

src/org/atriasoft/ephysics/collision/shapes/BoxShape.java

@@ -1,11 +1,4 @@
-/** @file
- * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
- * @author Daniel CHAPPUIS
- * @author Edouard DUPIN
- * @copyright 2010-2016, Daniel Chappuis
- * @copyright 2017-now, Edouard DUPIN
- * @license MPL v2.0 (see license file)
- */
+
 package org.atriasoft.ephysics.collision.shapes;
 
 import org.atriasoft.ephysics.RaycastInfo;
@@ -26,17 +19,16 @@ import org.atriasoft.etk.math.Vector3f;
  * distance using the "margin" parameter in the constructor of the box shape. Otherwise, it is recommended to use the
  * default margin distance by not using the "margin" parameter in the constructor.
  *
- * @author Jason Sorensen <sorensenj@smert.net>
  */
 public class BoxShape extends ConvexShape {
 	
 	// Extent sizes of the box in the x, y and z direction
-	private final Vector3f extent;
+	private Vector3f extent;
 	
 	// Copy-constructor
 	public BoxShape(final BoxShape shape) {
 		super(CollisionShapeType.BOX, shape.margin);
-		this.extent = shape.extent.clone();
+		this.extent = shape.extent;
 	}
 	
 	// Constructor
@@ -46,10 +38,10 @@ public class BoxShape extends ConvexShape {
 	
 	public BoxShape(final Vector3f extent, final float margin) {
 		super(CollisionShapeType.BOX, margin);
-		assert (extent.getX() > 0.0f && extent.getX() > margin);
-		assert (extent.getY() > 0.0f && extent.getY() > margin);
-		assert (extent.getZ() > 0.0f && extent.getZ() > margin);
-		this.extent = extent.lessNew(margin);
+		assert (extent.x() > 0.0f && extent.x() > margin);
+		assert (extent.y() > 0.0f && extent.y() > margin);
+		assert (extent.z() > 0.0f && extent.z() > margin);
+		this.extent = extent.less(margin);
 	}
 	
 	@Override
@@ -58,13 +50,13 @@ public class BoxShape extends ConvexShape {
 	}
 	
 	@Override
-	public void computeLocalInertiaTensor(final Matrix3f tensor, final float _mass) {
-		final float factor = (1.0f / 3.0f) * _mass;
-		final Vector3f realExtent = this.extent.addNew(new Vector3f(this.margin, this.margin, this.margin));
-		final float xSquare = realExtent.x * realExtent.x;
-		final float ySquare = realExtent.y * realExtent.y;
-		final float zSquare = realExtent.z * realExtent.z;
-		tensor.set(factor * (ySquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + ySquare));
+	public Matrix3f computeLocalInertiaTensor(final float mass) {
+		final float factor = (1.0f / 3.0f) * mass;
+		final Vector3f realExtent = this.extent.add(new Vector3f(this.margin, this.margin, this.margin));
+		final float xSquare = realExtent.x() * realExtent.x();
+		final float ySquare = realExtent.y() * realExtent.y();
+		final float zSquare = realExtent.z() * realExtent.z();
+		return new Matrix3f(factor * (ySquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + ySquare));
 	}
 	
 	public Vector3f getExtent() {
@@ -72,48 +64,48 @@ public class BoxShape extends ConvexShape {
 	}
 	
 	@Override
-	public void getLocalBounds(final Vector3f _min, final Vector3f _max) {
-		// Maximum bounds
-		_max.set(this.extent.x + this.margin, this.extent.y + this.margin, this.extent.z + this.margin);
-		// Minimum bounds
-		_min.set(-_max.x, -_max.y, -_max.z);
+	public Bounds getLocalBounds() {
+		Vector3f tmp = new Vector3f(this.extent.x() + this.margin, this.extent.y() + this.margin, this.extent.z() + this.margin);
+		return new Bounds(tmp.multiply(-1), tmp);
 	}
 	
 	/*
-	protected size_t getSizeInBytes() {
+	protected sizet getSizeInBytes() {
 		return sizeof(BoxShape);
 	}
 	 */
 	@Override
-	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
-		//Log.error("getLocalSupportPointWithoutMargin(" + _direction);
-		//Log.error("    extends = " + this.extent);
-		return new Vector3f(_direction.x < 0.0 ? -this.extent.x : this.extent.x, _direction.y < 0.0 ? -this.extent.y : this.extent.y, _direction.z < 0.0 ? -this.extent.z : this.extent.z);
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) {
+		//LOGGER.error("getLocalSupportPointWithoutMargin(" + direction);
+		//LOGGER.error("    extends = " + this.extent);
+		return new Vector3f(direction.x() < 0.0 ? -this.extent.x() : this.extent.x(), direction.y() < 0.0 ? -this.extent.y() : this.extent.y(),
+				direction.z() < 0.0 ? -this.extent.z() : this.extent.z());
 	}
 	
 	@Override
-	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
-		final Vector3f rayDirection = _ray.point2.less(_ray.point1);
-		float tMin = Float.MIN_VALUE;
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
+		final Vector3f rayDirection = ray.point2.less(ray.point1);
+		float tMin = -Float.MAX_VALUE;
 		float tMax = Float.MAX_VALUE;
-		Vector3f normalDirection = new Vector3f(0, 0, 0);
-		Vector3f currentNormal = new Vector3f(0, 0, 0);
+		Vector3f normalDirection = Vector3f.ZERO;
+		Vector3f currentNormal = Vector3f.ZERO;
 		// For each of the three slabs
 		for (int iii = 0; iii < 3; ++iii) {
 			// If ray is parallel to the slab
 			if (FMath.abs(rayDirection.get(iii)) < Constant.FLOAT_EPSILON) {
 				// If the ray's origin is not inside the slab, there is no hit
-				if (_ray.point1.get(iii) > this.extent.get(iii) || _ray.point1.get(iii) < -this.extent.get(iii)) {
+				if (ray.point1.get(iii) > this.extent.get(iii) || ray.point1.get(iii) < -this.extent.get(iii)) {
 					return false;
 				}
 			} else {
 				// Compute the intersection of the ray with the near and far plane of the slab
 				final float oneOverD = 1.0f / rayDirection.get(iii);
-				float t1 = (-this.extent.get(iii) - _ray.point1.get(iii)) * oneOverD;
-				float t2 = (this.extent.get(iii) - _ray.point1.get(iii)) * oneOverD;
-				currentNormal.x = (iii == 0) ? -this.extent.get(iii) : 0.0f;
-				currentNormal.y = (iii == 1) ? -this.extent.get(iii) : 0.0f;
-				currentNormal.z = (iii == 2) ? -this.extent.get(iii) : 0.0f;
+				float t1 = (-this.extent.get(iii) - ray.point1.get(iii)) * oneOverD;
+				float t2 = (this.extent.get(iii) - ray.point1.get(iii)) * oneOverD;
+				float tmpX = (iii == 0) ? -this.extent.get(iii) : 0.0f;
+				float tmpY = (iii == 1) ? -this.extent.get(iii) : 0.0f;
+				float tmpZ = (iii == 2) ? -this.extent.get(iii) : 0.0f;
+				currentNormal = new Vector3f(tmpX, tmpY, tmpZ);
 				// Swap t1 and t2 if need so that t1 is intersection with near plane and
 				// t2 with far plane
 				if (t1 > t2) {
@@ -129,7 +121,7 @@ public class BoxShape extends ConvexShape {
 				}
 				tMax = FMath.min(tMax, t2);
 				// If tMin is larger than the maximum raycasting fraction, we return no hit
-				if (tMin > _ray.maxFraction) {
+				if (tMin > ray.maxFraction) {
 					return false;
 				}
 				// If the slabs intersection is empty, there is no hit
@@ -139,32 +131,32 @@ public class BoxShape extends ConvexShape {
 			}
 		}
 		// If tMin is negative, we return no hit
-		if (tMin < 0.0f || tMin > _ray.maxFraction) {
+		if (tMin < 0.0f || tMin > ray.maxFraction) {
 			return false;
 		}
 		if (normalDirection.isZero()) {
 			return false;
 		}
 		// The ray longersects the three slabs, we compute the hit point
-		final Vector3f localHitPoint = rayDirection.multiplyNew(tMin).add(_ray.point1);
-		_raycastInfo.body = _proxyShape.getBody();
-		_raycastInfo.proxyShape = _proxyShape;
-		_raycastInfo.hitFraction = tMin;
-		_raycastInfo.worldPoint = localHitPoint;
-		_raycastInfo.worldNormal = normalDirection;
+		final Vector3f localHitPoint = rayDirection.multiply(tMin).add(ray.point1);
+		raycastInfo.body = proxyShape.getBody();
+		raycastInfo.proxyShape = proxyShape;
+		raycastInfo.hitFraction = tMin;
+		raycastInfo.worldPoint = localHitPoint;
+		raycastInfo.worldNormal = normalDirection;
 		return true;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
-		this.extent.divide(this.scaling).multiply(_scaling);
-		super.setLocalScaling(_scaling);
+	public void setLocalScaling(final Vector3f scaling) {
+		this.extent = this.extent.divide(this.scaling).multiply(scaling);
+		super.setLocalScaling(scaling);
 	}
 	
 	@Override
-	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
-		return (_localPoint.x < this.extent.x && _localPoint.x > -this.extent.x && _localPoint.y < this.extent.y && _localPoint.y > -this.extent.y && _localPoint.z < this.extent.z
-				&& _localPoint.z > -this.extent.z);
+	public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) {
+		return (localPoint.x() < this.extent.x() && localPoint.x() > -this.extent.x() && localPoint.y() < this.extent.y() && localPoint.y() > -this.extent.y() && localPoint.z() < this.extent.z()
+				&& localPoint.z() > -this.extent.z());
 	}
 	
 }

src/org/atriasoft/ephysics/collision/shapes/CapsuleShape.java

@@ -1,11 +1,3 @@
-/** @file
- * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
- * @author Daniel CHAPPUIS
- * @author Edouard DUPIN
- * @copyright 2010-2016, Daniel Chappuis
- * @copyright 2017-now, Edouard DUPIN
- * @license MPL v2.0 (see license file)
- */
 package org.atriasoft.ephysics.collision.shapes;
 
 import org.atriasoft.ephysics.RaycastInfo;
@@ -23,7 +15,6 @@ import org.atriasoft.etk.math.Vector3f;
  * margin distance. The margin is implicitly the radius and height of the shape. Therefore, no need to specify an object
  * margin for a capsule shape.
  *
- * @author Jason Sorensen <sorensenj@smert.net>
  */
 public class CapsuleShape extends ConvexShape {
 	// Half height of the capsule (height = distance between the centers of the two spheres)
@@ -37,7 +28,7 @@ public class CapsuleShape extends ConvexShape {
 	
 	// Constructor
 	public CapsuleShape(final float radius, final float height) {
-		// TODO: Should radius really be the margin for a capsule? Seems like a bug.
+		// TODO Should radius really be the margin for a capsule? Seems like a bug.
 		super(CollisionShapeType.CAPSULE, radius);
 		this.halfHeight = height * 0.5f;
 	}
@@ -48,7 +39,7 @@ public class CapsuleShape extends ConvexShape {
 	}
 	
 	@Override
-	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
+	public Matrix3f computeLocalInertiaTensor(final float mass) {
 		// The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1
 		final float height = this.halfHeight + this.halfHeight;
 		final float radiusSquare = this.margin * this.margin;
@@ -59,9 +50,9 @@ public class CapsuleShape extends ConvexShape {
 		final float sum1 = 0.4f * radiusSquareDouble;
 		final float sum2 = 0.75f * height * this.margin + 0.5f * heightSquare;
 		final float sum3 = 0.25f * radiusSquare + 1.0f / 12.0f * heightSquare;
-		final float IxxAndzz = factor1 * _mass * (sum1 + sum2) + factor2 * _mass * sum3;
-		final float Iyy = factor1 * _mass * sum1 + factor2 * _mass * 0.25f * radiusSquareDouble;
-		_tensor.set(IxxAndzz, 0.0f, 0.0f, 0.0f, Iyy, 0.0f, 0.0f, 0.0f, IxxAndzz);
+		final float ixxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3;
+		final float iyy = factor1 * mass * sum1 + factor2 * mass * 0.25f * radiusSquareDouble;
+		return new Matrix3f(ixxAndzz, 0.0f, 0.0f, 0.0f, iyy, 0.0f, 0.0f, 0.0f, ixxAndzz);
 	}
 	
 	// Return the height of the capsule
@@ -70,23 +61,17 @@ public class CapsuleShape extends ConvexShape {
 	}
 	
 	@Override
-	public void getLocalBounds(final Vector3f min, final Vector3f max) {
-		// Maximum bounds
-		max.setX(this.margin);
-		max.setY(this.halfHeight + this.margin);
-		max.setZ(this.margin);
-		// Minimum bounds
-		min.setX(-max.x);
-		min.setY(-max.y);
-		min.setZ(-max.z);
+	public Bounds getLocalBounds() {
+		Vector3f tmp = new Vector3f(this.margin, this.halfHeight + this.margin, this.margin);
+		return new Bounds(tmp.multiply(-1), tmp);
 	}
 	
 	@Override
-	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) {
 		// Support point top sphere
-		final float dotProductTop = this.halfHeight * _direction.y;
+		final float dotProductTop = this.halfHeight * direction.y();
 		// Support point bottom sphere
-		final float dotProductBottom = -this.halfHeight * _direction.y;
+		final float dotProductBottom = -this.halfHeight * direction.y();
 		// Return the point with the maximum dot product
 		if (dotProductTop > dotProductBottom) {
 			return new Vector3f(0, this.halfHeight, 0);
@@ -100,23 +85,23 @@ public class CapsuleShape extends ConvexShape {
 	}
 	
 	@Override
-	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
-		final Vector3f n = _ray.point2.lessNew(_ray.point1);
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
+		final Vector3f n = ray.point2.less(ray.point1);
 		final float epsilon = 0.01f;
 		final Vector3f p = new Vector3f(0.0f, -this.halfHeight, 0.0f);
 		final Vector3f q = new Vector3f(0.0f, this.halfHeight, 0.0f);
-		final Vector3f d = q.lessNew(p);
-		final Vector3f m = _ray.point1.lessNew(p);
+		final Vector3f d = q.less(p);
+		final Vector3f m = ray.point1.less(p);
 		float t;
 		final float mDotD = m.dot(d);
 		final float nDotD = n.dot(d);
 		final float dDotD = d.dot(d);
 		// Test if the segment is outside the cylinder
-		final float vec1DotD = _ray.point1.lessNew(new Vector3f(0.0f, -this.halfHeight - this.margin, 0.0f)).dot(d);
+		final float vec1DotD = ray.point1.less(new Vector3f(0.0f, -this.halfHeight - this.margin, 0.0f)).dot(d);
 		if (vec1DotD < 0.0f && vec1DotD + nDotD < 0.0f) {
 			return false;
 		}
-		final float ddotDExtraCaps = 2.0f * this.margin * d.y;
+		final float ddotDExtraCaps = 2.0f * this.margin * d.y();
 		if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) {
 			return false;
 		}
@@ -135,36 +120,36 @@ public class CapsuleShape extends ConvexShape {
 			// If the ray longersects with the "p" endcap of the capsule
 			if (mDotD < 0.0f) {
 				// Check longersection between the ray and the "p" sphere endcap of the capsule
-				final Vector3f hitLocalPoint = new Vector3f();
+				final Vector3f hitLocalPoint = Vector3f.ZERO;
 				final Float hitFraction = 0.0f;
-				if (raycastWithSphereEndCap(_ray.point1, _ray.point2, p, _ray.maxFraction, hitLocalPoint, hitFraction)) {
-					_raycastInfo.body = _proxyShape.getBody();
-					_raycastInfo.proxyShape = _proxyShape;
-					_raycastInfo.hitFraction = hitFraction;
-					_raycastInfo.worldPoint = hitLocalPoint;
-					final Vector3f normalDirection = hitLocalPoint.lessNew(p);
-					_raycastInfo.worldNormal = normalDirection;
+				if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
+					raycastInfo.body = proxyShape.getBody();
+					raycastInfo.proxyShape = proxyShape;
+					raycastInfo.hitFraction = hitFraction;
+					raycastInfo.worldPoint = hitLocalPoint;
+					final Vector3f normalDirection = hitLocalPoint.less(p);
+					raycastInfo.worldNormal = normalDirection;
 					return true;
 				}
 				return false;
-			} else if (mDotD > dDotD) { // If the ray longersects with the "q" endcap of the cylinder
-				// Check longersection between the ray and the "q" sphere endcap of the capsule
-				final Vector3f hitLocalPoint = new Vector3f();
-				final Float hitFraction = 0.0f;
-				if (raycastWithSphereEndCap(_ray.point1, _ray.point2, q, _ray.maxFraction, hitLocalPoint, hitFraction)) {
-					_raycastInfo.body = _proxyShape.getBody();
-					_raycastInfo.proxyShape = _proxyShape;
-					_raycastInfo.hitFraction = hitFraction;
-					_raycastInfo.worldPoint = hitLocalPoint;
-					final Vector3f normalDirection = hitLocalPoint.lessNew(q);
-					_raycastInfo.worldNormal = normalDirection;
-					return true;
-				}
-				return false;
-			} else {
+			}
+			if (mDotD <= dDotD) {
 				// If the origin is inside the cylinder, we return no hit
 				return false;
 			}
+			// Check longersection between the ray and the "q" sphere endcap of the capsule
+			final Vector3f hitLocalPoint = Vector3f.ZERO;
+			final Float hitFraction = 0.0f;
+			if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
+				raycastInfo.body = proxyShape.getBody();
+				raycastInfo.proxyShape = proxyShape;
+				raycastInfo.hitFraction = hitFraction;
+				raycastInfo.worldPoint = hitLocalPoint;
+				final Vector3f normalDirection = hitLocalPoint.less(q);
+				raycastInfo.worldNormal = normalDirection;
+				return true;
+			}
+			return false;
 		}
 		final float b = dDotD * mDotN - nDotD * mDotD;
 		final float discriminant = b * b - a * c;
@@ -178,29 +163,30 @@ public class CapsuleShape extends ConvexShape {
 		final float value = mDotD + t * nDotD;
 		if (value < 0.0f) {
 			// Check longersection between the ray and the "p" sphere endcap of the capsule
-			final Vector3f hitLocalPoint = new Vector3f();
+			final Vector3f hitLocalPoint = Vector3f.ZERO;
 			final Float hitFraction = 0.0f;
-			if (raycastWithSphereEndCap(_ray.point1, _ray.point2, p, _ray.maxFraction, hitLocalPoint, hitFraction)) {
-				_raycastInfo.body = _proxyShape.getBody();
-				_raycastInfo.proxyShape = _proxyShape;
-				_raycastInfo.hitFraction = hitFraction;
-				_raycastInfo.worldPoint = hitLocalPoint;
-				final Vector3f normalDirection = hitLocalPoint.lessNew(p);
-				_raycastInfo.worldNormal = normalDirection;
+			if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
+				raycastInfo.body = proxyShape.getBody();
+				raycastInfo.proxyShape = proxyShape;
+				raycastInfo.hitFraction = hitFraction;
+				raycastInfo.worldPoint = hitLocalPoint;
+				final Vector3f normalDirection = hitLocalPoint.less(p);
+				raycastInfo.worldNormal = normalDirection;
 				return true;
 			}
 			return false;
-		} else if (value > dDotD) { // If the longersection is outside the finite cylinder on the "q" side
+		}
+		if (value > dDotD) { // If the longersection is outside the finite cylinder on the "q" side
 			// Check longersection between the ray and the "q" sphere endcap of the capsule
-			final Vector3f hitLocalPoint = new Vector3f();
+			final Vector3f hitLocalPoint = Vector3f.ZERO;
 			final Float hitFraction = 0.0f;
-			if (raycastWithSphereEndCap(_ray.point1, _ray.point2, q, _ray.maxFraction, hitLocalPoint, hitFraction)) {
-				_raycastInfo.body = _proxyShape.getBody();
-				_raycastInfo.proxyShape = _proxyShape;
-				_raycastInfo.hitFraction = hitFraction;
-				_raycastInfo.worldPoint = hitLocalPoint;
-				final Vector3f normalDirection = hitLocalPoint.lessNew(q);
-				_raycastInfo.worldNormal = normalDirection;
+			if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
+				raycastInfo.body = proxyShape.getBody();
+				raycastInfo.proxyShape = proxyShape;
+				raycastInfo.hitFraction = hitFraction;
+				raycastInfo.worldPoint = hitLocalPoint;
+				final Vector3f normalDirection = hitLocalPoint.less(q);
+				raycastInfo.worldNormal = normalDirection;
 				return true;
 			}
 			return false;
@@ -208,33 +194,33 @@ public class CapsuleShape extends ConvexShape {
 		t = t0;
 		// If the longersection is behind the origin of the ray or beyond the maximum
 		// raycasting distance, we return no hit
-		if (t < 0.0f || t > _ray.maxFraction) {
+		if (t < 0.0f || t > ray.maxFraction) {
 			return false;
 		}
 		// Compute the hit information
-		final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1);
-		_raycastInfo.body = _proxyShape.getBody();
-		_raycastInfo.proxyShape = _proxyShape;
-		_raycastInfo.hitFraction = t;
-		_raycastInfo.worldPoint = localHitPoint;
-		final Vector3f v = localHitPoint.lessNew(p);
-		final Vector3f w = d.multiplyNew(v.dot(d) / d.length2());
-		final Vector3f normalDirection = localHitPoint.lessNew(p.addNew(w)).safeNormalize();
-		_raycastInfo.worldNormal = normalDirection;
+		final Vector3f localHitPoint = n.multiply(t).add(ray.point1);
+		raycastInfo.body = proxyShape.getBody();
+		raycastInfo.proxyShape = proxyShape;
+		raycastInfo.hitFraction = t;
+		raycastInfo.worldPoint = localHitPoint;
+		final Vector3f v = localHitPoint.less(p);
+		final Vector3f w = d.multiply(v.dot(d) / d.length2());
+		final Vector3f normalDirection = localHitPoint.less(p.add(w)).safeNormalize();
+		raycastInfo.worldNormal = normalDirection;
 		return true;
 	}
 	
 	/**
-	 * @brief Raycasting method between a ray one of the two spheres end cap of the capsule
+	 * Raycasting method between a ray one of the two spheres end cap of the capsule
 	 */
-	protected boolean raycastWithSphereEndCap(final Vector3f _point1, final Vector3f _point2, final Vector3f _sphereCenter, final float _maxFraction, Vector3f _hitLocalPoint, Float _hitFraction) {
-		final Vector3f m = _point1.lessNew(_sphereCenter);
+	protected boolean raycastWithSphereEndCap(final Vector3f point1, final Vector3f point2, final Vector3f sphereCenter, final float maxFraction, Vector3f hitLocalPoint, Float hitFraction) {
+		final Vector3f m = point1.less(sphereCenter);
 		final float c = m.dot(m) - this.margin * this.margin;
 		// If the origin of the ray is inside the sphere, we return no longersection
 		if (c < 0.0f) {
 			return false;
 		}
-		final Vector3f rayDirection = _point2.lessNew(_point1);
+		final Vector3f rayDirection = point2.less(point1);
 		final float b = m.dot(rayDirection);
 		// If the origin of the ray is outside the sphere and the ray
 		// is pointing away from the sphere, there is no longersection
@@ -252,32 +238,32 @@ public class CapsuleShape extends ConvexShape {
 		float t = -b - FMath.sqrt(discriminant);
 		assert (t >= 0.0f);
 		// If the hit point is withing the segment ray fraction
-		if (t < _maxFraction * raySquareLength) {
+		if (t < maxFraction * raySquareLength) {
 			// Compute the longersection information
 			t /= raySquareLength;
-			_hitFraction = t;
-			_hitLocalPoint = rayDirection.multiplyNew(t).add(_point1);
+			hitFraction = t;
+			hitLocalPoint = rayDirection.multiply(t).add(point1);
 			return true;
 		}
 		return false;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
-		this.halfHeight = (this.halfHeight / this.scaling.y) * _scaling.y;
-		this.margin = (this.margin / this.scaling.x) * _scaling.x;
-		super.setLocalScaling(_scaling);
+	public void setLocalScaling(final Vector3f scaling) {
+		this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
+		this.margin = (this.margin / this.scaling.x()) * scaling.x();
+		super.setLocalScaling(scaling);
 	}
 	
 	@Override
-	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
-		final float diffYCenterSphere1 = _localPoint.y - this.halfHeight;
-		final float diffYCenterSphere2 = _localPoint.y + this.halfHeight;
-		final float xSquare = _localPoint.x * _localPoint.x;
-		final float zSquare = _localPoint.z * _localPoint.z;
+	public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) {
+		final float diffYCenterSphere1 = localPoint.y() - this.halfHeight;
+		final float diffYCenterSphere2 = localPoint.y() + this.halfHeight;
+		final float xSquare = localPoint.x() * localPoint.x();
+		final float zSquare = localPoint.z() * localPoint.z();
 		final float squareRadius = this.margin * this.margin;
 		// Return true if the point is inside the cylinder or one of the two spheres of the capsule
-		return ((xSquare + zSquare) < squareRadius && _localPoint.y < this.halfHeight && _localPoint.y > -this.halfHeight)
+		return ((xSquare + zSquare) < squareRadius && localPoint.y() < this.halfHeight && localPoint.y() > -this.halfHeight)
 				|| (xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius || (xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
 	}
 	

src/org/atriasoft/ephysics/collision/shapes/CollisionShape.java

@@ -1,21 +1,13 @@
-/** @file
- * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
- * @author Daniel CHAPPUIS
- * @author Edouard DUPIN
- * @copyright 2010-2016, Daniel Chappuis
- * @copyright 2017-now, Edouard DUPIN
- * @license MPL v2.0 (see license file)
- */
-package org.atriasoft.ephysics.collision.shapes;
 
-import org.atriasoft.etk.math.Matrix3f;
-import org.atriasoft.etk.math.Transform3D;
-import org.atriasoft.etk.math.Vector3f;
+package org.atriasoft.ephysics.collision.shapes;
 
 import org.atriasoft.ephysics.RaycastInfo;
 import org.atriasoft.ephysics.collision.ProxyShape;
 import org.atriasoft.ephysics.configuration.Defaults;
 import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
 
 /**
  * This abstract class represents the collision shape associated with a
@@ -23,31 +15,31 @@ import org.atriasoft.ephysics.mathematics.Ray;
  */
 public abstract class CollisionShape {
 	/**
-	 * @brief Get the maximum number of contact
+	 * Get the maximum number of contact
 	 * @return The maximum number of contact manifolds in an overlapping pair given two shape types
 	 */
-	public static int computeNbMaxContactManifolds(final CollisionShapeType _shapeType1, final CollisionShapeType _shapeType2) {
+	public static int computeNbMaxContactManifolds(final CollisionShapeType shapeType1, final CollisionShapeType shapeType2) {
 		// If both shapes are convex
-		if (isConvex(_shapeType1) && isConvex(_shapeType2)) {
-			return Defaults.NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
+		if (CollisionShape.isConvex(shapeType1) && CollisionShape.isConvex(shapeType2)) {
+			return Defaults.NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
 		}
 		// If there is at least one concave shape
 		return Defaults.NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
 	}
 	
 	/**
-	 * @brief Check if the shape is convex
-	 * @param[in] _shapeType shape type
+	 * Check if the shape is convex
+	 * @param shapeType shape type
 	 * @return true If the collision shape is convex
 	 * @return false If it is concave
 	 */
-	public static boolean isConvex(final CollisionShapeType _shapeType) {
-		return _shapeType != CollisionShapeType.CONCAVE_MESH && _shapeType != CollisionShapeType.HEIGHTFIELD;
+	public static boolean isConvex(final CollisionShapeType shapeType) {
+		return shapeType != CollisionShapeType.CONCAVE_MESH && shapeType != CollisionShapeType.HEIGHTFIELD;
 	}
 	
-	protected CollisionShapeType type; //!< Type of the collision shape
 	protected Vector3f scaling; //!< Scaling vector of the collision shape
 	/// Constructor
+	protected CollisionShapeType type; //!< Type of the collision shape
 	
 	public CollisionShape(final CollisionShapeType type) {
 		this.type = type;
@@ -55,41 +47,39 @@ public abstract class CollisionShape {
 	}
 	
 	/**
-	 * @brief Update the AABB of a body using its collision shape
-	 * @param[out] _aabb The axis-aligned bounding box (AABB) of the collision shape computed in world-space coordinates
-	 * @param[in] _transform Transform3D used to compute the AABB of the collision shape
+	 * Update the AABB of a body using its collision shape
+	 * @param aabb The axis-aligned bounding box (AABB) of the collision shape computed in world-space coordinates
+	 * @param transform Transform3D used to compute the AABB of the collision shape
 	 */
-	public void computeAABB(final AABB _aabb, final Transform3D _transform) {
+	public void computeAABB(final AABB aabb, final Transform3D transform) {
 		// Get the local bounds in x,y and z direction
-		final Vector3f minBounds = new Vector3f(0, 0, 0);
-		final Vector3f maxBounds = new Vector3f(0, 0, 0);
-		getLocalBounds(minBounds, maxBounds);
+		Bounds bounds = getLocalBounds();
 		// Rotate the local bounds according to the orientation of the body
-		final Matrix3f worldAxis = _transform.getOrientation().getMatrix().getAbsolute();
-		final Vector3f worldMinBounds = new Vector3f(worldAxis.getColumn(0).dot(minBounds), worldAxis.getColumn(1).dot(minBounds), worldAxis.getColumn(2).dot(minBounds));
-		final Vector3f worldMaxBounds = new Vector3f(worldAxis.getColumn(0).dot(maxBounds), worldAxis.getColumn(1).dot(maxBounds), worldAxis.getColumn(2).dot(maxBounds));
+		final Matrix3f worldAxis = transform.getOrientation().getMatrix().abs();
+		final Vector3f worldMinBounds = new Vector3f(worldAxis.getColumn(0).dot(bounds.min()), worldAxis.getColumn(1).dot(bounds.min()), worldAxis.getColumn(2).dot(bounds.min()));
+		final Vector3f worldMaxBounds = new Vector3f(worldAxis.getColumn(0).dot(bounds.max()), worldAxis.getColumn(1).dot(bounds.max()), worldAxis.getColumn(2).dot(bounds.max()));
 		// Compute the minimum and maximum coordinates of the rotated extents
-		final Vector3f minCoordinates = _transform.getPosition().addNew(worldMinBounds);
-		final Vector3f maxCoordinates = _transform.getPosition().addNew(worldMaxBounds);
+		final Vector3f minCoordinates = transform.getPosition().add(worldMinBounds);
+		final Vector3f maxCoordinates = transform.getPosition().add(worldMaxBounds);
 		// Update the AABB with the new minimum and maximum coordinates
-		_aabb.setMin(minCoordinates);
-		_aabb.setMax(maxCoordinates);
+		aabb.setMin(minCoordinates);
+		aabb.setMax(maxCoordinates);
 	}
 	
 	/**
-	 * @brief Compute the local inertia tensor of the sphere
-	 * @param[out] _tensor The 3x3 inertia tensor matrix of the shape in local-space coordinates
-	 * @param[in] _mass Mass to use to compute the inertia tensor of the collision shape
+	 * Compute the local inertia tensor of the sphere
+	 * @return The 3x3 inertia tensor matrix of the shape in local-space coordinates
+	 * @param mass Mass to use to compute the inertia tensor of the collision shape
 	 */
-	public abstract void computeLocalInertiaTensor(Matrix3f _tensor, float _mass);
+	public abstract Matrix3f computeLocalInertiaTensor(float mass);
 	
 	/**
-	 * @brief Get the local bounds of the shape in x, y and z directions.
+	 * Get the local bounds of the shape in x, y and z directions.
 	 * This method is used to compute the AABB of the box
-	 * @param _min The minimum bounds of the shape in local-space coordinates
-	 * @param _max The maximum bounds of the shape in local-space coordinates
+	 * @param min The minimum bounds of the shape in local-space coordinates
+	 * @param max The maximum bounds of the shape in local-space coordinates
 	 */
-	public abstract void getLocalBounds(Vector3f _min, Vector3f _max);
+	public abstract Bounds getLocalBounds();
 	
 	/// Return the scaling vector of the collision shape
 	public Vector3f getScaling() {
@@ -97,7 +87,7 @@ public abstract class CollisionShape {
 	}
 	
 	/**
-	 * @brief Get the type of the collision shapes
+	 * Get the type of the collision shapes
 	 * @return The type of the collision shape (box, sphere, cylinder, ...)
 	 */
 	public CollisionShapeType getType() {
@@ -105,7 +95,7 @@ public abstract class CollisionShape {
 	}
 	
 	/**
-	 * @brief Check if the shape is convex
+	 * Check if the shape is convex
 	 * @return true If the collision shape is convex
 	 * @return false If it is concave
 	 */
@@ -115,12 +105,12 @@ public abstract class CollisionShape {
 	public abstract boolean raycast(Ray ray, RaycastInfo raycastInfo, ProxyShape proxyShape);
 	
 	/**
-	 * @brief Set the scaling vector of the collision shape
+	 * Set the scaling vector of the collision shape
 	 */
-	public void setLocalScaling(final Vector3f _scaling) {
-		this.scaling = _scaling;
+	public void setLocalScaling(final Vector3f scaling) {
+		this.scaling = scaling;
 	}
 	
 	/// Return true if a point is inside the collision shape
 	public abstract boolean testPointInside(Vector3f worldPoint, ProxyShape proxyShape);
-};
+}

src/org/atriasoft/ephysics/collision/shapes/CollisionShapeType.java

@@ -1,53 +1,24 @@
-/** @file
- * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
- * @author Daniel CHAPPUIS
- * @author Edouard DUPIN
- * @copyright 2010-2016, Daniel Chappuis
- * @copyright 2017-now, Edouard DUPIN
- * @license MPL v2.0 (see license file)
- */
+
 package org.atriasoft.ephysics.collision.shapes;
 
-/**
- * Type of the collision shape
- *
- * @author Jason Sorensen <sorensenj@smert.net>
- */
 public enum CollisionShapeType {
-	TRIANGLE(0),
-	BOX(1),
-	SPHERE(2),
-	CONE(3),
-	CYLINDER(4),
-	CAPSULE(5),
-	CONVEX_MESH(6),
-	CONCAVE_MESH(7),
-	HEIGHTFIELD(8);
+	BOX(1), CAPSULE(5), CONCAVE_MESH(7), CONE(3), CONVEX_MESH(6), CYLINDER(4), HEIGHTFIELD(8), SPHERE(2), TRIANGLE(0);
 	
 	public static final int NB_COLLISION_SHAPE_TYPES = 9;
 	
 	public static CollisionShapeType getType(final int value) {
-		switch (value) {
-			case 0:
-				return TRIANGLE;
-			case 1:
-				return BOX;
-			case 2:
-				return SPHERE;
-			case 3:
-				return CONE;
-			case 4:
-				return CYLINDER;
-			case 5:
-				return CAPSULE;
-			case 6:
-				return CONVEX_MESH;
-			case 7:
-				return CONCAVE_MESH;
-			case 8:
-				return HEIGHTFIELD;
-		}
-		return null;
+		return switch (value) {
+			case 0 -> TRIANGLE;
+			case 1 -> BOX;
+			case 2 -> SPHERE;
+			case 3 -> CONE;
+			case 4 -> CYLINDER;
+			case 5 -> CAPSULE;
+			case 6 -> CONVEX_MESH;
+			case 7 -> CONCAVE_MESH;
+			case 8 -> HEIGHTFIELD;
+			default -> null;
+		};
 	}
 	
 	public final int value;

src/org/atriasoft/ephysics/collision/shapes/ConcaveMeshShape.java

@@ -13,10 +13,11 @@ import org.atriasoft.ephysics.collision.broadphase.CallbackRaycast;
 import org.atriasoft.ephysics.collision.broadphase.DTree;
 import org.atriasoft.ephysics.collision.broadphase.DynamicAABBTree;
 import org.atriasoft.ephysics.collision.shapes.TriangleShape.TriangleRaycastSide;
-import org.atriasoft.ephysics.internal.Log;
 import org.atriasoft.ephysics.mathematics.Ray;
 import org.atriasoft.etk.math.Matrix3f;
 import org.atriasoft.etk.math.Vector3f;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 /**
  *  Represents a static concave mesh shape. Note that collision detection
@@ -24,52 +25,56 @@ import org.atriasoft.etk.math.Vector3f;
  * this shape for a static mesh.
  */
 public class ConcaveMeshShape extends ConcaveShape {
+	static final Logger LOGGER = LoggerFactory.getLogger(ConcaveMeshShape.class);
+	
 	class ConcaveMeshRaycastCallback {
-		private final List<DTree> hitAABBNodes = new ArrayList<>();
-		private final DynamicAABBTree dynamicAABBTree;
 		private final ConcaveMeshShape concaveMeshShape;
-		private final ProxyShape proxyShape;
-		private final RaycastInfo raycastInfo;
-		private final Ray ray;
+		private final DynamicAABBTree dynamicAABBTree;
+		private final List<DTree> hitAABBNodes = new ArrayList<>();
 		private boolean isHit;
-		
+		private final ProxyShape proxyShape;
+		private final Ray ray;
+		private final RaycastInfo raycastInfo;
+
 		// Constructor
-		ConcaveMeshRaycastCallback(final DynamicAABBTree _dynamicAABBTree, final ConcaveMeshShape _concaveMeshShape, final ProxyShape _proxyShape, final RaycastInfo _raycastInfo, final Ray _ray) {
-			this.dynamicAABBTree = _dynamicAABBTree;
-			this.concaveMeshShape = _concaveMeshShape;
-			this.proxyShape = _proxyShape;
-			this.raycastInfo = _raycastInfo;
-			this.ray = _ray;
+		ConcaveMeshRaycastCallback(final DynamicAABBTree dynamicAABBTree, final ConcaveMeshShape concaveMeshShape,
+				final ProxyShape proxyShape, final RaycastInfo raycastInfo, final Ray ray) {
+			this.dynamicAABBTree = dynamicAABBTree;
+			this.concaveMeshShape = concaveMeshShape;
+			this.proxyShape = proxyShape;
+			this.raycastInfo = raycastInfo;
+			this.ray = ray;
 			this.isHit = false;
-			
+
 		}
-		
+
 		/// Return true if a raycast hit has been found
 		public boolean getIsHit() {
 			return this.isHit;
 		}
-		
+
 		/// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
-		public float operator__parenthese(final DTree _node, final Ray _ray) {
+		public float operatorparenthese(final DTree node, final Ray ray) {
 			// Add the id of the hit AABB node longo
-			this.hitAABBNodes.add(_node);
-			return _ray.maxFraction;
+			this.hitAABBNodes.add(node);
+			return ray.maxFraction;
 		}
-		
+
 		/// Raycast all collision shapes that have been collected
 		public void raycastTriangles() {
 			float smallestHitFraction = this.ray.maxFraction;
 			for (final DTree it : this.hitAABBNodes) {
 				// Get the node data (triangle index and mesh subpart index)
-				final int data_0 = this.dynamicAABBTree.getNodeDataInt_0(it);
-				final int data_1 = this.dynamicAABBTree.getNodeDataInt_1(it);
+				final int data0 = this.dynamicAABBTree.getNodeDataInt0(it);
+				final int data1 = this.dynamicAABBTree.getNodeDataInt1(it);
 				// Get the triangle vertices for this node from the concave mesh shape
 				final Vector3f[] trianglePoints = new Vector3f[3];
-				this.concaveMeshShape.getTriangleVerticesWithIndexPointer(data_0, data_1, trianglePoints);
+				this.concaveMeshShape.getTriangleVerticesWithIndexPointer(data0, data1, trianglePoints);
 				// Create a triangle collision shape
 				final float margin = this.concaveMeshShape.getTriangleMargin();
-				
-				final TriangleShape triangleShape = new TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
+
+				final TriangleShape triangleShape = new TriangleShape(trianglePoints[0], trianglePoints[1],
+						trianglePoints[2], margin);
 				triangleShape.setRaycastTestType(this.concaveMeshShape.getRaycastTestType());
 				// Ray casting test against the collision shape
 				final RaycastInfo raycastInfo = new RaycastInfo();
@@ -82,59 +87,61 @@ public class ConcaveMeshShape extends ConcaveShape {
 					this.raycastInfo.hitFraction = raycastInfo.hitFraction;
 					this.raycastInfo.worldPoint = raycastInfo.worldPoint;
 					this.raycastInfo.worldNormal = raycastInfo.worldNormal;
-					this.raycastInfo.meshSubpart = data_0;
-					this.raycastInfo.triangleIndex = data_1;
+					this.raycastInfo.meshSubpart = data0;
+					this.raycastInfo.triangleIndex = data1;
 					smallestHitFraction = raycastInfo.hitFraction;
 					this.isHit = true;
 				}
 			}
 		}
-		
-	};
-	
-	protected TriangleMesh triangleMesh; //!< Triangle mesh
-	
+
+	}
+
 	protected DynamicAABBTree dynamicAABBTree; //!< Dynamic AABB tree to accelerate collision with the triangles
-	
-	public ConcaveMeshShape(final TriangleMesh _triangleMesh) {
+
+	protected TriangleMesh triangleMesh; //!< Triangle mesh
+
+	public ConcaveMeshShape(final TriangleMesh triangleMesh) {
 		super(CollisionShapeType.CONCAVE_MESH);
-		this.triangleMesh = _triangleMesh;
+		this.triangleMesh = triangleMesh;
 		this.raycastTestType = TriangleRaycastSide.FRONT;
 		initBVHTree();
 	}
-	
+
 	@Override
-	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
+	public Matrix3f computeLocalInertiaTensor(final float mass) {
 		// Default inertia tensor
 		// Note that this is not very realistic for a concave triangle mesh.
 		// However, in most cases, it will only be used static bodies and therefore,
 		// the inertia tensor is not used.
-		_tensor.set(_mass, 0.0f, 0.0f, 0.0f, _mass, 0.0f, 0.0f, 0.0f, _mass);
+		return new Matrix3f(mass, 0.0f, 0.0f, 0.0f, mass, 0.0f, 0.0f, 0.0f, mass);
 	}
-	
+
 	@Override
-	public void getLocalBounds(final Vector3f _min, final Vector3f _max) {
+	public Bounds getLocalBounds() {
 		// Get the AABB of the whole tree
 		final AABB treeAABB = this.dynamicAABBTree.getRootAABB();
-		_min.set(treeAABB.getMin());
-		_max.set(treeAABB.getMax());
+		return new Bounds(treeAABB.getMin(), treeAABB.getMax());
 	}
-	
+
 	/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
 	/// given the start vertex index pointer of the triangle.
-	protected void getTriangleVerticesWithIndexPointer(final int _subPart, final int _triangleIndex, final Vector3f[] _outTriangleVertices) {
-		assert (_outTriangleVertices != null);
+	protected void getTriangleVerticesWithIndexPointer(
+			final int subPart,
+			final int triangleIndex,
+			final Vector3f[] outTriangleVertices) {
+		assert (outTriangleVertices != null);
 		// Get the triangle vertex array of the current sub-part
-		final TriangleVertexArray triangleVertexArray = this.triangleMesh.getSubpart(_subPart);
+		final TriangleVertexArray triangleVertexArray = this.triangleMesh.getSubpart(subPart);
 		if (triangleVertexArray == null) {
-			Log.error("get null ...");
+			LOGGER.error("get null ...");
 		}
-		final Triangle trianglePoints = triangleVertexArray.getTriangle(_triangleIndex);
-		_outTriangleVertices[0] = trianglePoints.get(0).multiplyNew(this.scaling);
-		_outTriangleVertices[1] = trianglePoints.get(1).multiplyNew(this.scaling);
-		_outTriangleVertices[2] = trianglePoints.get(2).multiplyNew(this.scaling);
+		final Triangle trianglePoints = triangleVertexArray.getTriangle(triangleIndex);
+		outTriangleVertices[0] = trianglePoints.get(0).multiply(this.scaling);
+		outTriangleVertices[1] = trianglePoints.get(1).multiply(this.scaling);
+		outTriangleVertices[2] = trianglePoints.get(2).multiply(this.scaling);
 	}
-	
+
 	/// Insert all the triangles longo the dynamic AABB tree
 	protected void initBVHTree() {
 		// TODO : Try to randomly add the triangles into the tree to obtain a better tree
@@ -157,53 +164,54 @@ public class ConcaveMeshShape extends ConcaveShape {
 			}
 		}
 	}
-	
+
 	@Override
 	public boolean isConvex() {
 		return false;
 	}
-	
+
 	@Override
-	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
 		// Create the callback object that will compute ray casting against triangles
-		final ConcaveMeshRaycastCallback raycastCallback = new ConcaveMeshRaycastCallback(this.dynamicAABBTree, this, _proxyShape, _raycastInfo, _ray);
+		final ConcaveMeshRaycastCallback raycastCallback = new ConcaveMeshRaycastCallback(this.dynamicAABBTree, this,
+				proxyShape, raycastInfo, ray);
 		// Ask the Dynamic AABB Tree to report all AABB nodes that are hit by the ray.
 		// The raycastCallback object will then compute ray casting against the triangles
 		// in the hit AABBs.
-		this.dynamicAABBTree.raycast(_ray, new CallbackRaycast() {
-			
+		this.dynamicAABBTree.raycast(ray, new CallbackRaycast() {
+
 			@Override
-			public float callback(final DTree _node, final Ray _ray) {
-				return raycastCallback.operator__parenthese(_node, _ray);
+			public float callback(final DTree node, final Ray ray) {
+				return raycastCallback.operatorparenthese(node, ray);
 			}
 		});
 		raycastCallback.raycastTriangles();
 		return raycastCallback.getIsHit();
 	}
-	
+
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
-		super.setLocalScaling(_scaling);
+	public void setLocalScaling(final Vector3f scaling) {
+		super.setLocalScaling(scaling);
 		this.dynamicAABBTree.reset();
 		initBVHTree();
 	}
-	
+
 	@Override
-	public void testAllTriangles(final ConcaveShape.TriangleCallback _callback, final AABB _localAABB) {
+	public void testAllTriangles(final ConcaveShape.TriangleCallback callback, final AABB localAABB) {
 		// Ask the Dynamic AABB Tree to report all the triangles that are overlapping
 		// with the AABB of the convex shape.
 		final ConcaveMeshShape self = this;
-		this.dynamicAABBTree.reportAllShapesOverlappingWithAABB(_localAABB, new CallbackOverlapping() {
+		this.dynamicAABBTree.reportAllShapesOverlappingWithAABB(localAABB, new CallbackOverlapping() {
 			@Override
-			public void callback(final DTree _node) {
+			public void callback(final DTree node) {
 				// Get the node data (triangle index and mesh subpart index)
-				final int data_0 = self.dynamicAABBTree.getNodeDataInt_0(_node);
-				final int data_1 = self.dynamicAABBTree.getNodeDataInt_1(_node);
+				final int data0 = self.dynamicAABBTree.getNodeDataInt0(node);
+				final int data1 = self.dynamicAABBTree.getNodeDataInt1(node);
 				// Get the triangle vertices for this node from the concave mesh shape
 				final Vector3f[] trianglePoints = new Vector3f[3];
-				getTriangleVerticesWithIndexPointer(data_0, data_1, trianglePoints);
+				getTriangleVerticesWithIndexPointer(data0, data1, trianglePoints);
 				// Call the callback to test narrow-phase collision with this triangle
-				_callback.testTriangle(trianglePoints);
+				callback.testTriangle(trianglePoints);
 			}
 		});
 	}

src/org/atriasoft/ephysics/collision/shapes/ConcaveShape.java

@@ -1,17 +1,8 @@
-/** @file
- * Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
- * @author Daniel CHAPPUIS
- * @author Edouard DUPIN
- * @copyright 2010-2016, Daniel Chappuis
- * @copyright 2017-now, Edouard DUPIN
- * @license MPL v2.0 (see license file)
- */
 package org.atriasoft.ephysics.collision.shapes;
 
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.collision.ProxyShape;
 import org.atriasoft.ephysics.collision.shapes.TriangleShape.TriangleRaycastSide;
+import org.atriasoft.etk.math.Vector3f;
 
 /**
  *  This abstract class represents a concave collision shape associated with a
@@ -25,20 +16,20 @@ public abstract class ConcaveShape extends CollisionShape {
 	 */
 	public interface TriangleCallback {
 		/// Report a triangle
-		public void testTriangle(Vector3f[] _trianglePoints);
+		public void testTriangle(Vector3f[] trianglePoints);
 	}
 	
-	boolean isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled
-	
-	protected float triangleMargin; //!< Margin use for collision detection for each triangle
-	
-	protected TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
-	
 	/// Return true if the collision shape is convex, false if it is concave
 	public boolean isConvex;
 	
-	public ConcaveShape(final CollisionShapeType _type) {
-		super(_type);
+	boolean isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled
+	
+	protected TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
+	
+	protected float triangleMargin; //!< Margin use for collision detection for each triangle
+	
+	public ConcaveShape(final CollisionShapeType type) {
+		super(type);
 		this.isSmoothMeshCollisionEnabled = false;
 		this.triangleMargin = 0;
 		this.raycastTestType = TriangleRaycastSide.FRONT;
@@ -65,24 +56,24 @@ public abstract class ConcaveShape extends CollisionShape {
 	 * Smooth mesh collision is used to avoid collisions against some longernal edges of the triangle mesh.
 	 * If it is enabled, collsions with the mesh will be smoother but collisions computation is a bit more expensive.
 	 */
-	public void setIsSmoothMeshCollisionEnabled(final boolean _isEnabled) {
-		this.isSmoothMeshCollisionEnabled = _isEnabled;
+	public void setIsSmoothMeshCollisionEnabled(final boolean isEnabled) {
+		this.isSmoothMeshCollisionEnabled = isEnabled;
 	}
 	
 	/**
 	 *  Set the raycast test type (front, back, front-back)
 	 * @param testType Raycast test type for the triangle (front, back, front-back)
 	 */
-	public void setRaycastTestType(final TriangleRaycastSide _testType) {
-		this.raycastTestType = _testType;
+	public void setRaycastTestType(final TriangleRaycastSide testType) {
+		this.raycastTestType = testType;
 	}
 	
 	/// Use a callback method on all triangles of the concave shape inside a given AABB
-	public abstract void testAllTriangles(TriangleCallback _callback, AABB _localAABB);
+	public abstract void testAllTriangles(TriangleCallback callback, AABB localAABB);
 	
 	@Override
-	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+	public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) {
 		return false;
 	}
 	
-};
+}

src/org/atriasoft/ephysics/collision/shapes/ConeShape.java

@@ -48,34 +48,34 @@ import org.atriasoft.etk.math.Vector3f;
  * default margin distance by not using the "margin" parameter in the ructor.
  */
 public class ConeShape extends ConvexShape {
-	protected float radius; //!< Radius of the base
 	protected float halfHeight; //!< Half height of the cone
+	protected float radius; //!< Radius of the base
 	
 	protected float sinTheta; //!< sine of the semi angle at the apex point
 	
 	/**
 	 *  Constructor
-	 * @param _radius Radius of the cone (in meters)
-	 * @param _height Height of the cone (in meters)
-	 * @param _margin Collision margin (in meters) around the collision shape
+	 * @param radius Radius of the cone (in meters)
+	 * @param height Height of the cone (in meters)
+	 * @param margin Collision margin (in meters) around the collision shape
 	 */
-	public ConeShape(final float _radius, final float _height) {
-		this(_radius, _height, Defaults.OBJECT_MARGIN);
+	public ConeShape(final float radius, final float height) {
+		this(radius, height, Defaults.OBJECT_MARGIN);
 	}
 	
-	public ConeShape(final float _radius, final float _height, final float _margin) {
-		super(CollisionShapeType.CONE, _margin);
-		this.radius = _radius;
-		this.halfHeight = _height * 0.5f;
+	public ConeShape(final float radius, final float height, final float margin) {
+		super(CollisionShapeType.CONE, margin);
+		this.radius = radius;
+		this.halfHeight = height * 0.5f;
 		// Compute the sine of the semi-angle at the apex point
-		this.sinTheta = this.radius / (FMath.sqrt(this.radius * this.radius + _height * _height));
+		this.sinTheta = this.radius / (FMath.sqrt(this.radius * this.radius + height * height));
 	}
 	
 	@Override
-	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
+	public Matrix3f computeLocalInertiaTensor(final float mass) {
 		final float rSquare = this.radius * this.radius;
-		final float diagXZ = 0.15f * _mass * (rSquare + this.halfHeight);
-		_tensor.set(diagXZ, 0.0f, 0.0f, 0.0f, 0.3f * _mass * rSquare, 0.0f, 0.0f, 0.0f, diagXZ);
+		final float diagXZ = 0.15f * mass * (rSquare + this.halfHeight);
+		return new Matrix3f(diagXZ, 0.0f, 0.0f, 0.0f, 0.3f * mass * rSquare, 0.0f, 0.0f, 0.0f, diagXZ);
 	}
 	
 	/**
@@ -87,29 +87,23 @@ public class ConeShape extends ConvexShape {
 	}
 	
 	@Override
-	public void getLocalBounds(final Vector3f min, final Vector3f max) {
-		// Maximum bounds
-		max.setX(this.radius + this.margin);
-		max.setY(this.halfHeight + this.margin);
-		max.setZ(this.radius + this.margin);
-		// Minimum bounds
-		min.setX(-max.x);
-		min.setY(-max.y);
-		min.setZ(-max.z);
+	public Bounds getLocalBounds() {
+		Vector3f max = new Vector3f(this.radius + this.margin, this.halfHeight + this.margin, this.radius + this.margin);
+		return new Bounds(max.multiply(-1), max);
 	}
 	
 	@Override
-	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
-		final Vector3f v = _direction;
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) {
+		final Vector3f v = direction;
 		final float sinThetaTimesLengthV = this.sinTheta * v.length();
 		Vector3f supportPoint;
-		if (v.y > sinThetaTimesLengthV) {
+		if (v.y() > sinThetaTimesLengthV) {
 			supportPoint = new Vector3f(0.0f, this.halfHeight, 0.0f);
 		} else {
-			final float projectedLength = FMath.sqrt(v.x * v.x + v.z * v.z);
+			final float projectedLength = FMath.sqrt(v.x() * v.x() + v.z() * v.z());
 			if (projectedLength > Constant.FLOAT_EPSILON) {
 				final float d = this.radius / projectedLength;
-				supportPoint = new Vector3f(v.x * d, -this.halfHeight, v.z * d);
+				supportPoint = new Vector3f(v.x() * d, -this.halfHeight, v.z() * d);
 			} else {
 				supportPoint = new Vector3f(0.0f, -this.halfHeight, 0.0f);
 			}
@@ -126,20 +120,20 @@ public class ConeShape extends ConvexShape {
 	}
 	
 	@Override
-	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
-		final Vector3f r = _ray.point2.lessNew(_ray.point1);
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
+		final Vector3f r = ray.point2.less(ray.point1);
 		final float epsilon = 0.00001f;
-		final Vector3f V = new Vector3f(0, this.halfHeight, 0);
+		final Vector3f vvv = new Vector3f(0, this.halfHeight, 0);
 		final Vector3f centerBase = new Vector3f(0, -this.halfHeight, 0);
 		final Vector3f axis = new Vector3f(0, -1.0f, 0);
 		final float heightSquare = 4.0f * this.halfHeight * this.halfHeight;
 		final float cosThetaSquare = heightSquare / (heightSquare + this.radius * this.radius);
 		final float factor = 1.0f - cosThetaSquare;
-		final Vector3f delta = _ray.point1.lessNew(V);
-		final float c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y - cosThetaSquare * delta.z * delta.z;
-		final float c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z;
-		final float c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z;
-		final float tHit[] = { -1.0f, -1.0f, -1.0f };
+		final Vector3f delta = ray.point1.less(vvv);
+		final float c0 = -cosThetaSquare * delta.x() * delta.x() + factor * delta.y() * delta.y() - cosThetaSquare * delta.z() * delta.z();
+		final float c1 = -cosThetaSquare * delta.x() * r.x() + factor * delta.y() * r.y() - cosThetaSquare * delta.z() * r.z();
+		final float c2 = -cosThetaSquare * r.x() * r.x() + factor * r.y() * r.y() - cosThetaSquare * r.z() * r.z();
+		final float[] tHit = { -1.0f, -1.0f, -1.0f };
 		final Vector3f[] localHitPoint = new Vector3f[3];
 		final Vector3f[] localNormal = new Vector3f[3];
 		// If c2 is different from zero
@@ -148,7 +142,8 @@ public class ConeShape extends ConvexShape {
 			// If there is no real roots in the quadratic equation
 			if (gamma < 0.0f) {
 				return false;
-			} else if (gamma > 0.0f) { // The equation has two real roots
+			}
+			if (gamma > 0.0f) { // The equation has two real roots
 				// Compute two longersections
 				final float sqrRoot = FMath.sqrt(gamma);
 				tHit[0] = (-c1 - sqrRoot) / c2;
@@ -169,32 +164,32 @@ public class ConeShape extends ConvexShape {
 			return false;
 		}
 		// If the origin of the ray is inside the cone, we return no hit
-		if (testPointInside(_ray.point1, null)) {
+		if (testPointInside(ray.point1, null)) {
 			return false;
 		}
-		localHitPoint[0] = r.multiplyNew(tHit[0]).add(_ray.point1);
-		localHitPoint[1] = r.multiplyNew(tHit[1]).add(_ray.point1);
+		localHitPoint[0] = r.multiply(tHit[0]).add(ray.point1);
+		localHitPoint[1] = r.multiply(tHit[1]).add(ray.point1);
 		// Only keep hit points in one side of the double cone (the cone we are longerested in)
-		if (axis.dot(localHitPoint[0].lessNew(V)) < 0.0f) {
+		if (axis.dot(localHitPoint[0].less(vvv)) < 0.0f) {
 			tHit[0] = -1.0f;
 		}
-		if (axis.dot(localHitPoint[1].lessNew(V)) < 0.0f) {
+		if (axis.dot(localHitPoint[1].less(vvv)) < 0.0f) {
 			tHit[1] = -1.0f;
 		}
 		// Only keep hit points that are within the correct height of the cone
-		if (localHitPoint[0].y < -this.halfHeight) {
+		if (localHitPoint[0].y() < -this.halfHeight) {
 			tHit[0] = -1.0f;
 		}
-		if (localHitPoint[1].y < -this.halfHeight) {
+		if (localHitPoint[1].y() < -this.halfHeight) {
 			tHit[1] = -1.0f;
 		}
 		// If the ray is in direction of the base plane of the cone
-		if (r.y > epsilon) {
+		if (r.y() > epsilon) {
 			// Compute the longersection with the base plane of the cone
-			tHit[2] = (-_ray.point1.y - this.halfHeight) / (r.y);
+			tHit[2] = (-ray.point1.y() - this.halfHeight) / (r.y());
 			// Only keep this longersection if it is inside the cone radius
-			localHitPoint[2] = r.multiplyNew(tHit[2]).add(_ray.point1);
-			if ((localHitPoint[2].lessNew(centerBase)).length2() > this.radius * this.radius) {
+			localHitPoint[2] = r.multiply(tHit[2]).add(ray.point1);
+			if ((localHitPoint[2].less(centerBase)).length2() > this.radius * this.radius) {
 				tHit[2] = -1.0f;
 			}
 			// Compute the normal direction
@@ -215,41 +210,39 @@ public class ConeShape extends ConvexShape {
 		if (hitIndex < 0) {
 			return false;
 		}
-		if (t > _ray.maxFraction) {
+		if (t > ray.maxFraction) {
 			return false;
 		}
 		// Compute the normal direction for hit against side of the cone
 		if (hitIndex != 2) {
 			final float h = 2.0f * this.halfHeight;
-			final float value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x + localHitPoint[hitIndex].z * localHitPoint[hitIndex].z);
+			final float value1 = (localHitPoint[hitIndex].x() * localHitPoint[hitIndex].x() + localHitPoint[hitIndex].z() * localHitPoint[hitIndex].z());
 			final float rOverH = this.radius / h;
 			final float value2 = 1.0f + rOverH * rOverH;
 			final float factor22 = 1.0f / FMath.sqrt(value1 * value2);
-			final float x = localHitPoint[hitIndex].x * factor22;
-			final float z = localHitPoint[hitIndex].z * factor22;
-			localNormal[hitIndex].setX(x);
-			localNormal[hitIndex].setY(FMath.sqrt(x * x + z * z) * rOverH);
-			localNormal[hitIndex].setZ(z);
+			final float x = localHitPoint[hitIndex].x() * factor22;
+			final float z = localHitPoint[hitIndex].z() * factor22;
+			localNormal[hitIndex] = new Vector3f(x, FMath.sqrt(x * x + z * z) * rOverH, z);
 		}
-		_raycastInfo.body = _proxyShape.getBody();
-		_raycastInfo.proxyShape = _proxyShape;
-		_raycastInfo.hitFraction = t;
-		_raycastInfo.worldPoint = localHitPoint[hitIndex];
-		_raycastInfo.worldNormal = localNormal[hitIndex];
+		raycastInfo.body = proxyShape.getBody();
+		raycastInfo.proxyShape = proxyShape;
+		raycastInfo.hitFraction = t;
+		raycastInfo.worldPoint = localHitPoint[hitIndex];
+		raycastInfo.worldNormal = localNormal[hitIndex];
 		return true;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
-		this.halfHeight = (this.halfHeight / this.scaling.y) * _scaling.y;
-		this.radius = (this.radius / this.scaling.x) * _scaling.x;
-		super.setLocalScaling(_scaling);
+	public void setLocalScaling(final Vector3f scaling) {
+		this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
+		this.radius = (this.radius / this.scaling.x()) * scaling.x();
+		super.setLocalScaling(scaling);
 	}
 	
 	@Override
-	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
-		final float radiusHeight = this.radius * (-_localPoint.y + this.halfHeight) / (this.halfHeight * 2.0f);
-		return (_localPoint.y < this.halfHeight && _localPoint.y > -this.halfHeight) && (_localPoint.x * _localPoint.x + _localPoint.z * _localPoint.z < radiusHeight * radiusHeight);
+	public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) {
+		final float radiusHeight = this.radius * (-localPoint.y() + this.halfHeight) / (this.halfHeight * 2.0f);
+		return (localPoint.y() < this.halfHeight && localPoint.y() > -this.halfHeight) && (localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z() < radiusHeight * radiusHeight);
 	}
 	
 }

src/org/atriasoft/ephysics/collision/shapes/ConvexMeshShape.java

@@ -29,15 +29,14 @@ import java.util.HashMap;
 import java.util.List;
 import java.util.Map;
 
-import org.atriasoft.etk.math.Matrix3f;
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.RaycastInfo;
 import org.atriasoft.ephysics.collision.ProxyShape;
 import org.atriasoft.ephysics.collision.TriangleVertexArray;
 import org.atriasoft.ephysics.configuration.Defaults;
 import org.atriasoft.ephysics.mathematics.Ray;
 import org.atriasoft.ephysics.mathematics.SetInteger;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Vector3f;
 
 /**
  *  It represents a convex mesh shape. In order to create a convex mesh shape, you
@@ -55,13 +54,13 @@ import org.atriasoft.ephysics.mathematics.SetInteger;
  * in order to use the edges information for collision detection.
  */
 public class ConvexMeshShape extends ConvexShape {
-	protected List<Vector3f> vertices = new ArrayList<>(); //!< Array with the vertices of the mesh
-	protected int numberVertices = 0; //!< Number of vertices in the mesh
-	protected Vector3f minBounds = new Vector3f(); //!< Mesh minimum bounds in the three local x, y and z directions
-	protected Vector3f maxBounds = new Vector3f(); //!< Mesh maximum bounds in the three local x, y and z directions
-	protected boolean isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster
-	
 	protected Map<Integer, SetInteger> edgesAdjacencyList = new HashMap<>(); //!< Adjacency list representing the edges of the mesh
+	protected boolean isEdgesInformationUsed; //!< True if the shape contains the edges of the convex mesh in order to make the collision detection faster
+	protected Vector3f maxBounds = Vector3f.ZERO; //!< Mesh maximum bounds in the three local x, y and z directions
+	protected Vector3f minBounds = Vector3f.ZERO; //!< Mesh minimum bounds in the three local x, y and z directions
+	protected int numberVertices = 0; //!< Number of vertices in the mesh
+	
+	protected List<Vector3f> vertices = new ArrayList<>(); //!< Array with the vertices of the mesh
 	
 	/**
 	 *  Constructor.
@@ -71,8 +70,8 @@ public class ConvexMeshShape extends ConvexShape {
 		this(Defaults.OBJECT_MARGIN);
 	}
 	
-	public ConvexMeshShape(final float _margin) {
-		super(CollisionShapeType.CONVEX_MESH, _margin);
+	public ConvexMeshShape(final float margin) {
+		super(CollisionShapeType.CONVEX_MESH, margin);
 		this.minBounds = new Vector3f(0, 0, 0);
 		this.maxBounds = new Vector3f(0, 0, 0);
 		this.numberVertices = 0;
@@ -82,26 +81,26 @@ public class ConvexMeshShape extends ConvexShape {
 	/**
 	 *  Constructor to initialize with an array of 3D vertices.
 	 * This method creates an longernal copy of the input vertices.
-	 * @param _arrayVertices Array with the vertices of the convex mesh
-	 * @param _nbVertices Number of vertices in the convex mesh
-	 * @param _stride Stride between the beginning of two elements in the vertices array
-	 * @param _margin Collision margin (in meters) around the collision shape
+	 * @param arrayVertices Array with the vertices of the convex mesh
+	 * @param nbVertices Number of vertices in the convex mesh
+	 * @param stride Stride between the beginning of two elements in the vertices array
+	 * @param margin Collision margin (in meters) around the collision shape
 	 */
-	public ConvexMeshShape(final float[] _arrayVertices, final int _nbVertices, final int _stride) {
-		this(_arrayVertices, _nbVertices, _stride, Defaults.OBJECT_MARGIN);
+	public ConvexMeshShape(final float[] arrayVertices, final int nbVertices, final int stride) {
+		this(arrayVertices, nbVertices, stride, Defaults.OBJECT_MARGIN);
 	}
 	
-	public ConvexMeshShape(final float[] _arrayVertices, final int _nbVertices, final int _stride, final float _margin) {
-		super(CollisionShapeType.CONVEX_MESH, _margin);
-		this.numberVertices = _nbVertices;
+	public ConvexMeshShape(final float[] arrayVertices, final int nbVertices, final int stride, final float margin) {
+		super(CollisionShapeType.CONVEX_MESH, margin);
+		this.numberVertices = nbVertices;
 		this.minBounds = new Vector3f(0, 0, 0);
 		this.maxBounds = new Vector3f(0, 0, 0);
 		this.isEdgesInformationUsed = false;
 		int offset = 0;
 		// Copy all the vertices longo the longernal array
 		for (long iii = 0; iii < this.numberVertices; iii++) {
-			this.vertices.add(new Vector3f(_arrayVertices[offset], _arrayVertices[offset + 1], _arrayVertices[offset + 2]));
-			offset += _stride;
+			this.vertices.add(new Vector3f(arrayVertices[offset], arrayVertices[offset + 1], arrayVertices[offset + 2]));
+			offset += stride;
 		}
 		// Recalculate the bounds of the mesh
 		recalculateBounds();
@@ -110,35 +109,35 @@ public class ConvexMeshShape extends ConvexShape {
 	/**
 	 *  Constructor to initialize with a triangle mesh
 	 * This method creates an internal copy of the input vertices.
-	 * @param _triangleVertexArray Array with the vertices and indices of the vertices and triangles of the mesh
-	 * @param _isEdgesInformationUsed True if you want to use edges information for collision detection (faster but requires more memory)
-	 * @param _margin Collision margin (in meters) around the collision shape
+	 * @param triangleVertexArray Array with the vertices and indices of the vertices and triangles of the mesh
+	 * @param isEdgesInformationUsed True if you want to use edges information for collision detection (faster but requires more memory)
+	 * @param margin Collision margin (in meters) around the collision shape
 	 */
-	public ConvexMeshShape(final TriangleVertexArray _triangleVertexArray) {
-		this(_triangleVertexArray, true);
+	public ConvexMeshShape(final TriangleVertexArray triangleVertexArray) {
+		this(triangleVertexArray, true);
 	}
 	
-	public ConvexMeshShape(final TriangleVertexArray _triangleVertexArray, final boolean _isEdgesInformationUsed) {
-		this(_triangleVertexArray, true, Defaults.OBJECT_MARGIN);
+	public ConvexMeshShape(final TriangleVertexArray triangleVertexArray, final boolean isEdgesInformationUsed) {
+		this(triangleVertexArray, true, Defaults.OBJECT_MARGIN);
 	}
 	
-	public ConvexMeshShape(final TriangleVertexArray _triangleVertexArray, final boolean _isEdgesInformationUsed, final float _margin) {
-		super(CollisionShapeType.CONVEX_MESH, _margin);
+	public ConvexMeshShape(final TriangleVertexArray triangleVertexArray, final boolean isEdgesInformationUsed, final float margin) {
+		super(CollisionShapeType.CONVEX_MESH, margin);
 		this.minBounds = new Vector3f(0, 0, 0);
 		this.maxBounds = new Vector3f(0, 0, 0);
-		this.isEdgesInformationUsed = _isEdgesInformationUsed;
+		this.isEdgesInformationUsed = isEdgesInformationUsed;
 		// For each vertex of the mesh
-		for (final Vector3f it : _triangleVertexArray.getVertices()) {
-			this.vertices.add(it.multiplyNew(this.scaling));
+		for (final Vector3f it : triangleVertexArray.getVertices()) {
+			this.vertices.add(it.multiply(this.scaling));
 		}
 		// If we need to use the edges information of the mesh
 		if (this.isEdgesInformationUsed) {
 			// For each triangle of the mesh
-			for (int iii = 0; iii < _triangleVertexArray.getNbTriangles(); iii++) {
-				final int vertexIndex[] = { 0, 0, 0 };
-				vertexIndex[0] = _triangleVertexArray.getIndices()[iii * 3];
-				vertexIndex[1] = _triangleVertexArray.getIndices()[iii * 3 + 1];
-				vertexIndex[2] = _triangleVertexArray.getIndices()[iii * 3 + 2];
+			for (int iii = 0; iii < triangleVertexArray.getNbTriangles(); iii++) {
+				final int[] vertexIndex = { 0, 0, 0 };
+				vertexIndex[0] = triangleVertexArray.getIndices()[iii * 3];
+				vertexIndex[1] = triangleVertexArray.getIndices()[iii * 3 + 1];
+				vertexIndex[2] = triangleVertexArray.getIndices()[iii * 3 + 2];
 				// Add information about the edges
 				addEdge(vertexIndex[0], vertexIndex[1]);
 				addEdge(vertexIndex[0], vertexIndex[2]);
@@ -154,112 +153,69 @@ public class ConvexMeshShape extends ConvexShape {
 	 * @note that the vertex indices start at zero and need to correspond to the order of
 	 * the vertices in the vertices array in the ructor or the order of the calls
 	 * of the addVertex methods that you use to add vertices longo the convex mesh.
-	 * @param _v1 Index of the first vertex of the edge to add
-	 * @param _v2 Index of the second vertex of the edge to add
+	 * @param v1 Index of the first vertex of the edge to add
+	 * @param v2 Index of the second vertex of the edge to add
 	 */
-	public void addEdge(final int _v1, final int _v2) {
+	public void addEdge(final int v1, final int v2) {
 		// If the entry for vertex v1 does not exist in the adjacency list
-		if (!this.edgesAdjacencyList.containsKey(_v1)) {
-			this.edgesAdjacencyList.put(_v1, new SetInteger());
+		if (!this.edgesAdjacencyList.containsKey(v1)) {
+			this.edgesAdjacencyList.put(v1, new SetInteger());
 		}
 		// If the entry for vertex v2 does not exist in the adjacency list
-		if (!this.edgesAdjacencyList.containsKey(_v2)) {
-			this.edgesAdjacencyList.put(_v2, new SetInteger());
+		if (!this.edgesAdjacencyList.containsKey(v2)) {
+			this.edgesAdjacencyList.put(v2, new SetInteger());
 		}
 		// Add the edge in the adjacency list
-		this.edgesAdjacencyList.get(_v1).add(_v2);
-		this.edgesAdjacencyList.get(_v2).add(_v1);
+		this.edgesAdjacencyList.get(v1).add(v2);
+		this.edgesAdjacencyList.get(v2).add(v1);
 	}
 	
 	/**
 	 *  Add a vertex longo the convex mesh
 	 * @param vertex Vertex to be added
 	 */
-	public void addVertex(final Vector3f _vertex) {
+	public void addVertex(final Vector3f vertex) {
 		// Add the vertex in to vertices array
-		this.vertices.add(_vertex);
+		this.vertices.add(vertex);
 		this.numberVertices++;
-		// Update the bounds of the mesh
-		if (_vertex.x * this.scaling.x > this.maxBounds.x) {
-			this.maxBounds.setX(_vertex.x * this.scaling.x);
-		}
-		if (_vertex.x * this.scaling.x < this.minBounds.x) {
-			this.minBounds.setX(_vertex.x * this.scaling.x);
-		}
-		if (_vertex.y * this.scaling.y > this.maxBounds.y) {
-			this.maxBounds.setY(_vertex.y * this.scaling.y);
-		}
-		if (_vertex.y * this.scaling.y < this.minBounds.y) {
-			this.minBounds.setY(_vertex.y * this.scaling.y);
-		}
-		if (_vertex.z * this.scaling.z > this.maxBounds.z) {
-			this.maxBounds.setZ(_vertex.z * this.scaling.z);
-		}
-		if (_vertex.z * this.scaling.z < this.minBounds.z) {
-			this.minBounds.setZ(_vertex.z * this.scaling.z);
-		}
+		this.maxBounds = this.maxBounds.max(vertex.multiply(this.scaling));
+		this.minBounds = this.minBounds.max(vertex.multiply(this.scaling));
 	}
 	
 	@Override
-	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
-		final float factor = (1.0f / 3.0f) * _mass;
-		final Vector3f realExtent = this.maxBounds.lessNew(this.minBounds).multiply(0.5f);
-		assert (realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0);
-		final float xSquare = realExtent.x * realExtent.x;
-		final float ySquare = realExtent.y * realExtent.y;
-		final float zSquare = realExtent.z * realExtent.z;
-		_tensor.set(factor * (ySquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + ySquare));
+	public Matrix3f computeLocalInertiaTensor(final float mass) {
+		final float factor = (1.0f / 3.0f) * mass;
+		final Vector3f realExtent = this.maxBounds.less(this.minBounds).multiply(0.5f);
+		assert (realExtent.x() > 0 && realExtent.y() > 0 && realExtent.z() > 0);
+		final float xSquare = realExtent.x() * realExtent.x();
+		final float ySquare = realExtent.y() * realExtent.y();
+		final float zSquare = realExtent.z() * realExtent.z();
+		return new Matrix3f(factor * (ySquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + zSquare), 0.0f, 0.0f, 0.0f, factor * (xSquare + ySquare));
 	}
 	
 	@Override
-	public void getLocalBounds(final Vector3f _min, final Vector3f _max) {
-		_min.set(this.minBounds);
-		_max.set(this.maxBounds);
+	public Bounds getLocalBounds() {
+		return new Bounds(this.minBounds, this.maxBounds);
 	}
 	
 	@Override
-	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) {
 		assert (this.numberVertices == this.vertices.size());
-		assert (_cachedCollisionData != null);
+		assert (cachedCollisionData != null);
 		// Allocate memory for the cached collision data if not allocated yet
-		if (_cachedCollisionData.data == null) {
+		if (cachedCollisionData.data == null) {
 			// TODO the data is nort set outside ==> find how ...  !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
-			_cachedCollisionData.data = 0;
+			cachedCollisionData.data = 0;
 		}
 		// If the edges information is used to speed up the collision detection
-		if (this.isEdgesInformationUsed) {
-			assert (this.edgesAdjacencyList.size() == this.numberVertices);
-			int maxVertex = (Integer) (_cachedCollisionData.data);
-			float maxDotProduct = _direction.dot(this.vertices.get(maxVertex));
-			boolean isOptimal;
-			// Perform hill-climbing (local search)
-			do {
-				isOptimal = true;
-				assert (this.edgesAdjacencyList.get(maxVertex).size() > 0);
-				// For all neighbors of the current vertex
-				for (final Integer it : this.edgesAdjacencyList.get(maxVertex).getRaw()) {
-					// Compute the dot product
-					final float dotProduct = _direction.dot(this.vertices.get(it));
-					// If the current vertex is a better vertex (larger dot product)
-					if (dotProduct > maxDotProduct) {
-						maxVertex = it;
-						maxDotProduct = dotProduct;
-						isOptimal = false;
-					}
-				}
-			} while (!isOptimal);
-			// Cache the support vertex
-			_cachedCollisionData.data = maxVertex;
-			// Return the support vertex
-			return this.vertices.get(maxVertex).multiplyNew(this.scaling);
-		} else {
+		if (!this.isEdgesInformationUsed) {
 			// If the edges information is not used
-			double maxDotProduct = Float.MIN_VALUE;
+			double maxDotProduct = -Float.MAX_VALUE;
 			int indexMaxDotProduct = 0;
 			// For each vertex of the mesh
 			for (int i = 0; i < this.numberVertices; i++) {
 				// Compute the dot product of the current vertex
-				final double dotProduct = _direction.dot(this.vertices.get(i));
+				final double dotProduct = direction.dot(this.vertices.get(i));
 				// If the current dot product is larger than the maximum one
 				if (dotProduct > maxDotProduct) {
 					indexMaxDotProduct = i;
@@ -268,8 +224,32 @@ public class ConvexMeshShape extends ConvexShape {
 			}
 			assert (maxDotProduct >= 0.0f);
 			// Return the vertex with the largest dot product in the support direction
-			return this.vertices.get(indexMaxDotProduct).multiplyNew(this.scaling);
+			return this.vertices.get(indexMaxDotProduct).multiply(this.scaling);
 		}
+		assert (this.edgesAdjacencyList.size() == this.numberVertices);
+		int maxVertex = (Integer) (cachedCollisionData.data);
+		float maxDotProduct = direction.dot(this.vertices.get(maxVertex));
+		boolean isOptimal;
+		// Perform hill-climbing (local search)
+		do {
+			isOptimal = true;
+			assert (this.edgesAdjacencyList.get(maxVertex).size() > 0);
+			// For all neighbors of the current vertex
+			for (final Integer it : this.edgesAdjacencyList.get(maxVertex).getRaw()) {
+				// Compute the dot product
+				final float dotProduct = direction.dot(this.vertices.get(it));
+				// If the current vertex is a better vertex (larger dot product)
+				if (dotProduct > maxDotProduct) {
+					maxVertex = it;
+					maxDotProduct = dotProduct;
+					isOptimal = false;
+				}
+			}
+		} while (!isOptimal);
+		// Cache the support vertex
+		cachedCollisionData.data = maxVertex;
+		// Return the support vertex
+		return this.vertices.get(maxVertex).multiply(this.scaling);
 	}
 	
 	/**
@@ -281,43 +261,27 @@ public class ConvexMeshShape extends ConvexShape {
 	}
 	
 	@Override
-	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
-		return _proxyShape.getBody().getWorld().getCollisionDetection().getNarrowPhaseGJKAlgorithm().raycast(_ray, _proxyShape, _raycastInfo);
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
+		return proxyShape.getBody().getWorld().getCollisionDetection().getNarrowPhaseGJKAlgorithm().raycast(ray, proxyShape, raycastInfo);
 	}
 	
 	/// Recompute the bounds of the mesh
 	protected void recalculateBounds() {
 		// TODO : Only works if the local origin is inside the mesh
 		//		=> Make it more robust (init with first vertex of mesh instead)
-		this.minBounds.setZero();
-		this.maxBounds.setZero();
+		this.minBounds = Vector3f.ZERO;
+		this.maxBounds = Vector3f.ZERO;
 		// For each vertex of the mesh
 		for (int i = 0; i < this.numberVertices; i++) {
-			if (this.vertices.get(i).x > this.maxBounds.x) {
-				this.maxBounds.setX(this.vertices.get(i).x);
-			}
-			if (this.vertices.get(i).x < this.minBounds.x) {
-				this.minBounds.setX(this.vertices.get(i).x);
-			}
-			if (this.vertices.get(i).y > this.maxBounds.y) {
-				this.maxBounds.setY(this.vertices.get(i).y);
-			}
-			if (this.vertices.get(i).y < this.minBounds.y) {
-				this.minBounds.setY(this.vertices.get(i).y);
-			}
-			if (this.vertices.get(i).z > this.maxBounds.z) {
-				this.maxBounds.setZ(this.vertices.get(i).z);
-			}
-			if (this.vertices.get(i).z < this.minBounds.z) {
-				this.minBounds.setZ(this.vertices.get(i).z);
-			}
+			this.minBounds = this.minBounds.min(this.vertices.get(i));
+			this.maxBounds = this.maxBounds.max(this.vertices.get(i));
 		}
 		// Apply the local scaling factor
-		this.maxBounds.multiply(this.scaling);
-		this.minBounds.multiply(this.scaling);
+		this.maxBounds = this.maxBounds.multiply(this.scaling);
+		this.minBounds = this.minBounds.multiply(this.scaling);
 		// Add the object margin to the bounds
-		this.maxBounds.add(this.margin);
-		this.minBounds.less(this.margin);
+		this.maxBounds = this.maxBounds.add(this.margin);
+		this.minBounds = this.minBounds.less(this.margin);
 	}
 	
 	/**
@@ -325,19 +289,19 @@ public class ConvexMeshShape extends ConvexShape {
 	 * collision detection
 	 * @param isEdgesUsed True if you want to use the edges information to speed up the collision detection with the convex mesh shape
 	 */
-	public void setIsEdgesInformationUsed(final boolean _isEdgesUsed) {
-		this.isEdgesInformationUsed = _isEdgesUsed;
+	public void setIsEdgesInformationUsed(final boolean isEdgesUsed) {
+		this.isEdgesInformationUsed = isEdgesUsed;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
-		super.setLocalScaling(_scaling);
+	public void setLocalScaling(final Vector3f scaling) {
+		super.setLocalScaling(scaling);
 		recalculateBounds();
 	}
 	
 	@Override
-	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+	public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) {
 		// Use the GJK algorithm to test if the point is inside the convex mesh
-		return _proxyShape.getBody().getWorld().getCollisionDetection().getNarrowPhaseGJKAlgorithm().testPointInside(_localPoint, _proxyShape);
+		return proxyShape.getBody().getWorld().getCollisionDetection().getNarrowPhaseGJKAlgorithm().testPointInside(localPoint, proxyShape);
 	}
-};
+}

src/org/atriasoft/ephysics/collision/shapes/ConvexShape.java

@@ -8,38 +8,38 @@ public abstract class ConvexShape extends CollisionShape {
 	protected float margin; //!< Margin used for the GJK collision detection algorithm
 	/// Constructor
 	
-	public ConvexShape(final CollisionShapeType _type, final float _margin) {
-		super(_type);
-		this.margin = _margin;
+	public ConvexShape(final CollisionShapeType type, final float margin) {
+		super(type);
+		this.margin = margin;
 	}
 	
 	// Return a local support point in a given direction with the object margin
-	public Vector3f getLocalSupportPointWithMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
-		////Log.error(" ->  getLocalSupportPointWithMargin(" + _direction);
+	public Vector3f getLocalSupportPointWithMargin(final Vector3f direction, final CacheData cachedCollisionData) {
+		////LOGGER.error(" ->  getLocalSupportPointWithMargin(" + direction);
 		// Get the support point without margin
-		final Vector3f supportPoint = getLocalSupportPointWithoutMargin(_direction, _cachedCollisionData);
-		////Log.error(" ->  supportPoint = " + supportPoint);
-		////Log.error(" ->  margin = " + FMath.floatToString(this.margin));
+		Vector3f supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
+		////LOGGER.error(" ->  supportPoint = " + supportPoint);
+		////LOGGER.error(" ->  margin = " + FMath.floatToString(this.margin));
 		if (this.margin != 0.0f) {
 			// Add the margin to the support point
 			Vector3f unitVec = new Vector3f(0.0f, -1.0f, 0.0f);
-			////Log.error(" ->      _direction.length2()=" + FMath.floatToString(_direction.length2()));
-			////Log.error(" ->      Constant.FLOAT_EPSILON=" + FMath.floatToString(Constant.FLOAT_EPSILON));
-			if (_direction.length2() > Constant.FLOAT_EPSILON * Constant.FLOAT_EPSILON) {
-				unitVec = _direction.safeNormalizeNew();
-				////Log.error(" ->          unitVec= " + unitVec);
+			////LOGGER.error(" ->      direction.length2()=" + FMath.floatToString(direction.length2()));
+			////LOGGER.error(" ->      Constant.FLOATEPSILON=" + FMath.floatToString(Constant.FLOATEPSILON));
+			if (direction.length2() > Constant.FLOAT_EPSILON * Constant.FLOAT_EPSILON) {
+				unitVec = direction.safeNormalize();
+				////LOGGER.error(" ->          unitVec= " + unitVec);
 			}
-			supportPoint.add(unitVec.multiplyNew(this.margin));
+			supportPoint = supportPoint.add(unitVec.multiply(this.margin));
 		}
-		////Log.error(" ->      ==> supportPoint = " + supportPoint);
+		////LOGGER.error(" ->      ==> supportPoint = " + supportPoint);
 		return supportPoint;
 	}
 	
 	/// Return a local support point in a given direction without the object margin
-	public abstract Vector3f getLocalSupportPointWithoutMargin(Vector3f _direction, CacheData _cachedCollisionData);
+	public abstract Vector3f getLocalSupportPointWithoutMargin(Vector3f direction, CacheData cachedCollisionData);
 	
 	/**
-	 * @brief Get the current object margin
+	 * Get the current object margin
 	 * @return The margin (in meters) around the collision shape
 	 */
 	public float getMargin() {
@@ -52,5 +52,5 @@ public abstract class ConvexShape extends CollisionShape {
 	}
 	
 	@Override
-	public abstract boolean testPointInside(Vector3f _worldPoint, ProxyShape _proxyShape);
+	public abstract boolean testPointInside(Vector3f worldPoint, ProxyShape proxyShape);
 }

src/org/atriasoft/ephysics/collision/shapes/CylinderShape.java

@@ -58,8 +58,8 @@ import org.atriasoft.etk.math.Vector3f;
  * default margin distance by not using the "margin" parameter in the ructor.
  */
 public class CylinderShape extends ConvexShape {
-	protected float radius; //!< Radius of the base
 	protected float halfHeight; //!< Half height of the cylinder
+	protected float radius; //!< Radius of the base
 	
 	/**
 	 * Contructor
@@ -67,21 +67,21 @@ public class CylinderShape extends ConvexShape {
 	 * @param height Height of the cylinder (in meters)
 	 * @param margin Collision margin (in meters) around the collision shape
 	 */
-	public CylinderShape(final float _radius, final float _height) {
-		this(_radius, _height, Defaults.OBJECT_MARGIN);
+	public CylinderShape(final float radius, final float height) {
+		this(radius, height, Defaults.OBJECT_MARGIN);
 	}
 	
-	public CylinderShape(final float _radius, final float _height, final float _margin) {
-		super(CollisionShapeType.CYLINDER, _margin);
-		this.radius = _radius;
-		this.halfHeight = _height / 2.0f;
+	public CylinderShape(final float radius, final float height, final float margin) {
+		super(CollisionShapeType.CYLINDER, margin);
+		this.radius = radius;
+		this.halfHeight = height / 2.0f;
 	}
 	
 	@Override
-	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
+	public Matrix3f computeLocalInertiaTensor(final float mass) {
 		final float height = 2.0f * this.halfHeight;
-		final float diag = (1.0f / 12.0f) * _mass * (3.0f * this.radius * this.radius + height * height);
-		_tensor.set(diag, 0.0f, 0.0f, 0.0f, 0.5f * _mass * this.radius * this.radius, 0.0f, 0.0f, 0.0f, diag);
+		final float diag = (1.0f / 12.0f) * mass * (3.0f * this.radius * this.radius + height * height);
+		return new Matrix3f(diag, 0.0f, 0.0f, 0.0f, 0.5f * mass * this.radius * this.radius, 0.0f, 0.0f, 0.0f, diag);
 	}
 	
 	/**
@@ -93,36 +93,31 @@ public class CylinderShape extends ConvexShape {
 	}
 	
 	@Override
-	public void getLocalBounds(final Vector3f min, final Vector3f max) {
-		// Maximum bounds
-		max.setX(this.radius + this.margin);
-		max.setY(this.halfHeight + this.margin);
-		max.setZ(this.radius + this.margin);
-		// Minimum bounds
-		min.setX(-max.x);
-		min.setY(-max.y);
-		min.setZ(-max.z);
+	public Bounds getLocalBounds() {
+		Vector3f max = new Vector3f(this.radius + this.margin, this.halfHeight + this.margin, this.radius + this.margin);
+		return new Bounds(max.multiply(-1), max);
 	}
 	
 	@Override
-	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
-		final Vector3f supportPoint = new Vector3f(0.0f, 0.0f, 0.0f);
-		final float uDotv = _direction.y;
-		final Vector3f w = new Vector3f(_direction.x, 0.0f, _direction.z);
-		final float lengthW = FMath.sqrt(_direction.x * _direction.x + _direction.z * _direction.z);
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) {
+		Vector3f supportPoint = Vector3f.ZERO;
+		final float uDotv = direction.y();
+		final Vector3f w = new Vector3f(direction.x(), 0.0f, direction.z());
+		final float lengthW = FMath.sqrt(direction.x() * direction.x() + direction.z() * direction.z());
+		float yyy = 0;
 		if (lengthW > Constant.FLOAT_EPSILON) {
 			if (uDotv < 0.0f) {
-				supportPoint.setY(-this.halfHeight);
+				yyy = -this.halfHeight;
 			} else {
-				supportPoint.setY(this.halfHeight);
+				yyy = this.halfHeight;
 			}
-			supportPoint.add(w.multiply(this.radius / lengthW));
+			supportPoint = w.multiply(this.radius / lengthW);
 		} else if (uDotv < 0.0f) {
-			supportPoint.setY(-this.halfHeight);
+			yyy = -this.halfHeight;
 		} else {
-			supportPoint.setY(this.halfHeight);
+			yyy = this.halfHeight;
 		}
-		return supportPoint;
+		return supportPoint.add(new Vector3f(0, yyy, 0));
 	}
 	
 	/**
@@ -134,13 +129,13 @@ public class CylinderShape extends ConvexShape {
 	}
 	
 	@Override
-	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
-		final Vector3f n = _ray.point2.lessNew(_ray.point1);
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
+		final Vector3f n = ray.point2.less(ray.point1);
 		final float epsilon = 0.01f;
 		final Vector3f p = new Vector3f(0.0f, -this.halfHeight, 0.0f);
 		final Vector3f q = new Vector3f(0.0f, this.halfHeight, 0.0f);
-		final Vector3f d = q.lessNew(p);
-		final Vector3f m = _ray.point1.lessNew(p);
+		final Vector3f d = q.less(p);
+		final Vector3f m = ray.point1.less(p);
 		float t;
 		final float mDotD = m.dot(d);
 		final float nDotD = n.dot(d);
@@ -170,17 +165,17 @@ public class CylinderShape extends ConvexShape {
 				t = -mDotN / nDotN;
 				// If the longersection is behind the origin of the ray or beyond the maximum
 				// raycasting distance, we return no hit
-				if (t < 0.0f || t > _ray.maxFraction) {
+				if (t < 0.0f || t > ray.maxFraction) {
 					return false;
 				}
 				// Compute the hit information
-				final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1);
-				_raycastInfo.body = _proxyShape.getBody();
-				_raycastInfo.proxyShape = _proxyShape;
-				_raycastInfo.hitFraction = t;
-				_raycastInfo.worldPoint = localHitPoint;
+				final Vector3f localHitPoint = n.multiply(t).add(ray.point1);
+				raycastInfo.body = proxyShape.getBody();
+				raycastInfo.proxyShape = proxyShape;
+				raycastInfo.hitFraction = t;
+				raycastInfo.worldPoint = localHitPoint;
 				final Vector3f normalDirection = new Vector3f(0.0f, -1.0f, 0.0f);
-				_raycastInfo.worldNormal = normalDirection;
+				raycastInfo.worldNormal = normalDirection;
 				return true;
 			}
 			// If the ray longersects with the "q" endcap of the cylinder
@@ -188,16 +183,16 @@ public class CylinderShape extends ConvexShape {
 				t = (nDotD - mDotN) / nDotN;
 				// If the longersection is behind the origin of the ray or beyond the maximum
 				// raycasting distance, we return no hit
-				if (t < 0.0f || t > _ray.maxFraction) {
+				if (t < 0.0f || t > ray.maxFraction) {
 					return false;
 				}
 				// Compute the hit information
-				final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1);
-				_raycastInfo.body = _proxyShape.getBody();
-				_raycastInfo.proxyShape = _proxyShape;
-				_raycastInfo.hitFraction = t;
-				_raycastInfo.worldPoint = localHitPoint;
-				_raycastInfo.worldNormal = new Vector3f(0, 1.0f, 0);
+				final Vector3f localHitPoint = n.multiply(t).add(ray.point1);
+				raycastInfo.body = proxyShape.getBody();
+				raycastInfo.proxyShape = proxyShape;
+				raycastInfo.hitFraction = t;
+				raycastInfo.worldPoint = localHitPoint;
+				raycastInfo.worldNormal = new Vector3f(0, 1.0f, 0);
 				return true;
 			}
 			// If the origin is inside the cylinder, we return no hit
@@ -226,16 +221,16 @@ public class CylinderShape extends ConvexShape {
 			}
 			// If the longersection is behind the origin of the ray or beyond the maximum
 			// raycasting distance, we return no hit
-			if (t < 0.0f || t > _ray.maxFraction) {
+			if (t < 0.0f || t > ray.maxFraction) {
 				return false;
 			}
 			// Compute the hit information
-			final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1);
-			_raycastInfo.body = _proxyShape.getBody();
-			_raycastInfo.proxyShape = _proxyShape;
-			_raycastInfo.hitFraction = t;
-			_raycastInfo.worldPoint = localHitPoint;
-			_raycastInfo.worldNormal = new Vector3f(0, -1.0f, 0);
+			final Vector3f localHitPoint = n.multiply(t).add(ray.point1);
+			raycastInfo.body = proxyShape.getBody();
+			raycastInfo.proxyShape = proxyShape;
+			raycastInfo.hitFraction = t;
+			raycastInfo.worldPoint = localHitPoint;
+			raycastInfo.worldNormal = new Vector3f(0, -1.0f, 0);
 			return true;
 		}
 		// If the longersection is outside the cylinder on the "q" side
@@ -252,46 +247,46 @@ public class CylinderShape extends ConvexShape {
 			}
 			// If the longersection is behind the origin of the ray or beyond the maximum
 			// raycasting distance, we return no hit
-			if (t < 0.0f || t > _ray.maxFraction) {
+			if (t < 0.0f || t > ray.maxFraction) {
 				return false;
 			}
 			// Compute the hit information
-			final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1);
-			_raycastInfo.body = _proxyShape.getBody();
-			_raycastInfo.proxyShape = _proxyShape;
-			_raycastInfo.hitFraction = t;
-			_raycastInfo.worldPoint = localHitPoint;
-			_raycastInfo.worldNormal = new Vector3f(0, 1.0f, 0);
+			final Vector3f localHitPoint = n.multiply(t).add(ray.point1);
+			raycastInfo.body = proxyShape.getBody();
+			raycastInfo.proxyShape = proxyShape;
+			raycastInfo.hitFraction = t;
+			raycastInfo.worldPoint = localHitPoint;
+			raycastInfo.worldNormal = new Vector3f(0, 1.0f, 0);
 			return true;
 		}
 		t = t0;
 		// If the longersection is behind the origin of the ray or beyond the maximum
 		// raycasting distance, we return no hit
-		if (t < 0.0f || t > _ray.maxFraction) {
+		if (t < 0.0f || t > ray.maxFraction) {
 			return false;
 		}
 		// Compute the hit information
-		final Vector3f localHitPoint = n.multiplyNew(t).add(_ray.point1);
-		_raycastInfo.body = _proxyShape.getBody();
-		_raycastInfo.proxyShape = _proxyShape;
-		_raycastInfo.hitFraction = t;
-		_raycastInfo.worldPoint = localHitPoint;
-		final Vector3f v = localHitPoint.lessNew(p);
-		final Vector3f w = d.multiplyNew(v.dot(d) / d.length2());
-		final Vector3f normalDirection = localHitPoint.lessNew(p.addNew(w));
-		_raycastInfo.worldNormal = normalDirection;
+		final Vector3f localHitPoint = n.multiply(t).add(ray.point1);
+		raycastInfo.body = proxyShape.getBody();
+		raycastInfo.proxyShape = proxyShape;
+		raycastInfo.hitFraction = t;
+		raycastInfo.worldPoint = localHitPoint;
+		final Vector3f v = localHitPoint.less(p);
+		final Vector3f w = d.multiply(v.dot(d) / d.length2());
+		final Vector3f normalDirection = localHitPoint.less(p.add(w));
+		raycastInfo.worldNormal = normalDirection;
 		return true;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
-		this.halfHeight = (this.halfHeight / this.scaling.y) * _scaling.y;
-		this.radius = (this.radius / this.scaling.x) * _scaling.x;
-		super.setLocalScaling(_scaling);
+	public void setLocalScaling(final Vector3f scaling) {
+		this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
+		this.radius = (this.radius / this.scaling.x()) * scaling.x();
+		super.setLocalScaling(scaling);
 	}
 	
 	@Override
-	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
-		return ((_localPoint.x * _localPoint.x + _localPoint.z * _localPoint.z) < this.radius * this.radius && _localPoint.y < this.halfHeight && _localPoint.y > -this.halfHeight);
+	public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) {
+		return ((localPoint.x() * localPoint.x() + localPoint.z() * localPoint.z()) < this.radius * this.radius && localPoint.y() < this.halfHeight && localPoint.y() > -this.halfHeight);
 	}
 }

src/org/atriasoft/ephysics/collision/shapes/HeightFieldShape.java

@@ -1,17 +1,16 @@
 package org.atriasoft.ephysics.collision.shapes;
 
-import org.atriasoft.etk.math.FMath;
-import org.atriasoft.etk.math.Matrix3f;
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.RaycastInfo;
 import org.atriasoft.ephysics.collision.ProxyShape;
 import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Vector3f;
 
 /**
  *  This class represents a static height field that can be used to represent
  * a terrain. The height field is made of a grid with rows and columns with a
- * height value at each grid point. Note that the height values are not copied longo the shape
+ * height value at each grid point. Note that the height values are not copied into the shape
  * but are shared instead. The height values can be of type longeger, float or double.
  * When creating a HeightFieldShape, you need to specify the minimum and maximum height value of
  * your height field. Note that the HeightFieldShape will be re-centered based on its AABB. It means
@@ -24,18 +23,18 @@ public class HeightFieldShape extends ConcaveShape {
 	 *  This class is used for testing AABB and triangle overlap for raycasting
 	 */
 	public class TriangleOverlapCallback implements TriangleCallback {
-		protected Ray ray;
-		protected ProxyShape proxyShape;
-		protected RaycastInfo raycastInfo;
-		protected boolean isHit;
-		protected float smallestHitFraction;
 		protected HeightFieldShape heightFieldShape;
+		protected boolean isHit;
+		protected ProxyShape proxyShape;
+		protected Ray ray;
+		protected RaycastInfo raycastInfo;
+		protected float smallestHitFraction;
 		
-		public TriangleOverlapCallback(final Ray _ray, final ProxyShape _proxyShape, final RaycastInfo _raycastInfo, final HeightFieldShape _heightFieldShape) {
-			this.ray = _ray;
-			this.proxyShape = _proxyShape;
-			this.raycastInfo = _raycastInfo;
-			this.heightFieldShape = _heightFieldShape;
+		public TriangleOverlapCallback(final Ray ray, final ProxyShape proxyShape, final RaycastInfo raycastInfo, final HeightFieldShape heightFieldShape) {
+			this.ray = ray;
+			this.proxyShape = proxyShape;
+			this.raycastInfo = raycastInfo;
+			this.heightFieldShape = heightFieldShape;
 			this.isHit = false;
 			this.smallestHitFraction = this.ray.maxFraction;
 		}
@@ -46,10 +45,10 @@ public class HeightFieldShape extends ConcaveShape {
 		
 		/// Raycast test between a ray and a triangle of the heightfield
 		@Override
-		public void testTriangle(final Vector3f[] _trianglePoints) {
+		public void testTriangle(final Vector3f[] trianglePoints) {
 			// Create a triangle collision shape
 			final float margin = this.heightFieldShape.getTriangleMargin();
-			final TriangleShape triangleShape = new TriangleShape(_trianglePoints[0], _trianglePoints[1], _trianglePoints[2], margin);
+			final TriangleShape triangleShape = new TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
 			triangleShape.setRaycastTestType(this.heightFieldShape.getRaycastTestType());
 			// Ray casting test against the collision shape
 			final RaycastInfo raycastInfo = new RaycastInfo();
@@ -68,18 +67,18 @@ public class HeightFieldShape extends ConcaveShape {
 				this.isHit = true;
 			}
 		}
-	};
+	}
 	
+	protected AABB aabb; //!< Local AABB of the height field (without scaling)
+	protected float[] heightFieldData; //!< Array of data with all the height values of the height field
+	
+	protected float length; //!< Height field length
+	protected float maxHeight; //!< Maximum height of the height field
+	protected float minHeight; //!< Minimum height of the height field
 	protected int numberColumns; //!< Number of columns in the grid of the height field
 	protected int numberRows; //!< Number of rows in the grid of the height field
-	
-	protected float width; //!< Height field width
-	protected float length; //!< Height field length
-	protected float minHeight; //!< Minimum height of the height field
-	protected float maxHeight; //!< Maximum height of the height field
 	protected int upAxis; //!< Up axis direction (0 => x, 1 => y, 2 => z)
-	protected float[] heightFieldData; //!< Array of data with all the height values of the height field
-	protected AABB AABB; //!< Local AABB of the height field (without scaling)
+	protected float width; //!< Height field width
 	
 	/**
 		 *  Contructor
@@ -92,87 +91,87 @@ public class HeightFieldShape extends ConcaveShape {
 		 * @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z)
 		 * @param longegerHeightScale Scaling factor used to scale the height values (only when height values type is longeger)
 		 */
-	public HeightFieldShape(final int _nbGridColumns, final int _nbGridRows, final float _minHeight, final float _maxHeight, final float[] _heightFieldData) {
-		this(_nbGridColumns, _nbGridRows, _minHeight, _maxHeight, _heightFieldData, 1);
+	public HeightFieldShape(final int nbGridColumns, final int nbGridRows, final float minHeight, final float maxHeight, final float[] heightFieldData) {
+		this(nbGridColumns, nbGridRows, minHeight, maxHeight, heightFieldData, 1);
 	}
 	
 	/// Insert all the triangles longo the dynamic AABB tree
 	//protected void initBVHTree();
 	
-	public HeightFieldShape(final int _nbGridColumns, final int _nbGridRows, final float _minHeight, final float _maxHeight, final float[] _heightFieldData, final int _upAxis) {
+	public HeightFieldShape(final int nbGridColumns, final int nbGridRows, final float minHeight, final float maxHeight, final float[] heightFieldData, final int upAxis) {
 		super(CollisionShapeType.HEIGHTFIELD);
-		this.numberColumns = _nbGridColumns;
-		this.numberRows = _nbGridRows;
-		this.width = _nbGridColumns - 1;
-		this.length = _nbGridRows - 1;
-		this.minHeight = _minHeight;
-		this.maxHeight = _maxHeight;
-		this.upAxis = _upAxis;
-		assert (_nbGridColumns >= 2);
-		assert (_nbGridRows >= 2);
+		this.numberColumns = nbGridColumns;
+		this.numberRows = nbGridRows;
+		this.width = nbGridColumns - 1;
+		this.length = nbGridRows - 1;
+		this.minHeight = minHeight;
+		this.maxHeight = maxHeight;
+		this.upAxis = upAxis;
+		assert (nbGridColumns >= 2);
+		assert (nbGridRows >= 2);
 		assert (this.width >= 1);
 		assert (this.length >= 1);
-		assert (_minHeight <= _maxHeight);
-		assert (_upAxis == 0 || _upAxis == 1 || _upAxis == 2);
-		this.heightFieldData = _heightFieldData;
+		assert (minHeight <= maxHeight);
+		assert (upAxis == 0 || upAxis == 1 || upAxis == 2);
+		this.heightFieldData = heightFieldData;
 		final float halfHeight = (this.maxHeight - this.minHeight) * 0.5f;
 		assert (halfHeight >= 0);
 		// Compute the local AABB of the height field
 		if (this.upAxis == 0) {
-			this.AABB.setMin(new Vector3f(-halfHeight, -this.width * 0.5f, -this.length * 0.5f));
-			this.AABB.setMax(new Vector3f(halfHeight, this.width * 0.5f, this.length * 0.5f));
+			this.aabb.setMin(new Vector3f(-halfHeight, -this.width * 0.5f, -this.length * 0.5f));
+			this.aabb.setMax(new Vector3f(halfHeight, this.width * 0.5f, this.length * 0.5f));
 		} else if (this.upAxis == 1) {
-			this.AABB.setMin(new Vector3f(-this.width * 0.5f, -halfHeight, -this.length * 0.5f));
-			this.AABB.setMax(new Vector3f(this.width * 0.5f, halfHeight, this.length * 0.5f));
+			this.aabb.setMin(new Vector3f(-this.width * 0.5f, -halfHeight, -this.length * 0.5f));
+			this.aabb.setMax(new Vector3f(this.width * 0.5f, halfHeight, this.length * 0.5f));
 		} else if (this.upAxis == 2) {
-			this.AABB.setMin(new Vector3f(-this.width * 0.5f, -this.length * 0.5f, -halfHeight));
-			this.AABB.setMax(new Vector3f(this.width * 0.5f, this.length * 0.5f, halfHeight));
+			this.aabb.setMin(new Vector3f(-this.width * 0.5f, -this.length * 0.5f, -halfHeight));
+			this.aabb.setMax(new Vector3f(this.width * 0.5f, this.length * 0.5f, halfHeight));
 		}
 	}
 	
 	/// Return the closest inside longeger grid value of a given floating grid value
-	protected int computeIntegerGridValue(final float _value) {
-		return (int) ((_value < 0.0f) ? _value - 0.5f : _value + 0.5f);
+	protected int computeIntegerGridValue(final float value) {
+		return (int) ((value < 0.0f) ? value - 0.5f : value + 0.5f);
 	}
 	
 	@Override
-	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
+	public Matrix3f computeLocalInertiaTensor(final float mass) {
 		// Default inertia tensor
 		// Note that this is not very realistic for a concave triangle mesh.
 		// However, in most cases, it will only be used static bodies and therefore,
 		// the inertia tensor is not used.
-		_tensor.set(_mass, 0.0f, 0.0f, 0.0f, _mass, 0.0f, 0.0f, 0.0f, _mass);
+		return new Matrix3f(mass, 0.0f, 0.0f, 0.0f, mass, 0.0f, 0.0f, 0.0f, mass);
 	}
 	
 	/// Compute the min/max grid coords corresponding to the longersection of the AABB of the height field and the AABB to collide
-	protected void computeMinMaxGridCoordinates(final Vector3f _minCoords, final Vector3f _maxCoords, final AABB _aabbToCollide) {
+	protected Bounds computeMinMaxGridCoordinates(final AABB aabbToCollide) {
 		// Clamp the min/max coords of the AABB to collide inside the height field AABB
-		Vector3f minPoint = FMath.max(_aabbToCollide.getMin(), this.AABB.getMin());
-		minPoint = FMath.min(minPoint, this.AABB.getMax());
-		Vector3f maxPoint = FMath.min(_aabbToCollide.getMax(), this.AABB.getMax());
-		maxPoint = FMath.max(maxPoint, this.AABB.getMin());
+		Vector3f minPoint = FMath.max(aabbToCollide.getMin(), this.aabb.getMin());
+		minPoint = FMath.min(minPoint, this.aabb.getMax());
+		Vector3f maxPoint = FMath.min(aabbToCollide.getMax(), this.aabb.getMax());
+		maxPoint = FMath.max(maxPoint, this.aabb.getMin());
 		// Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints]
 		// and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... this.width/2]
 		// and [-this.length/2 ... this.length/2]
-		final Vector3f translateVec = this.AABB.getExtent().multiplyNew(0.5f);
-		minPoint.add(translateVec);
-		maxPoint.add(translateVec);
+		final Vector3f translateVec = this.aabb.getExtent().multiply(0.5f);
+		minPoint = minPoint.add(translateVec);
+		maxPoint = maxPoint.add(translateVec);
 		// Convert the floating min/max coords of the AABB longo closest longeger
 		// grid values (note that we use the closest grid coordinate that is out
 		// of the AABB)
-		_minCoords.set(computeIntegerGridValue(minPoint.x) - 1, computeIntegerGridValue(minPoint.y) - 1, computeIntegerGridValue(minPoint.z) - 1);
-		_maxCoords.set(computeIntegerGridValue(maxPoint.x) + 1, computeIntegerGridValue(maxPoint.y) + 1, computeIntegerGridValue(maxPoint.z) + 1);
+		Vector3f minCoords = new Vector3f(computeIntegerGridValue(minPoint.x()) - 1, computeIntegerGridValue(minPoint.y()) - 1, computeIntegerGridValue(minPoint.z()) - 1);
+		Vector3f maxCoords = new Vector3f(computeIntegerGridValue(maxPoint.x()) + 1, computeIntegerGridValue(maxPoint.y()) + 1, computeIntegerGridValue(maxPoint.z()) + 1);
+		return new Bounds(minCoords, maxCoords);
 	}
 	
 	/// Return the height of a given (x,y) point in the height field
-	protected float getHeightAt(final int _xxx, final int _yyy) {
-		return this.heightFieldData[_yyy * this.numberColumns + _xxx];
+	protected float getHeightAt(final int xxx, final int yyy) {
+		return this.heightFieldData[yyy * this.numberColumns + xxx];
 	}
 	
 	@Override
-	public void getLocalBounds(final Vector3f _min, final Vector3f _max) {
-		_min.set(this.AABB.getMin().multiplyNew(this.scaling));
-		_max.set(this.AABB.getMax().multiplyNew(this.scaling));
+	public Bounds getLocalBounds() {
+		return new Bounds(this.aabb.getMin().multiply(this.scaling), this.aabb.getMax().multiply(this.scaling));
 	}
 	
 	/// Return the number of columns in the height field
@@ -188,31 +187,31 @@ public class HeightFieldShape extends ConcaveShape {
 	/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
 	/// given the start vertex index pointer of the triangle.
 	/*
-	protected		void getTriangleVerticesWithIndexPointer(final long _subPart,
-		                                         final long _triangleIndex,
-		                                         Vector3f* _outTriangleVertices) ;
+	protected		void getTriangleVerticesWithIndexPointer(final long subPart,
+		                                         final long triangleIndex,
+		                                         Vector3f* outTriangleVertices) ;
 		                                         */
 	/// Return the vertex (local-coordinates) of the height field at a given (x,y) position
-	protected Vector3f getVertexAt(final int _xxx, final int _yyy) {
+	protected Vector3f getVertexAt(final int xxx, final int yyy) {
 		// Get the height value
-		final float height = getHeightAt(_xxx, _yyy);
+		final float height = getHeightAt(xxx, yyy);
 		// Height values origin
 		final float heightOrigin = -(this.maxHeight - this.minHeight) * 0.5f - this.minHeight;
 		Vector3f vertex = null;
 		switch (this.upAxis) {
 			case 0:
-				vertex = new Vector3f(heightOrigin + height, -this.width * 0.5f + _xxx, -this.length * 0.5f + _yyy);
+				vertex = new Vector3f(heightOrigin + height, -this.width * 0.5f + xxx, -this.length * 0.5f + yyy);
 				break;
 			case 1:
-				vertex = new Vector3f(-this.width * 0.5f + _xxx, heightOrigin + height, -this.length * 0.5f + _yyy);
+				vertex = new Vector3f(-this.width * 0.5f + xxx, heightOrigin + height, -this.length * 0.5f + yyy);
 				break;
 			case 2:
-				vertex = new Vector3f(-this.width * 0.5f + _xxx, -this.length * 0.5f + _yyy, heightOrigin + height);
+				vertex = new Vector3f(-this.width * 0.5f + xxx, -this.length * 0.5f + yyy, heightOrigin + height);
 				break;
 			default:
 				assert (false);
 		}
-		assert (this.AABB.contains(vertex));
+		assert (this.aabb.contains(vertex));
 		return vertex.multiply(this.scaling);
 	}
 	
@@ -222,35 +221,33 @@ public class HeightFieldShape extends ConcaveShape {
 	}
 	
 	@Override
-	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
 		// TODO : Implement raycasting without using an AABB for the ray
 		//		but using a dynamic AABB tree or octree instead
 		
-		final TriangleOverlapCallback triangleCallback = new TriangleOverlapCallback(_ray, _proxyShape, _raycastInfo, this);
+		final TriangleOverlapCallback triangleCallback = new TriangleOverlapCallback(ray, proxyShape, raycastInfo, this);
 		// Compute the AABB for the ray
-		final Vector3f rayEnd = _ray.point2.lessNew(_ray.point1).multiply(_ray.maxFraction).add(_ray.point1);
+		final Vector3f rayEnd = ray.point2.less(ray.point1).multiply(ray.maxFraction).add(ray.point1);
 		
-		final AABB rayAABB = new AABB(FMath.min(_ray.point1, rayEnd), FMath.max(_ray.point1, rayEnd));
+		final AABB rayAABB = new AABB(FMath.min(ray.point1, rayEnd), FMath.max(ray.point1, rayEnd));
 		testAllTriangles(triangleCallback, rayAABB);
 		return triangleCallback.getIsHit();
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
-		super.setLocalScaling(_scaling);
+	public void setLocalScaling(final Vector3f scaling) {
+		super.setLocalScaling(scaling);
 	}
 	
 	@Override
-	public void testAllTriangles(final TriangleCallback _callback, final AABB _localAABB) {
+	public void testAllTriangles(final TriangleCallback callback, final AABB localAABB) {
 		
 		// Compute the non-scaled AABB
-		final Vector3f inverseScaling = new Vector3f(1.0f / this.scaling.x, 1.0f / this.scaling.y, 1.0f / this.scaling.z);
+		final Vector3f inverseScaling = new Vector3f(1.0f / this.scaling.x(), 1.0f / this.scaling.y(), 1.0f / this.scaling.z());
 		
-		final AABB aabb = new AABB(_localAABB.getMin().multiplyNew(inverseScaling), _localAABB.getMax().multiplyNew(inverseScaling));
+		final AABB aabb = new AABB(localAABB.getMin().multiply(inverseScaling), localAABB.getMax().multiply(inverseScaling));
 		// Compute the longeger grid coordinates inside the area we need to test for collision
-		final Vector3f minGridCoords = new Vector3f();
-		final Vector3f maxGridCoords = new Vector3f();
-		computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb);
+		Bounds bounds = computeMinMaxGridCoordinates(aabb);
 		// Compute the starting and ending coords of the sub-grid according to the up axis
 		int iMin = 0;
 		int iMax = 0;
@@ -258,22 +255,24 @@ public class HeightFieldShape extends ConcaveShape {
 		int jMax = 0;
 		switch (this.upAxis) {
 			case 0:
-				iMin = FMath.clamp((int) minGridCoords.y, 0, this.numberColumns - 1);
-				iMax = FMath.clamp((int) maxGridCoords.y, 0, this.numberColumns - 1);
-				jMin = FMath.clamp((int) minGridCoords.z, 0, this.numberRows - 1);
-				jMax = FMath.clamp((int) maxGridCoords.z, 0, this.numberRows - 1);
+				iMin = FMath.clamp((int) bounds.min().y(), 0, this.numberColumns - 1);
+				iMax = FMath.clamp((int) bounds.max().y(), 0, this.numberColumns - 1);
+				jMin = FMath.clamp((int) bounds.min().z(), 0, this.numberRows - 1);
+				jMax = FMath.clamp((int) bounds.max().z(), 0, this.numberRows - 1);
 				break;
 			case 1:
-				iMin = FMath.clamp((int) minGridCoords.x, 0, this.numberColumns - 1);
-				iMax = FMath.clamp((int) maxGridCoords.x, 0, this.numberColumns - 1);
-				jMin = FMath.clamp((int) minGridCoords.z, 0, this.numberRows - 1);
-				jMax = FMath.clamp((int) maxGridCoords.z, 0, this.numberRows - 1);
+				iMin = FMath.clamp((int) bounds.min().x(), 0, this.numberColumns - 1);
+				iMax = FMath.clamp((int) bounds.max().x(), 0, this.numberColumns - 1);
+				jMin = FMath.clamp((int) bounds.min().z(), 0, this.numberRows - 1);
+				jMax = FMath.clamp((int) bounds.max().z(), 0, this.numberRows - 1);
 				break;
 			case 2:
-				iMin = FMath.clamp((int) minGridCoords.x, 0, this.numberColumns - 1);
-				iMax = FMath.clamp((int) maxGridCoords.x, 0, this.numberColumns - 1);
-				jMin = FMath.clamp((int) minGridCoords.y, 0, this.numberRows - 1);
-				jMax = FMath.clamp((int) maxGridCoords.y, 0, this.numberRows - 1);
+				iMin = FMath.clamp((int) bounds.min().x(), 0, this.numberColumns - 1);
+				iMax = FMath.clamp((int) bounds.max().x(), 0, this.numberColumns - 1);
+				jMin = FMath.clamp((int) bounds.min().y(), 0, this.numberRows - 1);
+				jMax = FMath.clamp((int) bounds.max().y(), 0, this.numberRows - 1);
+				break;
+			default:
 				break;
 		}
 		assert (iMin >= 0 && iMin < this.numberColumns);
@@ -291,13 +290,13 @@ public class HeightFieldShape extends ConcaveShape {
 				// Generate the first triangle for the current grid rectangle
 				final Vector3f[] trianglePoints = { p1, p2, p3 };
 				// Test collision against the first triangle
-				_callback.testTriangle(trianglePoints);
+				callback.testTriangle(trianglePoints);
 				// Generate the second triangle for the current grid rectangle
 				trianglePoints[0] = p3;
 				trianglePoints[1] = p2;
 				trianglePoints[2] = p4;
 				// Test collision against the second triangle
-				_callback.testTriangle(trianglePoints);
+				callback.testTriangle(trianglePoints);
 			}
 		}
 	}

src/org/atriasoft/ephysics/collision/shapes/SphereShape.java

@@ -45,40 +45,33 @@ public class SphereShape extends ConvexShape {
 		 *  Constructor
 		 * @param radius Radius of the sphere (in meters)
 		 */
-	public SphereShape(final float _radius) {
-		super(CollisionShapeType.SPHERE, _radius);
+	public SphereShape(final float radius) {
+		super(CollisionShapeType.SPHERE, radius);
 	}
 	
 	@Override
-	public void computeAABB(final AABB _aabb, final Transform3D _transform) {
+	public void computeAABB(final AABB aabb, final Transform3D transform) {
 		
 		// Get the local extents in x,y and z direction
 		final Vector3f extents = new Vector3f(this.margin, this.margin, this.margin);
 		// Update the AABB with the new minimum and maximum coordinates
-		_aabb.setMin(_transform.getPosition().lessNew(extents));
-		_aabb.setMax(_transform.getPosition().addNew(extents));
+		aabb.setMin(transform.getPosition().less(extents));
+		aabb.setMax(transform.getPosition().add(extents));
 	}
 	
 	@Override
-	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
-		final float diag = 0.4f * _mass * this.margin * this.margin;
-		_tensor.set(diag, 0.0f, 0.0f, 0.0f, diag, 0.0f, 0.0f, 0.0f, diag);
+	public Matrix3f computeLocalInertiaTensor(final float mass) {
+		final float diag = 0.4f * mass * this.margin * this.margin;
+		return new Matrix3f(diag, 0.0f, 0.0f, 0.0f, diag, 0.0f, 0.0f, 0.0f, diag);
 	}
 	
 	@Override
-	public void getLocalBounds(final Vector3f min, final Vector3f max) {
-		// Maximum bounds
-		max.setX(this.margin);
-		max.setY(this.margin);
-		max.setZ(this.margin);
-		// Minimum bounds
-		min.setX(-max.x);
-		min.setY(-max.y);
-		min.setZ(-max.z);
+	public Bounds getLocalBounds() {
+		return new Bounds(new Vector3f(-this.margin, -this.margin, -this.margin), new Vector3f(this.margin, this.margin, this.margin));
 	}
 	
 	@Override
-	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) {
 		return new Vector3f(0.0f, 0.0f, 0.0f);
 	}
 	
@@ -91,14 +84,14 @@ public class SphereShape extends ConvexShape {
 	}
 	
 	@Override
-	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
-		final Vector3f m = _ray.point1;
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
+		final Vector3f m = ray.point1;
 		final float c = m.dot(m) - this.margin * this.margin;
 		// If the origin of the ray is inside the sphere, we return no longersection
 		if (c < 0.0f) {
 			return false;
 		}
-		final Vector3f rayDirection = _ray.point2.lessNew(_ray.point1);
+		final Vector3f rayDirection = ray.point2.less(ray.point1);
 		final float b = m.dot(rayDirection);
 		// If the origin of the ray is outside the sphere and the ray
 		// is pointing away from the sphere, there is no longersection
@@ -116,27 +109,27 @@ public class SphereShape extends ConvexShape {
 		float t = -b - FMath.sqrt(discriminant);
 		assert (t >= 0.0f);
 		// If the hit point is withing the segment ray fraction
-		if (t < _ray.maxFraction * raySquareLength) {
+		if (t < ray.maxFraction * raySquareLength) {
 			// Compute the longersection information
 			t /= raySquareLength;
-			_raycastInfo.body = _proxyShape.getBody();
-			_raycastInfo.proxyShape = _proxyShape;
-			_raycastInfo.hitFraction = t;
-			_raycastInfo.worldPoint = rayDirection.multiplyNew(t).add(_ray.point1);
-			_raycastInfo.worldNormal = _raycastInfo.worldPoint;
+			raycastInfo.body = proxyShape.getBody();
+			raycastInfo.proxyShape = proxyShape;
+			raycastInfo.hitFraction = t;
+			raycastInfo.worldPoint = rayDirection.multiply(t).add(ray.point1);
+			raycastInfo.worldNormal = raycastInfo.worldPoint;
 			return true;
 		}
 		return false;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
-		this.margin = (this.margin / this.scaling.x) * _scaling.x;
-		super.setLocalScaling(_scaling);
+	public void setLocalScaling(final Vector3f scaling) {
+		this.margin = (this.margin / this.scaling.x()) * scaling.x();
+		super.setLocalScaling(scaling);
 	}
 	
 	@Override
-	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
-		return (_localPoint.length2() < this.margin * this.margin);
+	public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) {
+		return (localPoint.length2() < this.margin * this.margin);
 	}
-};
+}

src/org/atriasoft/ephysics/collision/shapes/TriangleShape.java

@@ -1,14 +1,13 @@
 package org.atriasoft.ephysics.collision.shapes;
 
-import org.atriasoft.etk.math.FMath;
-import org.atriasoft.etk.math.Matrix3f;
-import org.atriasoft.etk.math.Transform3D;
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.RaycastInfo;
 import org.atriasoft.ephysics.collision.ProxyShape;
 import org.atriasoft.ephysics.configuration.Defaults;
 import org.atriasoft.ephysics.mathematics.Ray;
+import org.atriasoft.etk.math.FMath;
+import org.atriasoft.etk.math.Matrix3f;
+import org.atriasoft.etk.math.Transform3D;
+import org.atriasoft.etk.math.Vector3f;
 
 /**
  * This class represents a triangle collision shape that is centered
@@ -19,66 +18,66 @@ public class TriangleShape extends ConvexShape {
 	 *  Raycast test side for the triangle
 	 */
 	public enum TriangleRaycastSide {
-		FRONT, //!< Raycast against front triangle
-		BACK, //!< Raycast against back triangle
-		FRONT_AND_BACK //!< Raycast against front and back triangle
-	};
+		BACK, //!< Raycast against front triangle
+		FRONT, //!< Raycast against back triangle
+		FRONTANDBACK //!< Raycast against front and back triangle
+	}
 	
 	protected Vector3f[] points = new Vector3f[3]; //!< Three points of the triangle
 	protected TriangleRaycastSide raycastTestType; //!< Raycast test type for the triangle (front, back, front-back)
 	
-	public TriangleShape(final Vector3f _point1, final Vector3f _point2, final Vector3f _point3) {
+	public TriangleShape(final Vector3f point1, final Vector3f point2, final Vector3f point3) {
 		super(CollisionShapeType.TRIANGLE, Defaults.OBJECT_MARGIN);
-		this.points[0] = _point1;
-		this.points[1] = _point2;
-		this.points[2] = _point3;
+		this.points[0] = point1;
+		this.points[1] = point2;
+		this.points[2] = point3;
 		this.raycastTestType = TriangleRaycastSide.FRONT;
 	}
 	
 	/**
 		 *  Constructor
-		 * @param _point1 First point of the triangle
-		 * @param _point2 Second point of the triangle
-		 * @param _point3 Third point of the triangle
-		 * @param _margin The collision margin (in meters) around the collision shape
+		 * @param point1 First point of the triangle
+		 * @param point2 Second point of the triangle
+		 * @param point3 Third point of the triangle
+		 * @param margin The collision margin (in meters) around the collision shape
 		 */
-	public TriangleShape(final Vector3f _point1, final Vector3f _point2, final Vector3f _point3, final float _margin) {
-		super(CollisionShapeType.TRIANGLE, _margin);
-		this.points[0] = _point1;
-		this.points[1] = _point2;
-		this.points[2] = _point3;
+	public TriangleShape(final Vector3f point1, final Vector3f point2, final Vector3f point3, final float margin) {
+		super(CollisionShapeType.TRIANGLE, margin);
+		this.points[0] = point1;
+		this.points[1] = point2;
+		this.points[2] = point3;
 		this.raycastTestType = TriangleRaycastSide.FRONT;
 	}
 	
 	@Override
-	public void computeAABB(final AABB aabb, final Transform3D _transform) {
-		final Vector3f worldPoint1 = _transform.multiplyNew(this.points[0]);
-		final Vector3f worldPoint2 = _transform.multiplyNew(this.points[1]);
-		final Vector3f worldPoint3 = _transform.multiplyNew(this.points[2]);
-		final Vector3f xAxis = new Vector3f(worldPoint1.x, worldPoint2.x, worldPoint3.x);
-		final Vector3f yAxis = new Vector3f(worldPoint1.y, worldPoint2.y, worldPoint3.y);
-		final Vector3f zAxis = new Vector3f(worldPoint1.z, worldPoint2.z, worldPoint3.z);
+	public void computeAABB(final AABB aabb, final Transform3D transform) {
+		final Vector3f worldPoint1 = transform.multiply(this.points[0]);
+		final Vector3f worldPoint2 = transform.multiply(this.points[1]);
+		final Vector3f worldPoint3 = transform.multiply(this.points[2]);
+		final Vector3f xAxis = new Vector3f(worldPoint1.x(), worldPoint2.x(), worldPoint3.x());
+		final Vector3f yAxis = new Vector3f(worldPoint1.y(), worldPoint2.y(), worldPoint3.y());
+		final Vector3f zAxis = new Vector3f(worldPoint1.z(), worldPoint2.z(), worldPoint3.z());
 		aabb.setMin(new Vector3f(xAxis.getMin(), yAxis.getMin(), zAxis.getMin()));
 		aabb.setMax(new Vector3f(xAxis.getMax(), yAxis.getMax(), zAxis.getMax()));
 	}
 	
 	@Override
-	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
-		_tensor.setZero();
+	public Matrix3f computeLocalInertiaTensor(final float mass) {
+		return Matrix3f.ZERO;
 	}
 	
 	@Override
-	public void getLocalBounds(final Vector3f min, final Vector3f max) {
-		final Vector3f xAxis = new Vector3f(this.points[0].x, this.points[1].x, this.points[2].x);
-		final Vector3f yAxis = new Vector3f(this.points[0].y, this.points[1].y, this.points[2].y);
-		final Vector3f zAxis = new Vector3f(this.points[0].z, this.points[1].z, this.points[2].z);
-		min.setValue(xAxis.getMin() - this.margin, yAxis.getMin() - this.margin, zAxis.getMin() - this.margin);
-		max.setValue(xAxis.getMax() + this.margin, yAxis.getMax() + this.margin, zAxis.getMax() + this.margin);
+	public Bounds getLocalBounds() {
+		final Vector3f xAxis = new Vector3f(this.points[0].x(), this.points[1].x(), this.points[2].x());
+		final Vector3f yAxis = new Vector3f(this.points[0].y(), this.points[1].y(), this.points[2].y());
+		final Vector3f zAxis = new Vector3f(this.points[0].z(), this.points[1].z(), this.points[2].z());
+		return new Bounds(new Vector3f(xAxis.getMin() - this.margin, yAxis.getMin() - this.margin, zAxis.getMin() - this.margin),
+				new Vector3f(xAxis.getMax() + this.margin, yAxis.getMax() + this.margin, zAxis.getMax() + this.margin));
 	}
 	
 	@Override
-	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
-		final Vector3f dotProducts = new Vector3f(_direction.dot(this.points[0]), _direction.dot(this.points[1]), _direction.dot(this.points[2]));
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) {
+		final Vector3f dotProducts = new Vector3f(direction.dot(this.points[0]), direction.dot(this.points[1]), direction.dot(this.points[2]));
 		return this.points[dotProducts.getMaxAxis()];
 	}
 	
@@ -89,19 +88,19 @@ public class TriangleShape extends ConvexShape {
 	
 	/**
 	 *  Return the coordinates of a given vertex of the triangle
-	 * @param _index Index (0 to 2) of a vertex of the triangle
+	 * @param index Index (0 to 2) of a vertex of the triangle
 	 */
-	public Vector3f getVertex(final int _index) {
-		assert (_index >= 0 && _index < 3);
-		return this.points[_index];
+	public Vector3f getVertex(final int index) {
+		assert (index >= 0 && index < 3);
+		return this.points[index];
 	}
 	
 	@Override
-	public boolean raycast(final Ray _ray, final RaycastInfo _raycastInfo, final ProxyShape _proxyShape) {
-		final Vector3f pq = _ray.point2.lessNew(_ray.point1);
-		final Vector3f pa = this.points[0].lessNew(_ray.point1);
-		final Vector3f pb = this.points[1].lessNew(_ray.point1);
-		final Vector3f pc = this.points[2].lessNew(_ray.point1);
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
+		final Vector3f pq = ray.point2.less(ray.point1);
+		final Vector3f pa = this.points[0].less(ray.point1);
+		final Vector3f pb = this.points[1].less(ray.point1);
+		final Vector3f pc = this.points[2].less(ray.point1);
 		// Test if the line PQ is inside the eges BC, CA and AB. We use the triple
 		// product for this test.
 		final Vector3f m = pq.cross(pc);
@@ -124,7 +123,7 @@ public class TriangleShape extends ConvexShape {
 			if (v > 0.0f) {
 				return false;
 			}
-		} else if (this.raycastTestType == TriangleRaycastSide.FRONT_AND_BACK) {
+		} else if (this.raycastTestType == TriangleRaycastSide.FRONTANDBACK) {
 			if (!FMath.sameSign(u, v)) {
 				return false;
 			}
@@ -138,7 +137,7 @@ public class TriangleShape extends ConvexShape {
 			if (w > 0.0f) {
 				return false;
 			}
-		} else if (this.raycastTestType == TriangleRaycastSide.FRONT_AND_BACK) {
+		} else if (this.raycastTestType == TriangleRaycastSide.FRONTANDBACK) {
 			if (!FMath.sameSign(u, w)) {
 				return false;
 			}
@@ -154,42 +153,42 @@ public class TriangleShape extends ConvexShape {
 		v *= denom;
 		w *= denom;
 		// Compute the local hit point using the barycentric coordinates
-		final Vector3f localHitPoint = this.points[0].multiplyNew(u).add(this.points[1].multiplyNew(v)).add(this.points[2].multiplyNew(w));
-		final float hitFraction = localHitPoint.lessNew(_ray.point1).length() / pq.length();
-		if (hitFraction < 0.0f || hitFraction > _ray.maxFraction) {
+		final Vector3f localHitPoint = this.points[0].multiply(u).add(this.points[1].multiply(v)).add(this.points[2].multiply(w));
+		final float hitFraction = localHitPoint.less(ray.point1).length() / pq.length();
+		if (hitFraction < 0.0f || hitFraction > ray.maxFraction) {
 			return false;
 		}
-		final Vector3f localHitNormal = this.points[1].lessNew(this.points[0]).cross(this.points[2].lessNew(this.points[0]));
+		Vector3f localHitNormal = this.points[1].less(this.points[0]).cross(this.points[2].less(this.points[0]));
 		if (localHitNormal.dot(pq) > 0.0f) {
-			localHitNormal.less(localHitNormal);
+			localHitNormal = localHitNormal.less(localHitNormal);
 		}
-		_raycastInfo.body = _proxyShape.getBody();
-		_raycastInfo.proxyShape = _proxyShape;
-		_raycastInfo.worldPoint = localHitPoint;
-		_raycastInfo.hitFraction = hitFraction;
-		_raycastInfo.worldNormal = localHitNormal;
+		raycastInfo.body = proxyShape.getBody();
+		raycastInfo.proxyShape = proxyShape;
+		raycastInfo.worldPoint = localHitPoint;
+		raycastInfo.hitFraction = hitFraction;
+		raycastInfo.worldNormal = localHitNormal;
 		return true;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
-		this.points[0].divide(this.scaling).multiply(_scaling);
-		this.points[1].divide(this.scaling).multiply(_scaling);
-		this.points[2].divide(this.scaling).multiply(_scaling);
-		super.setLocalScaling(_scaling);
+	public void setLocalScaling(final Vector3f scaling) {
+		this.points[0] = this.points[0].divide(this.scaling).multiply(scaling);
+		this.points[1] = this.points[1].divide(this.scaling).multiply(scaling);
+		this.points[2] = this.points[2].divide(this.scaling).multiply(scaling);
+		super.setLocalScaling(scaling);
 	}
 	
 	/**
 	 *  Set the raycast test type (front, back, front-back)
-	 * @param _testType Raycast test type for the triangle (front, back, front-back)
+	 * @param testType Raycast test type for the triangle (front, back, front-back)
 	 */
-	public void setRaycastTestType(final TriangleRaycastSide _testType) {
-		this.raycastTestType = _testType;
+	public void setRaycastTestType(final TriangleRaycastSide testType) {
+		this.raycastTestType = testType;
 	}
 	
 	@Override
-	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+	public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) {
 		return false;
 	}
 	
-};
+}

src/org/atriasoft/ephysics/configuration/Defaults.java

@@ -26,29 +26,45 @@ package org.atriasoft.ephysics.configuration;
 
 import org.atriasoft.etk.math.Constant;
 
-/**
- *
- * @author Jason Sorensen <sorensenj@smert.net>
- */
 public class Defaults {
-	
-	// Smallest decimal value (negative)
-	public static final float DECIMAL_SMALLEST = Float.MIN_VALUE;
-	
 	// Maximum decimal value
 	public static final float DECIMAL_LARGEST = Float.MAX_VALUE;
-	
-	// Default internal constant timestep in seconds
-	public static final float DEFAULT_TIMESTEP = 1.0f / 60.0f;
-	
-	// Default friction coefficient for a rigid body
-	public static final float DEFAULT_FRICTION_COEFFICIENT = 0.3f;
+	// Smallest decimal value (negative)
+	public static final float DECIMAL_SMALLEST = -Float.MAX_VALUE;
 	
 	// Default bounciness factor for a rigid body
 	public static final float DEFAULT_BOUNCINESS = 0.5f;
 	
-	// True if the spleeping technique is enabled
-	public static final boolean SPLEEPING_ENABLED = true;
+	// Default friction coefficient for a rigid body
+	public static final float DEFAULT_FRICTION_COEFFICIENT = 0.3f;
+	
+	// Number of iterations when solving the position constraints of the Sequential Impulse technique
+	public static final int DEFAULT_POSITION_SOLVER_NUM_ITERATIONS = 5;
+	
+	// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
+	// might enter sleeping mode
+	public static final float DEFAULT_SLEEP_ANGULAR_VELOCITY = 3.0f * (Constant.PI / 180.0f);
+	
+	// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
+	// might enter sleeping mode.
+	public static final float DEFAULT_SLEEP_LINEAR_VELOCITY = 0.02f;
+	
+	// Time (in seconds) that a body must stay still to be considered sleeping
+	public static final float DEFAULT_TIME_BEFORE_SLEEP = 1.0f;
+	
+	// Default internal constant timestep in seconds
+	public static final float DEFAULT_TIMESTEP = 1.0f / 60.0f;
+	
+	// Number of iterations when solving the velocity constraints of the Sequential Impulse technique
+	public static final int DEFAULT_VELOCITY_SOLVER_NUM_ITERATIONS = 10;
+	
+	/// Maximum number of contact manifolds in an overlapping pair that involves at
+	/// least one concave collision shape.
+	public static final int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
+	
+	/// Maximum number of contact manifolds in an overlapping pair that involves two
+	/// convex collision shapes.
+	public static final int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
 	
 	// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
 	public static final float OBJECT_MARGIN = 0.04f;
@@ -59,28 +75,8 @@ public class Defaults {
 	// Velocity threshold for contact velocity restitution
 	public static final float RESTITUTION_VELOCITY_THRESHOLD = 1.0f;
 	
-	// Number of iterations when solving the velocity constraints of the Sequential Impulse technique
-	public static final int DEFAULT_VELOCITY_SOLVER_NUM_ITERATIONS = 10;
+	// True if the spleeping technique is enabled
+	public static final boolean SPLEEPING_ENABLED = true;
 	
-	// Number of iterations when solving the position constraints of the Sequential Impulse technique
-	public static final int DEFAULT_POSITION_SOLVER_NUM_ITERATIONS = 5;
-	
-	// Time (in seconds) that a body must stay still to be considered sleeping
-	public static final float DEFAULT_TIME_BEFORE_SLEEP = 1.0f;
-	
-	// A body with a linear velocity smaller than the sleep linear velocity (in m/s)
-	// might enter sleeping mode.
-	public static final float DEFAULT_SLEEP_LINEAR_VELOCITY = 0.02f;
-	
-	// A body with angular velocity smaller than the sleep angular velocity (in rad/s)
-	// might enter sleeping mode
-	public static final float DEFAULT_SLEEP_ANGULAR_VELOCITY = 3.0f * (Constant.PI / 180.0f);
-	
-	/// Maximum number of contact manifolds in an overlapping pair that involves two
-	/// convex collision shapes.
-	public static final int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
-	
-	/// Maximum number of contact manifolds in an overlapping pair that involves at
-	/// least one concave collision shape.
-	public static final int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
+	private Defaults() {}
 }

src/org/atriasoft/ephysics/constraint/BallAndSocketJoint.java

@@ -1,25 +1,23 @@
 package org.atriasoft.ephysics.constraint;
 
+import org.atriasoft.ephysics.body.BodyType;
+import org.atriasoft.ephysics.engine.ConstraintSolverData;
 import org.atriasoft.etk.math.Matrix3f;
 import org.atriasoft.etk.math.Quaternion;
 import org.atriasoft.etk.math.Vector3f;
 
-import org.atriasoft.ephysics.body.BodyType;
-import org.atriasoft.ephysics.engine.ConstraintSolverData;
-
 public class BallAndSocketJoint extends Joint {
 	
-	private static float BETA = 0.2f; //!< Beta value for the bias factor of position correction
-	private Vector3f localAnchorPointBody1 = new Vector3f(0, 0, 0); //!< Anchor point of body 1 (in local-space coordinates of body 1)
-	private Vector3f localAnchorPointBody2 = new Vector3f(0, 0, 0); //!< Anchor point of body 2 (in local-space coordinates of body 2)
-	private Vector3f r1World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space
-	private Vector3f r2World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space
-	private Matrix3f i1 = new Matrix3f(); //!< Inertia tensor of body 1 (in world-space coordinates)
-	private Matrix3f i2 = new Matrix3f(); //!< Inertia tensor of body 2 (in world-space coordinates)
-	private Vector3f biasVector = new Vector3f(0, 0, 0); //!< Bias vector for the raint
-	private Matrix3f inverseMassMatrix = new Matrix3f(); //!< Inverse mass matrix K=JM^-1J^-t of the raint
-	private final Vector3f impulse = new Vector3f(0, 0, 0); //!< Accumulated impulse
-	/// Constructor
+	private static final float BETA = 0.2f; //!< Beta value for the bias factor of position correction
+	private Vector3f biasVector = Vector3f.ZERO; //!< Bias vector for the raint
+	private Matrix3f i1 = Matrix3f.IDENTITY; //!< Inertia tensor of body 1 (in world-space coordinates)
+	private Matrix3f i2 = Matrix3f.IDENTITY; //!< Inertia tensor of body 2 (in world-space coordinates)
+	private Vector3f impulse = Vector3f.ZERO; //!< Accumulated impulse
+	private Matrix3f inverseMassMatrix = Matrix3f.IDENTITY; //!< Inverse mass matrix K=JM^-1J^-t of the raint
+	private Vector3f localAnchorPointBody1 = Vector3f.ZERO; //!< Anchor point of body 1 (in local-space coordinates of body 1)
+	private Vector3f localAnchorPointBody2 = Vector3f.ZERO; //!< Anchor point of body 2 (in local-space coordinates of body 2)
+	private Vector3f r1World = Vector3f.ZERO; //!< Vector from center of body 2 to anchor point in world-space
+	private Vector3f r2World = Vector3f.ZERO; //!< Vector from center of body 2 to anchor point in world-space
 	
 	public BallAndSocketJoint(final BallAndSocketJointInfo jointInfo) {
 		super(jointInfo);
@@ -55,28 +53,28 @@ public class BallAndSocketJoint extends Joint {
 		
 		// Compute the matrix K=JM^-1J^t (3x3 matrix)
 		final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse;
-		final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
-		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew()));
-		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew()));
+		Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
+		massMatrix = massMatrix.add(skewSymmetricMatrixU1.multiply(this.i1).multiply(skewSymmetricMatrixU1.transpose()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU2.multiply(this.i2).multiply(skewSymmetricMatrixU2.transpose()));
 		
 		// Compute the inverse mass matrix K^-1
-		this.inverseMassMatrix.setZero();
+		this.inverseMassMatrix = Matrix3f.ZERO;
 		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
-			this.inverseMassMatrix = massMatrix.inverseNew();
+			this.inverseMassMatrix = massMatrix.inverse();
 		}
 		
 		// Compute the bias "b" of the raint
-		this.biasVector.setZero();
+		this.biasVector = Vector3f.ZERO;
 		if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
-			final float biasFactor = (BETA / raintSolverData.timeStep);
-			this.biasVector = (x2.addNew(this.r2World).less(x1).less(this.r1World)).multiply(biasFactor);
+			final float biasFactor = (BallAndSocketJoint.BETA / raintSolverData.timeStep);
+			this.biasVector = (x2.add(this.r2World).less(x1).less(this.r1World)).multiply(biasFactor);
 		}
 		
 		// If warm-starting is not enabled
 		if (!raintSolverData.isWarmStartingActive) {
 			
 			// Reset the accumulated impulse
-			this.impulse.setZero();
+			this.impulse = Vector3f.ZERO;
 		}
 	}
 	
@@ -84,32 +82,32 @@ public class BallAndSocketJoint extends Joint {
 	public void solvePositionConstraint(final ConstraintSolverData raintSolverData) {
 		
 		// Get the velocities
-		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
 		
 		// Compute J*v
-		final Vector3f Jv = v2.addNew(w2.cross(this.r2World)).less(v1).less(w1.cross(this.r1World));
+		final Vector3f jv = v2.add(w2.cross(this.r2World)).less(v1).less(w1.cross(this.r1World));
 		
 		// Compute the Lagrange multiplier lambda
-		final Vector3f deltaLambda = this.inverseMassMatrix.multiplyNew(Jv.multiplyNew(-1).less(this.biasVector));
-		this.impulse.add(deltaLambda);
+		final Vector3f deltaLambda = this.inverseMassMatrix.multiply(jv.multiply(-1).less(this.biasVector));
+		this.impulse = this.impulse.add(deltaLambda);
 		
 		// Compute the impulse P=J^T * lambda for the body 1
-		final Vector3f linearImpulseBody1 = deltaLambda.multiplyNew(-1);
+		final Vector3f linearImpulseBody1 = deltaLambda.multiply(-1);
 		final Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World);
 		
 		// Apply the impulse to the body 1
-		v1.add(linearImpulseBody1.multiplyNew(this.body1.massInverse));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		v1 = v1.add(linearImpulseBody1.multiply(this.body1.massInverse));
+		w1 = w1.add(this.i1.multiply(angularImpulseBody1));
 		
 		// Compute the impulse P=J^T * lambda for the body 2
-		final Vector3f angularImpulseBody2 = deltaLambda.cross(this.r2World).multiplyNew(-1);
+		final Vector3f angularImpulseBody2 = deltaLambda.cross(this.r2World).multiply(-1);
 		
 		// Apply the impulse to the body 2
-		v2.add(deltaLambda.multiplyNew(this.body2.massInverse));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		v2 = v2.add(deltaLambda.multiply(this.body2.massInverse));
+		w2 = w2.add(this.i2.multiply(angularImpulseBody2));
 	}
 	
 	@Override
@@ -122,10 +120,10 @@ public class BallAndSocketJoint extends Joint {
 		}
 		
 		// Get the bodies center of mass and orientations
-		final Vector3f x1 = raintSolverData.positions[this.indexBody1];
-		final Vector3f x2 = raintSolverData.positions[this.indexBody2];
-		final Quaternion q1 = raintSolverData.orientations[this.indexBody1];
-		final Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+		Vector3f x1 = raintSolverData.positions[this.indexBody1];
+		Vector3f x2 = raintSolverData.positions[this.indexBody2];
+		Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+		Quaternion q2 = raintSolverData.orientations[this.indexBody2];
 		
 		// Get the inverse mass and inverse inertia tensors of the bodies
 		final float inverseMassBody1 = this.body1.massInverse;
@@ -145,71 +143,71 @@ public class BallAndSocketJoint extends Joint {
 		
 		// Recompute the inverse mass matrix K=J^TM^-1J of of the 3 translation raints
 		final float inverseMassBodies = inverseMassBody1 + inverseMassBody2;
-		final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
-		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiplyNew(skewSymmetricMatrixU1.transposeNew()));
-		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiplyNew(skewSymmetricMatrixU2.transposeNew()));
-		this.inverseMassMatrix.setZero();
+		Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
+		massMatrix = massMatrix.add(skewSymmetricMatrixU1.multiply(this.i1).multiply(skewSymmetricMatrixU1.transpose()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU2.multiply(this.i2).multiply(skewSymmetricMatrixU2.transpose()));
+		this.inverseMassMatrix = Matrix3f.ZERO;
 		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
-			this.inverseMassMatrix = massMatrix.inverseNew();
+			this.inverseMassMatrix = massMatrix.inverse();
 		}
 		
 		// Compute the raint error (value of the C(x) function)
-		final Vector3f raintError = x2.addNew(this.r2World).less(x1).less(this.r1World);
+		final Vector3f raintError = x2.add(this.r2World).less(x1).less(this.r1World);
 		
 		// Compute the Lagrange multiplier lambda
 		// TODO : Do not solve the system by computing the inverse each time and multiplying with the
 		//		right-hand side vector but instead use a method to directly solve the linear system.
-		final Vector3f lambda = this.inverseMassMatrix.multiplyNew(raintError.multiplyNew(-1));
+		final Vector3f lambda = this.inverseMassMatrix.multiply(raintError.multiply(-1));
 		
 		// Compute the impulse of body 1
-		final Vector3f linearImpulseBody1 = lambda.multiplyNew(-1);
+		final Vector3f linearImpulseBody1 = lambda.multiply(-1);
 		final Vector3f angularImpulseBody1 = lambda.cross(this.r1World);
 		
 		// Compute the pseudo velocity of body 1
-		final Vector3f v1 = linearImpulseBody1.multiplyNew(inverseMassBody1);
-		final Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1);
+		final Vector3f v1 = linearImpulseBody1.multiply(inverseMassBody1);
+		final Vector3f w1 = this.i1.multiply(angularImpulseBody1);
 		
 		// Update the body center of mass and orientation of body 1
-		x1.add(v1);
-		q1.add((new Quaternion(0, w1)).multiplyNew(q1).multiplyNew(0.5f));
-		q1.normalize();
+		x1 = x1.add(v1);
+		q1 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1 = q1.normalize();
 		
 		// Compute the impulse of body 2
-		final Vector3f angularImpulseBody2 = lambda.cross(this.r2World).multiplyNew(-1);
+		final Vector3f angularImpulseBody2 = lambda.cross(this.r2World).multiply(-1);
 		
 		// Compute the pseudo velocity of body 2
-		final Vector3f v2 = lambda.multiplyNew(inverseMassBody2);
-		final Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2);
+		final Vector3f v2 = lambda.multiply(inverseMassBody2);
+		final Vector3f w2 = this.i2.multiply(angularImpulseBody2);
 		
 		// Update the body position/orientation of body 2
-		x2.add(v2);
-		q2.add((new Quaternion(0, w2)).multiplyNew(q2).multiply(0.5f));
-		q2.normalize();
+		x2 = x2.add(v2);
+		q2 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
+		q2 = q2.normalize();
 	}
 	
 	@Override
 	public void warmstart(final ConstraintSolverData raintSolverData) {
 		
 		// Get the velocities
-		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
 		
 		// Compute the impulse P=J^T * lambda for the body 1
-		final Vector3f linearImpulseBody1 = this.impulse.multiplyNew(-1);
+		final Vector3f linearImpulseBody1 = this.impulse.multiply(-1);
 		final Vector3f angularImpulseBody1 = this.impulse.cross(this.r1World);
 		
 		// Apply the impulse to the body 1
-		v1.add(linearImpulseBody1.multiplyNew(this.body1.massInverse));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		v1 = v1.add(linearImpulseBody1.multiply(this.body1.massInverse));
+		w1 = w1.add(this.i1.multiply(angularImpulseBody1));
 		
 		// Compute the impulse P=J^T * lambda for the body 2
-		final Vector3f angularImpulseBody2 = this.impulse.cross(this.r2World).multiplyNew(-1);
+		final Vector3f angularImpulseBody2 = this.impulse.cross(this.r2World).multiply(-1);
 		
 		// Apply the impulse to the body to the body 2
-		v2.add(this.impulse.multiplyNew(this.body2.massInverse));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		v2 = v2.add(this.impulse.multiply(this.body2.massInverse));
+		w2 = w2.add(this.i2.multiply(angularImpulseBody2));
 	}
 	
 }

src/org/atriasoft/ephysics/constraint/BallAndSocketJointInfo.java

@@ -1,8 +1,7 @@
 package org.atriasoft.ephysics.constraint;
 
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.body.RigidBody;
+import org.atriasoft.etk.math.Vector3f;
 
 /**
  *  It is used to gather the information needed to create a ball-and-socket
@@ -14,13 +13,13 @@ public class BallAndSocketJointInfo extends JointInfo {
 	
 	/**
 	 * Constructor
-	 * @param _rigidBody1 Pointer to the first body of the joint
-	 * @param _rigidBody2 Pointer to the second body of the joint
-	 * @param _initAnchorPointWorldSpace The anchor point in world-space coordinates
+	 * @param rigidBody1 Pointer to the first body of the joint
+	 * @param rigidBody2 Pointer to the second body of the joint
+	 * @param initAnchorPointWorldSpace The anchor point in world-space coordinates
 	 */
-	public BallAndSocketJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace) {
-		super(_rigidBody1, _rigidBody2, JointType.BALLSOCKETJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+	public BallAndSocketJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace) {
+		super(rigidBody1, rigidBody2, JointType.BALLSOCKETJOINT);
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
 		
 	}
 }

src/org/atriasoft/ephysics/constraint/ContactPoint.java

@@ -1,8 +1,7 @@
 package org.atriasoft.ephysics.constraint;
 
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.body.CollisionBody;
+import org.atriasoft.etk.math.Vector3f;
 
 /**
  *  This class represents a collision contact point between two
@@ -12,35 +11,35 @@ public class ContactPoint {
 	
 	private final CollisionBody body1; //!< First rigid body of the contact
 	private final CollisionBody body2; //!< Second rigid body of the contact
-	private final Vector3f normal; //!< Normalized normal vector of the contact (from body1 toward body2) in world space
+	private float frictionImpulse1; //!< Cached first friction impulse
 	
-	private float penetrationDepth; //!< Penetration depth
+	private float frictionImpulse2; //!< Cached second friction impulse
+	private Vector3f frictionVectors1; //!< Two orthogonal vectors that span the tangential friction plane
+	private Vector3f frictionVectors2; //!< Two orthogonal vectors that span the tangential friction plane
+	private boolean isRestingContact; //!< True if the contact is a resting contact (exists for more than one time step)
 	private final Vector3f localPointOnBody1; //!< Contact point on body 1 in local space of body 1
 	private final Vector3f localPointOnBody2; //!< Contact point on body 2 in local space of body 2
-	private Vector3f worldPointOnBody1; //!< Contact point on body 1 in world space
-	private Vector3f worldPointOnBody2; //!< Contact point on body 2 in world space
-	private boolean isRestingContact; //!< True if the contact is a resting contact (exists for more than one time step)
-	private Vector3f frictionVectors_1; //!< Two orthogonal vectors that span the tangential friction plane
-	private Vector3f frictionVectors_2; //!< Two orthogonal vectors that span the tangential friction plane
+	private final Vector3f normal; //!< Normalized normal vector of the contact (from body1 toward body2) in world space
+	private float penetrationDepth; //!< Penetration depth
 	private float penetrationImpulse; //!< Cached penetration impulse
-	private float frictionImpulse1; //!< Cached first friction impulse
-	private float frictionImpulse2; //!< Cached second friction impulse
 	private Vector3f rollingResistanceImpulse; //!< Cached rolling resistance impulse
 	/// Constructor
+	private Vector3f worldPointOnBody1; //!< Contact point on body 1 in world space
+	private Vector3f worldPointOnBody2; //!< Contact point on body 2 in world space
 	
 	public ContactPoint(final ContactPointInfo contactInfo) {
 		this.body1 = contactInfo.shape1.getBody();
 		this.body2 = contactInfo.shape2.getBody();
-		this.normal = contactInfo.normal.clone();
+		this.normal = contactInfo.normal;
 		this.penetrationDepth = contactInfo.penetrationDepth;
-		this.localPointOnBody1 = contactInfo.localPoint1.clone();
-		this.localPointOnBody2 = contactInfo.localPoint2.clone();
-		this.worldPointOnBody1 = contactInfo.shape1.getBody().getTransform().multiplyNew(contactInfo.shape1.getLocalToBodyTransform().multiply(contactInfo.localPoint1));
-		this.worldPointOnBody2 = contactInfo.shape2.getBody().getTransform().multiplyNew(contactInfo.shape2.getLocalToBodyTransform().multiply(contactInfo.localPoint2));
+		this.localPointOnBody1 = contactInfo.localPoint1;
+		this.localPointOnBody2 = contactInfo.localPoint2;
+		this.worldPointOnBody1 = contactInfo.shape1.getBody().getTransform().multiply(contactInfo.shape1.getLocalToBodyTransform().multiply(contactInfo.localPoint1));
+		this.worldPointOnBody2 = contactInfo.shape2.getBody().getTransform().multiply(contactInfo.shape2.getLocalToBodyTransform().multiply(contactInfo.localPoint2));
 		this.isRestingContact = false;
 		
-		this.frictionVectors_1 = new Vector3f(0, 0, 0);
-		this.frictionVectors_2 = new Vector3f(0, 0, 0);
+		this.frictionVectors1 = new Vector3f(0, 0, 0);
+		this.frictionVectors2 = new Vector3f(0, 0, 0);
 		
 		assert (this.penetrationDepth >= 0.0f);
 		
@@ -68,12 +67,12 @@ public class ContactPoint {
 	
 	/// Get the second friction vector
 	public Vector3f getFrictionvec2() {
-		return this.frictionVectors_2;
+		return this.frictionVectors2;
 	}
 	
 	/// Get the first friction vector
 	public Vector3f getFrictionVector1() {
-		return this.frictionVectors_1;
+		return this.frictionVectors1;
 	}
 	
 	/// Return true if the contact is a resting contact
@@ -133,12 +132,12 @@ public class ContactPoint {
 	
 	/// Set the second friction vector
 	public void setFrictionvec2(final Vector3f frictionvec2) {
-		this.frictionVectors_2 = frictionvec2;
+		this.frictionVectors2 = frictionvec2;
 	}
 	
 	/// Set the first friction vector
 	public void setFrictionVector1(final Vector3f frictionVector1) {
-		this.frictionVectors_1 = frictionVector1;
+		this.frictionVectors1 = frictionVector1;
 	}
 	
 	/// Set the this.isRestingContact variable

src/org/atriasoft/ephysics/constraint/ContactPointInfo.java

@@ -1,9 +1,8 @@
 package org.atriasoft.ephysics.constraint;
 
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.collision.ProxyShape;
 import org.atriasoft.ephysics.collision.shapes.CollisionShape;
+import org.atriasoft.etk.math.Vector3f;
 
 /**
  *  This structure contains informations about a collision contact
@@ -12,14 +11,14 @@ import org.atriasoft.ephysics.collision.shapes.CollisionShape;
  * between two bodies.
  */
 public class ContactPointInfo {
-	public ProxyShape shape1; //!< First proxy shape of the contact
-	public ProxyShape shape2; //!< Second proxy shape of the contact
 	public CollisionShape collisionShape1; //!< First collision shape
 	public CollisionShape collisionShape2; //!< Second collision shape
-	public Vector3f normal; //!< Normalized normal vector of the collision contact in world space
-	public float penetrationDepth; //!< Penetration depth of the contact
 	public Vector3f localPoint1; //!< Contact point of body 1 in local space of body 1
 	public Vector3f localPoint2; //!< Contact point of body 2 in local space of body 2
+	public Vector3f normal; //!< Normalized normal vector of the collision contact in world space
+	public float penetrationDepth; //!< Penetration depth of the contact
+	public ProxyShape shape1; //!< First proxy shape of the contact
+	public ProxyShape shape2; //!< Second proxy shape of the contact
 	
 	public ContactPointInfo() {
 		this.shape1 = null;
@@ -39,16 +38,16 @@ public class ContactPointInfo {
 		this.localPoint2 = obj.localPoint2;
 	}
 	
-	public ContactPointInfo(final ProxyShape _proxyShape1, final ProxyShape _proxyShape2, final CollisionShape _collShape1, final CollisionShape _collShape2, final Vector3f _normal,
-			final float _penetrationDepth, final Vector3f _localPoint1, final Vector3f _localPoint2) {
-		this.shape1 = _proxyShape1;
-		this.shape2 = _proxyShape2;
-		this.collisionShape1 = _collShape1;
-		this.collisionShape2 = _collShape2;
-		this.normal = _normal;
-		this.penetrationDepth = _penetrationDepth;
-		this.localPoint1 = _localPoint1;
-		this.localPoint2 = _localPoint2;
+	public ContactPointInfo(final ProxyShape proxyShape1, final ProxyShape proxyShape2, final CollisionShape collShape1, final CollisionShape collShape2, final Vector3f normal,
+			final float penetrationDepth, final Vector3f localPoint1, final Vector3f localPoint2) {
+		this.shape1 = proxyShape1;
+		this.shape2 = proxyShape2;
+		this.collisionShape1 = collShape1;
+		this.collisionShape2 = collShape2;
+		this.normal = normal;
+		this.penetrationDepth = penetrationDepth;
+		this.localPoint1 = localPoint1;
+		this.localPoint2 = localPoint2;
 		
 	}
-};
+}

src/org/atriasoft/ephysics/constraint/FixedJoint.java

@@ -1,29 +1,28 @@
 package org.atriasoft.ephysics.constraint;
 
+import org.atriasoft.ephysics.body.BodyType;
+import org.atriasoft.ephysics.engine.ConstraintSolverData;
 import org.atriasoft.etk.math.Matrix3f;
 import org.atriasoft.etk.math.Quaternion;
 import org.atriasoft.etk.math.Transform3D;
 import org.atriasoft.etk.math.Vector3f;
 
-import org.atriasoft.ephysics.body.BodyType;
-import org.atriasoft.ephysics.engine.ConstraintSolverData;
-
 public class FixedJoint extends Joint {
 	
-	protected static float BETA = 0.2f; //!< Beta value for the bias factor of position correction
-	protected Vector3f localAnchorPointBody1 = new Vector3f(0, 0, 0); //!< Anchor point of body 1 (in local-space coordinates of body 1)
-	protected Vector3f localAnchorPointBody2 = new Vector3f(0, 0, 0); //!< Anchor point of body 2 (in local-space coordinates of body 2)
-	protected Vector3f r1World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space
-	protected Vector3f r2World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space
-	protected Matrix3f i1 = new Matrix3f(); //!< Inertia tensor of body 1 (in world-space coordinates)
-	protected Matrix3f i2 = new Matrix3f(); //!< Inertia tensor of body 2 (in world-space coordinates)
-	protected Vector3f impulseTranslation = new Vector3f(0, 0, 0); //!< Accumulated impulse for the 3 translation raints
-	protected Vector3f impulseRotation = new Vector3f(0, 0, 0); //!< Accumulate impulse for the 3 rotation raints
-	protected Matrix3f inverseMassMatrixTranslation = new Matrix3f(); //!< Inverse mass matrix K=JM^-1J^-t of the 3 translation raints (3x3 matrix)
-	protected Matrix3f inverseMassMatrixRotation = new Matrix3f(); //!< Inverse mass matrix K=JM^-1J^-t of the 3 rotation raints (3x3 matrix)
-	protected Vector3f biasTranslation = new Vector3f(0, 0, 0); //!< Bias vector for the 3 translation raints
-	protected Vector3f biasRotation = new Vector3f(0, 0, 0); //!< Bias vector for the 3 rotation raints
-	protected Quaternion initOrientationDifferenceInv = new Quaternion(); //!< Inverse of the initial orientation difference between the two bodies
+	protected static final float BETA = 0.2f; //!< Beta value for the bias factor of position correction
+	protected Vector3f biasRotation = Vector3f.ZERO; //!< Anchor point of body 1 (in local-space coordinates of body 1)
+	protected Vector3f biasTranslation = Vector3f.ZERO; //!< Anchor point of body 2 (in local-space coordinates of body 2)
+	protected Matrix3f i1 = Matrix3f.IDENTITY; //!< Vector from center of body 2 to anchor point in world-space
+	protected Matrix3f i2 = Matrix3f.IDENTITY; //!< Vector from center of body 2 to anchor point in world-space
+	protected Vector3f impulseRotation = Vector3f.ZERO; //!< Inertia tensor of body 1 (in world-space coordinates)
+	protected Vector3f impulseTranslation = Vector3f.ZERO; //!< Inertia tensor of body 2 (in world-space coordinates)
+	protected Quaternion initOrientationDifferenceInv = Quaternion.IDENTITY; //!< Accumulated impulse for the 3 translation raints
+	protected Matrix3f inverseMassMatrixRotation = Matrix3f.IDENTITY; //!< Accumulate impulse for the 3 rotation raints
+	protected Matrix3f inverseMassMatrixTranslation = Matrix3f.IDENTITY; //!< Inverse mass matrix K=JM^-1J^-t of the 3 translation raints (3x3 matrix)
+	protected Vector3f localAnchorPointBody1 = Vector3f.ZERO; //!< Inverse mass matrix K=JM^-1J^-t of the 3 rotation raints (3x3 matrix)
+	protected Vector3f localAnchorPointBody2 = Vector3f.ZERO; //!< Bias vector for the 3 translation raints
+	protected Vector3f r1World = Vector3f.ZERO; //!< Bias vector for the 3 rotation raints
+	protected Vector3f r2World = Vector3f.ZERO; //!< Inverse of the initial orientation difference between the two bodies
 	
 	/// Constructor
 	public FixedJoint(final FixedJointInfo jointInfo) {
@@ -35,9 +34,9 @@ public class FixedJoint extends Joint {
 		this.localAnchorPointBody2 = transform2.inverseNew().multiply(jointInfo.anchorPointWorldSpace);
 		
 		// Compute the inverse of the initial orientation difference between the two bodies
-		this.initOrientationDifferenceInv = transform2.getOrientation().multiplyNew(transform1.getOrientation().inverseNew());
-		this.initOrientationDifferenceInv.normalize();
-		this.initOrientationDifferenceInv.inverse();
+		this.initOrientationDifferenceInv = transform2.getOrientation().multiply(transform1.getOrientation().inverse());
+		this.initOrientationDifferenceInv = this.initOrientationDifferenceInv.normalize();
+		this.initOrientationDifferenceInv = this.initOrientationDifferenceInv.inverse();
 	}
 	
 	@Override
@@ -67,44 +66,43 @@ public class FixedJoint extends Joint {
 		
 		// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
 		final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse;
-		final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
-		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew()));
-		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew()));
+		Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
+		massMatrix = massMatrix.add(skewSymmetricMatrixU1.multiply(this.i1).multiply(skewSymmetricMatrixU1.transpose()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU2.multiply(this.i2).multiply(skewSymmetricMatrixU2.transpose()));
 		
 		// Compute the inverse mass matrix K^-1 for the 3 translation raints
-		this.inverseMassMatrixTranslation.setZero();
+		this.inverseMassMatrixTranslation = Matrix3f.ZERO;
 		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
-			this.inverseMassMatrixTranslation = massMatrix.inverseNew();
+			this.inverseMassMatrixTranslation = massMatrix.inverse();
 		}
 		
 		// Compute the bias "b" of the raint for the 3 translation raints
-		final float biasFactor = (BETA / raintSolverData.timeStep);
-		this.biasTranslation.setZero();
+		final float biasFactor = (FixedJoint.BETA / raintSolverData.timeStep);
+		this.biasTranslation = Vector3f.ZERO;
 		if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
-			this.biasTranslation = x2.addNew(this.r2World).less(x1).less(this.r1World).multiply(biasFactor);
+			this.biasTranslation = x2.add(this.r2World).less(x1).less(this.r1World).multiply(biasFactor);
 		}
 		
 		// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
 		// contraints (3x3 matrix)
-		this.inverseMassMatrixRotation = this.i1.addNew(this.i2);
+		this.inverseMassMatrixRotation = this.i1.add(this.i2);
 		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
-			this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.inverseNew();
+			this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.inverse();
 		}
 		
 		// Compute the bias "b" for the 3 rotation raints
-		this.biasRotation.setZero();
+		this.biasRotation = Vector3f.ZERO;
 		if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
-			final Quaternion currentOrientationDifference = orientationBody2.multiplyNew(orientationBody1.inverseNew());
-			currentOrientationDifference.normalize();
-			final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv);
+			final Quaternion currentOrientationDifference = orientationBody2.multiply(orientationBody1.inverse()).normalize();
+			final Quaternion qError = currentOrientationDifference.multiply(this.initOrientationDifferenceInv);
 			this.biasRotation = qError.getVectorV().multiply(biasFactor * 2.0f);
 		}
 		
 		// If warm-starting is not enabled
 		if (!raintSolverData.isWarmStartingActive) {
 			// Reset the accumulated impulses
-			this.impulseTranslation.setZero();
-			this.impulseRotation.setZero();
+			this.impulseTranslation = Vector3f.ZERO;
+			this.impulseRotation = Vector3f.ZERO;
 		}
 	}
 	
@@ -118,10 +116,10 @@ public class FixedJoint extends Joint {
 		}
 		
 		// Get the bodies positions and orientations
-		final Vector3f x1 = raintSolverData.positions[this.indexBody1];
-		final Vector3f x2 = raintSolverData.positions[this.indexBody2];
-		final Quaternion q1 = raintSolverData.orientations[this.indexBody1];
-		final Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+		Vector3f x1 = raintSolverData.positions[this.indexBody1];
+		Vector3f x2 = raintSolverData.positions[this.indexBody2];
+		Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+		Quaternion q2 = raintSolverData.orientations[this.indexBody2];
 		
 		// Get the inverse mass and inverse inertia tensors of the bodies
 		final float inverseMassBody1 = this.body1.massInverse;
@@ -143,89 +141,88 @@ public class FixedJoint extends Joint {
 		
 		// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
 		final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse;
-		final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
-		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).add(skewSymmetricMatrixU1.transposeNew()));
-		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).add(skewSymmetricMatrixU2.transposeNew()));
-		this.inverseMassMatrixTranslation.setZero();
+		Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
+		massMatrix = massMatrix.add(skewSymmetricMatrixU1.multiply(this.i1).add(skewSymmetricMatrixU1.transpose()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU2.multiply(this.i2).add(skewSymmetricMatrixU2.transpose()));
+		this.inverseMassMatrixTranslation = Matrix3f.ZERO;
 		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
-			this.inverseMassMatrixTranslation = massMatrix.inverseNew();
+			this.inverseMassMatrixTranslation = massMatrix.inverse();
 		}
 		
 		// Compute position error for the 3 translation raints
-		final Vector3f errorTranslation = x2.addNew(this.r2World).less(x1).less(this.r1World);
+		final Vector3f errorTranslation = x2.add(this.r2World).less(x1).less(this.r1World);
 		
 		// Compute the Lagrange multiplier lambda
-		final Vector3f lambdaTranslation = this.inverseMassMatrixTranslation.multiplyNew(errorTranslation.multiplyNew(-1));
+		final Vector3f lambdaTranslation = this.inverseMassMatrixTranslation.multiply(errorTranslation.multiply(-1));
 		
 		// Compute the impulse of body 1
-		final Vector3f linearImpulseBody1 = lambdaTranslation.multiplyNew(-1);
+		final Vector3f linearImpulseBody1 = lambdaTranslation.multiply(-1);
 		Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World);
 		
 		// Compute the pseudo velocity of body 1
-		final Vector3f v1 = linearImpulseBody1.multiplyNew(inverseMassBody1);
-		Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1);
+		final Vector3f v1 = linearImpulseBody1.multiply(inverseMassBody1);
+		Vector3f w1 = this.i1.multiply(angularImpulseBody1);
 		
 		// Update the body position/orientation of body 1
-		x1.add(v1);
-		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
-		q1.normalize();
+		x1 = x1.add(v1);
+		q1 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1 = q1.normalize();
 		
 		// Compute the impulse of body 2
 		final Vector3f angularImpulseBody2 = lambdaTranslation.cross(this.r2World).multiply(-1);
 		
 		// Compute the pseudo velocity of body 2
-		final Vector3f v2 = lambdaTranslation.multiplyNew(inverseMassBody2);
-		Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2);
+		final Vector3f v2 = lambdaTranslation.multiply(inverseMassBody2);
+		Vector3f w2 = this.i2.multiply(angularImpulseBody2);
 		
 		// Update the body position/orientation of body 2
-		x2.add(v2);
-		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
-		q2.normalize();
+		x2 = x2.add(v2);
+		q2 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
+		q2 = q2.normalize();
 		
 		// --------------- Rotation Constraints --------------- //
 		
 		// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
 		// contraints (3x3 matrix)
-		this.inverseMassMatrixRotation = this.i1.addNew(this.i2);
+		this.inverseMassMatrixRotation = this.i1.add(this.i2);
 		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
-			this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.inverseNew();
+			this.inverseMassMatrixRotation = this.inverseMassMatrixRotation.inverse();
 		}
 		
 		// Compute the position error for the 3 rotation raints
-		final Quaternion currentOrientationDifference = q2.multiplyNew(q1.inverseNew());
-		currentOrientationDifference.normalize();
-		final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv);
+		final Quaternion currentOrientationDifference = q2.multiply(q1.inverse()).normalize();
+		final Quaternion qError = currentOrientationDifference.multiply(this.initOrientationDifferenceInv);
 		final Vector3f errorRotation = qError.getVectorV().multiply(2.0f);
 		
 		// Compute the Lagrange multiplier lambda for the 3 rotation raints
-		final Vector3f lambdaRotation = this.inverseMassMatrixRotation.multiplyNew(errorRotation.multiplyNew(-1));
+		final Vector3f lambdaRotation = this.inverseMassMatrixRotation.multiply(errorRotation.multiply(-1));
 		
 		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
-		angularImpulseBody1 = lambdaRotation.multiplyNew(-1);
+		angularImpulseBody1 = lambdaRotation.multiply(-1);
 		
 		// Compute the pseudo velocity of body 1
-		w1 = this.i1.multiplyNew(angularImpulseBody1);
+		w1 = this.i1.multiply(angularImpulseBody1);
 		
 		// Update the body position/orientation of body 1
-		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
-		q1.normalize();
+		q1 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1 = q1.normalize();
 		
 		// Compute the pseudo velocity of body 2
-		w2 = this.i2.multiplyNew(lambdaRotation);
+		w2 = this.i2.multiply(lambdaRotation);
 		
 		// Update the body position/orientation of body 2
-		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
-		q2.normalize();
+		q2 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
+		q2 = q2.normalize();
 	}
 	
 	@Override
 	public void solveVelocityConstraint(final ConstraintSolverData raintSolverData) {
 		
 		// Get the velocities
-		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
 		
 		// Get the inverse mass of the bodies
 		final float inverseMassBody1 = this.body1.massInverse;
@@ -234,79 +231,79 @@ public class FixedJoint extends Joint {
 		// --------------- Translation Constraints --------------- //
 		
 		// Compute J*v for the 3 translation raints
-		final Vector3f JvTranslation = v2.addNew(w2.cross(this.r2World)).less(v1).less(w1.cross(this.r1World));
+		final Vector3f jvTranslation = v2.add(w2.cross(this.r2World)).less(v1).less(w1.cross(this.r1World));
 		
 		// Compute the Lagrange multiplier lambda
-		final Vector3f deltaLambda = this.inverseMassMatrixTranslation.multiplyNew(JvTranslation.multiplyNew(-1).less(this.biasTranslation));
-		this.impulseTranslation.add(deltaLambda);
+		final Vector3f deltaLambda = this.inverseMassMatrixTranslation.multiply(jvTranslation.multiply(-1).less(this.biasTranslation));
+		this.impulseTranslation = this.impulseTranslation.add(deltaLambda);
 		
 		// Compute the impulse P=J^T * lambda for body 1
-		final Vector3f linearImpulseBody1 = deltaLambda.multiplyNew(-1);
+		final Vector3f linearImpulseBody1 = deltaLambda.multiply(-1);
 		Vector3f angularImpulseBody1 = deltaLambda.cross(this.r1World);
 		
 		// Apply the impulse to the body 1
-		v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		v1 = v1.add(linearImpulseBody1.multiply(inverseMassBody1));
+		w1 = w1.add(this.i1.multiply(angularImpulseBody1));
 		
 		// Compute the impulse P=J^T * lambda  for body 2
 		final Vector3f angularImpulseBody2 = deltaLambda.cross(this.r2World).multiply(-1);
 		
 		// Apply the impulse to the body 2
-		v2.add(deltaLambda.multiplyNew(inverseMassBody2));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		v2 = v2.add(deltaLambda.multiply(inverseMassBody2));
+		w2 = w2.add(this.i2.multiply(angularImpulseBody2));
 		
 		// --------------- Rotation Constraints --------------- //
 		
 		// Compute J*v for the 3 rotation raints
-		final Vector3f JvRotation = w2.lessNew(w1);
+		final Vector3f jvRotation = w2.less(w1);
 		
 		// Compute the Lagrange multiplier lambda for the 3 rotation raints
-		final Vector3f deltaLambda2 = this.inverseMassMatrixRotation.multiplyNew(JvRotation.multiplyNew(-1).less(this.biasRotation));
-		this.impulseRotation.add(deltaLambda2);
+		final Vector3f deltaLambda2 = this.inverseMassMatrixRotation.multiply(jvRotation.multiply(-1).less(this.biasRotation));
+		this.impulseRotation = this.impulseRotation.add(deltaLambda2);
 		
 		// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1
-		angularImpulseBody1 = deltaLambda2.multiplyNew(-1);
+		angularImpulseBody1 = deltaLambda2.multiply(-1);
 		
 		// Apply the impulse to the body 1
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		w1 = w1.add(this.i1.multiply(angularImpulseBody1));
 		
 		// Apply the impulse to the body 2
-		w2.add(this.i2.multiplyNew(deltaLambda2));
+		w2 = w2.add(this.i2.multiply(deltaLambda2));
 	}
 	
 	@Override
 	public void warmstart(final ConstraintSolverData raintSolverData) {
 		
 		// Get the velocities
-		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
 		
 		// Get the inverse mass of the bodies
 		final float inverseMassBody1 = this.body1.massInverse;
 		final float inverseMassBody2 = this.body2.massInverse;
 		
 		// Compute the impulse P=J^T * lambda for the 3 translation raints for body 1
-		final Vector3f linearImpulseBody1 = this.impulseTranslation.multiplyNew(-1);
-		final Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
+		Vector3f linearImpulseBody1 = this.impulseTranslation.multiply(-1);
+		Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
 		
 		// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 1
-		angularImpulseBody1.add(this.impulseRotation.multiplyNew(-1));
+		this.impulseTranslation = this.impulseTranslation.add(this.impulseRotation.multiply(-1));
 		
 		// Apply the impulse to the body 1
-		v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		v1 = v1.add(linearImpulseBody1.multiply(inverseMassBody1));
+		w1 = w1.add(this.i1.multiply(angularImpulseBody1));
 		
 		// Compute the impulse P=J^T * lambda for the 3 translation raints for body 2
-		final Vector3f angularImpulseBody2 = this.impulseTranslation.cross(this.r2World).multiplyNew(-1);
+		Vector3f angularImpulseBody2 = this.impulseTranslation.cross(this.r2World).multiply(-1);
 		
 		// Compute the impulse P=J^T * lambda for the 3 rotation raints for body 2
-		angularImpulseBody2.add(this.impulseRotation);
+		angularImpulseBody2 = angularImpulseBody2.add(this.impulseRotation);
 		
 		// Apply the impulse to the body 2
-		v2.add(this.impulseTranslation.multiplyNew(inverseMassBody2));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		v2 = v2.add(this.impulseTranslation.multiply(inverseMassBody2));
+		w2 = w2.add(this.i2.multiply(angularImpulseBody2));
 		
 	}
 	

src/org/atriasoft/ephysics/constraint/FixedJointInfo.java

@@ -1,8 +1,7 @@
 package org.atriasoft.ephysics.constraint;
 
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.body.RigidBody;
+import org.atriasoft.etk.math.Vector3f;
 
 /**
  * This structure is used to gather the information needed to create a fixed
@@ -14,12 +13,12 @@ public class FixedJointInfo extends JointInfo {
 	
 	/**
 	 * Constructor
-	 * @param _rigidBody1 The first body of the joint
-	 * @param _rigidBody2 The second body of the joint
-	 * @param _initAnchorPointWorldSpace The initial anchor point of the joint in world-space coordinates
+	 * @param rigidBody1 The first body of the joint
+	 * @param rigidBody2 The second body of the joint
+	 * @param initAnchorPointWorldSpace The initial anchor point of the joint in world-space coordinates
 	 */
-	FixedJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace) {
-		super(_rigidBody1, _rigidBody2, JointType.FIXEDJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+	FixedJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace) {
+		super(rigidBody1, rigidBody2, JointType.FIXEDJOINT);
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
 	}
 }

src/org/atriasoft/ephysics/constraint/HingeJoint.java

@@ -1,5 +1,8 @@
 package org.atriasoft.ephysics.constraint;
 
+import org.atriasoft.ephysics.body.BodyType;
+import org.atriasoft.ephysics.engine.ConstraintSolverData;
+import org.atriasoft.ephysics.mathematics.Matrix2f;
 import org.atriasoft.etk.math.Constant;
 import org.atriasoft.etk.math.FMath;
 import org.atriasoft.etk.math.Matrix3f;
@@ -8,10 +11,6 @@ import org.atriasoft.etk.math.Transform3D;
 import org.atriasoft.etk.math.Vector2f;
 import org.atriasoft.etk.math.Vector3f;
 
-import org.atriasoft.ephysics.body.BodyType;
-import org.atriasoft.ephysics.engine.ConstraintSolverData;
-import org.atriasoft.ephysics.mathematics.Matrix2f;
-
 /**
  *  It represents a hinge joint that allows arbitrary rotation
  * between two bodies around a single axis. This joint has one degree of freedom. It
@@ -19,41 +18,41 @@ import org.atriasoft.ephysics.mathematics.Matrix2f;
  */
 public class HingeJoint extends Joint {
 	
-	private static float BETA; //!< Beta value for the bias factor of position correction
-	private final Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
-	private final Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
-	private final Vector3f hingeLocalAxisBody1; //!< Hinge rotation axis (in local-space coordinates of body 1)
-	private final Vector3f hingeLocalAxisBody2; //!< Hinge rotation axis (in local-space coordiantes of body 2)
+	private static final float BETA = 0.2f; //!< Beta value for the bias factor of position correction
+	private Vector3f b2CrossA1; //!< Cross product of vector b2 and a1
+	private float bLowerLimit; //!< Bias of the lower limit raint
+	private Vector2f bRotation; //!< Bias vector for the error correction for the rotation raints
+	private Vector3f bTranslation; //!< Bias vector for the error correction for the translation raints
+	private float bUpperLimit; //!< Bias of the upper limit raint
+	private Vector3f c2CrossA1; //!< Cross product of vector c2 and a1;
+	private Vector3f hingeLocalAxisBody1; //!< Hinge rotation axis (in local-space coordinates of body 1)
+	private Vector3f hingeLocalAxisBody2; //!< Hinge rotation axis (in local-space coordiantes of body 2)
 	private Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates)
 	private Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates)
-	private Vector3f mA1; //!< Hinge rotation axis (in world-space coordinates) computed from body 1
-	private Vector3f r1World; //!< Vector from center of body 2 to anchor point in world-space
-	private Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space
-	private Vector3f b2CrossA1; //!< Cross product of vector b2 and a1
-	private Vector3f c2CrossA1; //!< Cross product of vector c2 and a1;
-	private final Vector3f impulseTranslation; //!< Impulse for the 3 translation raints
-	private final Vector2f impulseRotation; //!< Impulse for the 2 rotation raints
 	private float impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
-	private float impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
 	private float impulseMotor; //!< Accumulated impulse for the motor raint;
-	private Matrix3f inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^t for the 3 translation raints
-	private Matrix2f inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^t for the 2 rotation raints
+	private Vector2f impulseRotation; //!< Impulse for the 2 rotation raints
+	private Vector3f impulseTranslation; //!< Impulse for the 3 translation raints
+	private float impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
+	private final Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the bodies
 	private float inverseMassMatrixLimitMotor; //!< Inverse of mass matrix K=JM^-1J^t for the limits and motor raints (1x1 matrix)
 	private float inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
-	private Vector3f bTranslation; //!< Bias vector for the error correction for the translation raints
-	private Vector2f bRotation; //!< Bias vector for the error correction for the rotation raints
-	private float bLowerLimit; //!< Bias of the lower limit raint
-	private float bUpperLimit; //!< Bias of the upper limit raint
-	private final Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the bodies
+	private Matrix2f inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^t for the 2 rotation raints
+	private Matrix3f inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^t for the 3 translation raints
 	private boolean isLimitEnabled; //!< True if the joint limits are enabled
-	private boolean isMotorEnabled; //!< True if the motor of the joint in enabled
-	private float lowerLimit; //!< Lower limit (minimum allowed rotation angle in radian)
-	private float upperLimit; //!< Upper limit (maximum translation distance)
 	private boolean isLowerLimitViolated; //!< True if the lower limit is violated
+	private boolean isMotorEnabled; //!< True if the motor of the joint in enabled
 	private boolean isUpperLimitViolated; //!< True if the upper limit is violated
-	private float motorSpeed; //!< Motor speed (in rad/s)
+	private final Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
+	private final Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
+	private float lowerLimit; //!< Lower limit (minimum allowed rotation angle in radian)
+	private Vector3f mA1; //!< Hinge rotation axis (in world-space coordinates) computed from body 1
 	private float maxMotorTorque; //!< Maximum motor torque (in Newtons) that can be applied to reach to desired motor speed
 	/// Reset the limits
+	private float motorSpeed; //!< Motor speed (in rad/s)
+	private Vector3f r1World; //!< Vector from center of body 2 to anchor point in world-space
+	private Vector3f r2World; //!< Vector from center of body 2 to anchor point in world-space
+	private float upperLimit; //!< Upper limit (maximum translation distance)
 	
 	/// Constructor
 	public HingeJoint(final HingeJointInfo jointInfo) {
@@ -83,15 +82,13 @@ public class HingeJoint extends Joint {
 		this.localAnchorPointBody2 = transform2.inverseNew().multiply(jointInfo.anchorPointWorldSpace);
 		
 		// Compute the local-space hinge axis
-		this.hingeLocalAxisBody1 = transform1.getOrientation().inverseNew().multiply(jointInfo.rotationAxisWorld);
-		this.hingeLocalAxisBody2 = transform2.getOrientation().inverseNew().multiply(jointInfo.rotationAxisWorld);
-		this.hingeLocalAxisBody1.normalize();
-		this.hingeLocalAxisBody2.normalize();
+		this.hingeLocalAxisBody1 = transform1.getOrientation().inverse().multiply(jointInfo.rotationAxisWorld);
+		this.hingeLocalAxisBody2 = transform2.getOrientation().inverse().multiply(jointInfo.rotationAxisWorld);
+		this.hingeLocalAxisBody1 = this.hingeLocalAxisBody1.normalize();
+		this.hingeLocalAxisBody2 = this.hingeLocalAxisBody2.normalize();
 		
 		// Compute the inverse of the initial orientation difference between the two bodies
-		this.initOrientationDifferenceInv = transform2.getOrientation().multiplyNew(transform1.getOrientation().inverseNew());
-		this.initOrientationDifferenceInv.normalize();
-		this.initOrientationDifferenceInv.inverse();
+		this.initOrientationDifferenceInv = transform2.getOrientation().multiply(transform1.getOrientation().inverse()).normalize().inverse();
 	}
 	
 	/// Given an "inputAngle" in the range [-pi, pi], this method returns an
@@ -100,17 +97,18 @@ public class HingeJoint extends Joint {
 	private float computeCorrespondingAngleNearLimits(final float inputAngle, final float lowerLimitAngle, final float upperLimitAngle) {
 		if (upperLimitAngle <= lowerLimitAngle) {
 			return inputAngle;
-		} else if (inputAngle > upperLimitAngle) {
+		}
+		if (inputAngle > upperLimitAngle) {
 			final float diffToUpperLimit = FMath.abs(computeNormalizedAngle(inputAngle - upperLimitAngle));
 			final float diffToLowerLimit = FMath.abs(computeNormalizedAngle(inputAngle - lowerLimitAngle));
 			return (diffToUpperLimit > diffToLowerLimit) ? (inputAngle - Constant.PI_2) : inputAngle;
-		} else if (inputAngle < lowerLimitAngle) {
+		}
+		if (inputAngle < lowerLimitAngle) {
 			final float diffToUpperLimit = FMath.abs(computeNormalizedAngle(upperLimitAngle - inputAngle));
 			final float diffToLowerLimit = FMath.abs(computeNormalizedAngle(lowerLimitAngle - inputAngle));
 			return (diffToUpperLimit > diffToLowerLimit) ? inputAngle : (inputAngle + Constant.PI_2);
-		} else {
-			return inputAngle;
 		}
+		return inputAngle;
 	}
 	
 	/// Compute the current angle around the hinge axis
@@ -119,12 +117,12 @@ public class HingeJoint extends Joint {
 		float hingeAngle;
 		
 		// Compute the current orientation difference between the two bodies
-		final Quaternion currentOrientationDiff = orientationBody2.multiplyNew(orientationBody1.inverseNew());
-		currentOrientationDiff.normalize();
+		Quaternion currentOrientationDiff = orientationBody2.multiply(orientationBody1.inverse());
+		currentOrientationDiff = currentOrientationDiff.normalize();
 		
 		// Compute the relative rotation idering the initial orientation difference
-		final Quaternion relativeRotation = currentOrientationDiff.multiplyNew(this.initOrientationDifferenceInv);
-		relativeRotation.normalize();
+		Quaternion relativeRotation = currentOrientationDiff.multiply(this.initOrientationDifferenceInv);
+		relativeRotation = relativeRotation.normalize();
 		
 		// A quaternion q = [cos(theta/2); sin(theta/2) * rotAxis] where rotAxis is a unit
 		// length vector. We can extract cos(theta/2) with q.w and we can extract |sin(theta/2)| with :
@@ -133,7 +131,7 @@ public class HingeJoint extends Joint {
 		// axis is not pointing in the same direction as the hinge axis, we use the rotation -q which
 		// has the same |sin(theta/2)| value but the value cos(theta/2) is sign inverted. Some details
 		// about this trick is explained in the source code of OpenTissue (http://www.opentissue.org).
-		final float cosHalfAngle = relativeRotation.getW();
+		final float cosHalfAngle = relativeRotation.w();
 		final float sinHalfAngleAbs = relativeRotation.getVectorV().length();
 		
 		// Compute the dot product of the relative rotation axis and the hinge axis
@@ -163,11 +161,11 @@ public class HingeJoint extends Joint {
 		// Convert it into the range [-pi; pi]
 		if (angle < -Constant.PI) {
 			return angle + Constant.PI_2;
-		} else if (angle > Constant.PI) {
-			return angle - Constant.PI_2;
-		} else {
-			return angle;
 		}
+		if (angle > Constant.PI) {
+			return angle - Constant.PI_2;
+		}
+		return angle;
 	}
 	
 	/**Enable/Disable the limits of the joint
@@ -279,9 +277,9 @@ public class HingeJoint extends Joint {
 		
 		// Compute vectors needed in the Jacobian
 		this.mA1 = orientationBody1.multiply(this.hingeLocalAxisBody1);
-		final Vector3f a2 = orientationBody2.multiply(this.hingeLocalAxisBody2);
-		this.mA1.normalize();
-		a2.normalize();
+		Vector3f a2 = orientationBody2.multiply(this.hingeLocalAxisBody2);
+		this.mA1 = this.mA1.normalize();
+		a2 = a2.normalize();
 		final Vector3f b2 = a2.getOrthoVector();
 		final Vector3f c2 = a2.cross(b2);
 		this.b2CrossA1 = b2.cross(this.mA1);
@@ -293,30 +291,30 @@ public class HingeJoint extends Joint {
 		
 		// Compute the inverse mass matrix K=JM^-1J^t for the 3 translation raints (3x3 matrix)
 		final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse;
-		final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
-		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew()));
-		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew()));
-		this.inverseMassMatrixTranslation.setZero();
+		Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
+		massMatrix = massMatrix.add(skewSymmetricMatrixU1.multiply(this.i1).multiply(skewSymmetricMatrixU1.transpose()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU2.multiply(this.i2).multiply(skewSymmetricMatrixU2.transpose()));
+		this.inverseMassMatrixTranslation = Matrix3f.ZERO;
 		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
-			this.inverseMassMatrixTranslation = massMatrix.inverseNew();
+			this.inverseMassMatrixTranslation = massMatrix.inverse();
 		}
 		
 		// Compute the bias "b" of the translation raints
-		this.bTranslation.setZero();
-		final float biasFactor = (BETA / raintSolverData.timeStep);
+		this.bTranslation = Vector3f.ZERO;
+		final float biasFactor = (HingeJoint.BETA / raintSolverData.timeStep);
 		if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
-			this.bTranslation = x2.addNew(this.r2World).less(x1).less(this.r1World).multiply(biasFactor);
+			this.bTranslation = x2.add(this.r2World).less(x1).less(this.r1World).multiply(biasFactor);
 		}
 		
 		// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix)
-		final Vector3f I1B2CrossA1 = this.i1.multiplyNew(this.b2CrossA1);
-		final Vector3f I1C2CrossA1 = this.i1.multiplyNew(this.c2CrossA1);
-		final Vector3f I2B2CrossA1 = this.i2.multiplyNew(this.b2CrossA1);
-		final Vector3f I2C2CrossA1 = this.i2.multiplyNew(this.c2CrossA1);
-		final float el11 = this.b2CrossA1.dot(I1B2CrossA1) + this.b2CrossA1.dot(I2B2CrossA1);
-		final float el12 = this.b2CrossA1.dot(I1C2CrossA1) + this.b2CrossA1.dot(I2C2CrossA1);
-		final float el21 = this.c2CrossA1.dot(I1B2CrossA1) + this.c2CrossA1.dot(I2B2CrossA1);
-		final float el22 = this.c2CrossA1.dot(I1C2CrossA1) + this.c2CrossA1.dot(I2C2CrossA1);
+		final Vector3f i1B2CrossA1 = this.i1.multiply(this.b2CrossA1);
+		final Vector3f i1C2CrossA1 = this.i1.multiply(this.c2CrossA1);
+		final Vector3f i2B2CrossA1 = this.i2.multiply(this.b2CrossA1);
+		final Vector3f i2C2CrossA1 = this.i2.multiply(this.c2CrossA1);
+		final float el11 = this.b2CrossA1.dot(i1B2CrossA1) + this.b2CrossA1.dot(i2B2CrossA1);
+		final float el12 = this.b2CrossA1.dot(i1C2CrossA1) + this.b2CrossA1.dot(i2C2CrossA1);
+		final float el21 = this.c2CrossA1.dot(i1B2CrossA1) + this.c2CrossA1.dot(i2B2CrossA1);
+		final float el22 = this.c2CrossA1.dot(i1C2CrossA1) + this.c2CrossA1.dot(i2C2CrossA1);
 		
 		final Matrix2f matrixKRotation = new Matrix2f(el11, el12, el21, el22);
 		this.inverseMassMatrixRotation.setZero();
@@ -325,7 +323,7 @@ public class HingeJoint extends Joint {
 		}
 		
 		// Compute the bias "b" of the rotation raints
-		this.bRotation.setZero();
+		this.bRotation = Vector2f.ZERO;
 		if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
 			this.bRotation = new Vector2f(this.mA1.dot(b2) * biasFactor, this.mA1.dot(c2) * biasFactor);
 		}
@@ -334,8 +332,8 @@ public class HingeJoint extends Joint {
 		if (!raintSolverData.isWarmStartingActive) {
 			
 			// Reset all the accumulated impulses
-			this.impulseTranslation.setZero();
-			this.impulseRotation.setZero();
+			this.impulseTranslation = Vector3f.ZERO;
+			this.impulseRotation = Vector2f.ZERO;
 			this.impulseLowerLimit = 0.0f;
 			this.impulseUpperLimit = 0.0f;
 			this.impulseMotor = 0.0f;
@@ -345,7 +343,7 @@ public class HingeJoint extends Joint {
 		if (this.isMotorEnabled || (this.isLimitEnabled && (this.isLowerLimitViolated || this.isUpperLimitViolated))) {
 			
 			// Compute the inverse of the mass matrix K=JM^-1J^t for the limits and motor (1x1 matrix)
-			this.inverseMassMatrixLimitMotor = this.mA1.dot(this.i1.multiplyNew(this.mA1)) + this.mA1.dot(this.i2.multiplyNew(this.mA1));
+			this.inverseMassMatrixLimitMotor = this.mA1.dot(this.i1.multiply(this.mA1)) + this.mA1.dot(this.i2.multiply(this.mA1));
 			this.inverseMassMatrixLimitMotor = (this.inverseMassMatrixLimitMotor > 0.0f) ? 1.0f / this.inverseMassMatrixLimitMotor : 0.0f;
 			
 			if (this.isLimitEnabled) {
@@ -466,10 +464,10 @@ public class HingeJoint extends Joint {
 		}
 		
 		// Get the bodies positions and orientations
-		final Vector3f x1 = raintSolverData.positions[this.indexBody1];
-		final Vector3f x2 = raintSolverData.positions[this.indexBody2];
-		final Quaternion q1 = raintSolverData.orientations[this.indexBody1];
-		final Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+		Vector3f x1 = raintSolverData.positions[this.indexBody1];
+		Vector3f x2 = raintSolverData.positions[this.indexBody2];
+		Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+		Quaternion q2 = raintSolverData.orientations[this.indexBody2];
 		
 		// Get the inverse mass and inverse inertia tensors of the bodies
 		final float inverseMassBody1 = this.body1.massInverse;
@@ -494,9 +492,9 @@ public class HingeJoint extends Joint {
 		
 		// Compute vectors needed in the Jacobian
 		this.mA1 = q1.multiply(this.hingeLocalAxisBody1);
-		final Vector3f a2 = q2.multiply(this.hingeLocalAxisBody2);
-		this.mA1.normalize();
-		a2.normalize();
+		Vector3f a2 = q2.multiply(this.hingeLocalAxisBody2);
+		this.mA1 = this.mA1.normalize();
+		a2 = a2.normalize();
 		final Vector3f b2 = a2.getOrthoVector();
 		final Vector3f c2 = a2.cross(b2);
 		this.b2CrossA1 = b2.cross(this.mA1);
@@ -510,56 +508,56 @@ public class HingeJoint extends Joint {
 		
 		// Compute the matrix K=JM^-1J^t (3x3 matrix) for the 3 translation raints
 		final float inverseMassBodies = this.body1.massInverse + this.body2.massInverse;
-		final Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
-		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew()));
-		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew()));
-		this.inverseMassMatrixTranslation.setZero();
+		Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
+		massMatrix = massMatrix.add(skewSymmetricMatrixU1.multiply(this.i1).multiply(skewSymmetricMatrixU1.transpose()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU2.multiply(this.i2).multiply(skewSymmetricMatrixU2.transpose()));
+		this.inverseMassMatrixTranslation = Matrix3f.ZERO;
 		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
-			this.inverseMassMatrixTranslation = massMatrix.inverseNew();
+			this.inverseMassMatrixTranslation = massMatrix.inverse();
 		}
 		
 		// Compute position error for the 3 translation raints
-		final Vector3f errorTranslation = x2.addNew(this.r2World).less(x1).less(this.r1World);
+		final Vector3f errorTranslation = x2.add(this.r2World).less(x1).less(this.r1World);
 		
 		// Compute the Lagrange multiplier lambda
-		final Vector3f lambdaTranslation = this.inverseMassMatrixTranslation.multiplyNew(errorTranslation.multiplyNew(-1));
+		final Vector3f lambdaTranslation = this.inverseMassMatrixTranslation.multiply(errorTranslation.multiply(-1));
 		
 		// Compute the impulse of body 1
-		final Vector3f linearImpulseBody1 = lambdaTranslation.multiplyNew(-1);
+		final Vector3f linearImpulseBody1 = lambdaTranslation.multiply(-1);
 		Vector3f angularImpulseBody1 = lambdaTranslation.cross(this.r1World);
 		
 		// Compute the pseudo velocity of body 1
-		final Vector3f v1 = linearImpulseBody1.multiplyNew(inverseMassBody1);
-		Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1);
+		final Vector3f v1 = linearImpulseBody1.multiply(inverseMassBody1);
+		Vector3f w1 = this.i1.multiply(angularImpulseBody1);
 		
 		// Update the body position/orientation of body 1
-		x1.add(v1);
-		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
-		q1.normalize();
+		x1 = x1.add(v1);
+		q1 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1 = q1.normalize();
 		
 		// Compute the impulse of body 2
-		Vector3f angularImpulseBody2 = lambdaTranslation.cross(this.r2World).multiplyNew(-1);
+		Vector3f angularImpulseBody2 = lambdaTranslation.cross(this.r2World).multiply(-1);
 		
 		// Compute the pseudo velocity of body 2
-		final Vector3f v2 = lambdaTranslation.multiplyNew(inverseMassBody2);
-		Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2);
+		final Vector3f v2 = lambdaTranslation.multiply(inverseMassBody2);
+		Vector3f w2 = this.i2.multiply(angularImpulseBody2);
 		
 		// Update the body position/orientation of body 2
-		x2.add(v2);
-		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
-		q2.normalize();
+		x2 = x2.add(v2);
+		q2 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
+		q2 = q2.normalize();
 		
 		// --------------- Rotation Constraints --------------- //
 		
 		// Compute the inverse mass matrix K=JM^-1J^t for the 2 rotation raints (2x2 matrix)
-		final Vector3f I1B2CrossA1 = this.i1.multiplyNew(this.b2CrossA1);
-		final Vector3f I1C2CrossA1 = this.i1.multiplyNew(this.c2CrossA1);
-		final Vector3f I2B2CrossA1 = this.i2.multiplyNew(this.b2CrossA1);
-		final Vector3f I2C2CrossA1 = this.i2.multiplyNew(this.c2CrossA1);
-		final float el11 = this.b2CrossA1.dot(I1B2CrossA1) + this.b2CrossA1.dot(I2B2CrossA1);
-		final float el12 = this.b2CrossA1.dot(I1C2CrossA1) + this.b2CrossA1.dot(I2C2CrossA1);
-		final float el21 = this.c2CrossA1.dot(I1B2CrossA1) + this.c2CrossA1.dot(I2B2CrossA1);
-		final float el22 = this.c2CrossA1.dot(I1C2CrossA1) + this.c2CrossA1.dot(I2C2CrossA1);
+		final Vector3f i1B2CrossA1 = this.i1.multiply(this.b2CrossA1);
+		final Vector3f i1C2CrossA1 = this.i1.multiply(this.c2CrossA1);
+		final Vector3f i2B2CrossA1 = this.i2.multiply(this.b2CrossA1);
+		final Vector3f i2C2CrossA1 = this.i2.multiply(this.c2CrossA1);
+		final float el11 = this.b2CrossA1.dot(i1B2CrossA1) + this.b2CrossA1.dot(i2B2CrossA1);
+		final float el12 = this.b2CrossA1.dot(i1C2CrossA1) + this.b2CrossA1.dot(i2C2CrossA1);
+		final float el21 = this.c2CrossA1.dot(i1B2CrossA1) + this.c2CrossA1.dot(i2B2CrossA1);
+		final float el22 = this.c2CrossA1.dot(i1C2CrossA1) + this.c2CrossA1.dot(i2C2CrossA1);
 		
 		final Matrix2f matrixKRotation = new Matrix2f(el11, el12, el21, el22);
 		this.inverseMassMatrixRotation.setZero();
@@ -571,27 +569,27 @@ public class HingeJoint extends Joint {
 		final Vector2f errorRotation = new Vector2f(this.mA1.dot(b2), this.mA1.dot(c2));
 		
 		// Compute the Lagrange multiplier lambda for the 3 rotation raints
-		final Vector2f lambdaRotation = this.inverseMassMatrixRotation.multiplyNew(errorRotation.multiplyNew(-1));
+		final Vector2f lambdaRotation = this.inverseMassMatrixRotation.multiplyNew(errorRotation.multiply(-1));
 		
 		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
-		angularImpulseBody1 = this.b2CrossA1.multiplyNew(-1).multiply(lambdaRotation.x).less(this.c2CrossA1.multiplyNew(lambdaRotation.y));
+		angularImpulseBody1 = this.b2CrossA1.multiply(-1).multiply(lambdaRotation.x()).less(this.c2CrossA1.multiply(lambdaRotation.y()));
 		
 		// Compute the pseudo velocity of body 1
-		w1 = this.i1.multiplyNew(angularImpulseBody1);
+		w1 = this.i1.multiply(angularImpulseBody1);
 		
 		// Update the body position/orientation of body 1
-		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
-		q1.normalize();
+		q1 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1 = q1.normalize();
 		
 		// Compute the impulse of body 2
-		angularImpulseBody2 = this.b2CrossA1.multiplyNew(lambdaRotation.x).add(this.c2CrossA1.multiplyNew(lambdaRotation.y));
+		angularImpulseBody2 = this.b2CrossA1.multiply(lambdaRotation.x()).add(this.c2CrossA1.multiply(lambdaRotation.y()));
 		
 		// Compute the pseudo velocity of body 2
-		w2 = this.i2.multiplyNew(angularImpulseBody2);
+		w2 = this.i2.multiply(angularImpulseBody2);
 		
 		// Update the body position/orientation of body 2
-		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
-		q2.normalize();
+		q2 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
+		q2 = q2.normalize();
 		
 		// --------------- Limits Constraints --------------- //
 		
@@ -600,7 +598,7 @@ public class HingeJoint extends Joint {
 			if (this.isLowerLimitViolated || this.isUpperLimitViolated) {
 				
 				// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
-				this.inverseMassMatrixLimitMotor = this.mA1.dot(this.i1.multiplyNew(this.mA1)) + this.mA1.dot(this.i2.multiplyNew(this.mA1));
+				this.inverseMassMatrixLimitMotor = this.mA1.dot(this.i1.multiply(this.mA1)) + this.mA1.dot(this.i2.multiply(this.mA1));
 				this.inverseMassMatrixLimitMotor = (this.inverseMassMatrixLimitMotor > 0.0f) ? 1.0f / this.inverseMassMatrixLimitMotor : 0.0f;
 			}
 			
@@ -611,24 +609,24 @@ public class HingeJoint extends Joint {
 				final float lambdaLowerLimit = this.inverseMassMatrixLimitMotor * (-lowerLimitError);
 				
 				// Compute the impulse P=J^T * lambda of body 1
-				final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(lambdaLowerLimit);
+				final Vector3f angularImpulseBody1Tmp = this.mA1.multiply(lambdaLowerLimit);
 				
 				// Compute the pseudo velocity of body 1
-				final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp);
+				final Vector3f w1Tmp = this.i1.multiply(angularImpulseBody1Tmp);
 				
 				// Update the body position/orientation of body 1
-				q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f));
-				q1.normalize();
+				q1 = q1.add((new Quaternion(0, w1Tmp)).multiply(q1).multiply(0.5f));
+				q1 = q1.normalize();
 				
 				// Compute the impulse P=J^T * lambda of body 2
-				final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(lambdaLowerLimit);
+				final Vector3f angularImpulseBody2Tmp = this.mA1.multiply(lambdaLowerLimit);
 				
 				// Compute the pseudo velocity of body 2
-				final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp);
+				final Vector3f w2Tmp = this.i2.multiply(angularImpulseBody2Tmp);
 				
 				// Update the body position/orientation of body 2
-				q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f));
-				q2.normalize();
+				q2 = q2.add((new Quaternion(0, w2Tmp)).multiply(q2).multiply(0.5f));
+				q2 = q2.normalize();
 			}
 			
 			// If the upper limit is violated
@@ -638,24 +636,24 @@ public class HingeJoint extends Joint {
 				final float lambdaUpperLimit = this.inverseMassMatrixLimitMotor * (-upperLimitError);
 				
 				// Compute the impulse P=J^T * lambda of body 1
-				final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(lambdaUpperLimit);
+				final Vector3f angularImpulseBody1Tmp = this.mA1.multiply(lambdaUpperLimit);
 				
 				// Compute the pseudo velocity of body 1
-				final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp);
+				final Vector3f w1Tmp = this.i1.multiply(angularImpulseBody1Tmp);
 				
 				// Update the body position/orientation of body 1
-				q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f));
-				q1.normalize();
+				q1 = q1.add((new Quaternion(0, w1Tmp)).multiply(q1).multiply(0.5f));
+				q1 = q1.normalize();
 				
 				// Compute the impulse P=J^T * lambda of body 2
-				final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(-lambdaUpperLimit);
+				final Vector3f angularImpulseBody2Tmp = this.mA1.multiply(-lambdaUpperLimit);
 				
 				// Compute the pseudo velocity of body 2
-				final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp);
+				final Vector3f w2Tmp = this.i2.multiply(angularImpulseBody2Tmp);
 				
 				// Update the body position/orientation of body 2
-				q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f));
-				q2.normalize();
+				q2 = q2.add((new Quaternion(0, w2Tmp)).multiply(q2).multiply(0.5f));
+				q2 = q2.normalize();
 			}
 		}
 	}
@@ -664,10 +662,10 @@ public class HingeJoint extends Joint {
 	public void solveVelocityConstraint(final ConstraintSolverData raintSolverData) {
 		
 		// Get the velocities
-		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
 		
 		// Get the inverse mass and inverse inertia tensors of the bodies
 		final float inverseMassBody1 = this.body1.massInverse;
@@ -676,47 +674,47 @@ public class HingeJoint extends Joint {
 		// --------------- Translation Constraints --------------- //
 		
 		// Compute J*v
-		final Vector3f JvTranslation = v2.addNew(w2.cross(this.r2World)).less(v1).less(w1.cross(this.r1World));
+		final Vector3f jvTranslation = v2.add(w2.cross(this.r2World)).less(v1).less(w1.cross(this.r1World));
 		
 		// Compute the Lagrange multiplier lambda
-		final Vector3f deltaLambdaTranslation = this.inverseMassMatrixTranslation.multiplyNew(JvTranslation.multiplyNew(-1).less(this.bTranslation));
-		this.impulseTranslation.add(deltaLambdaTranslation);
+		final Vector3f deltaLambdaTranslation = this.inverseMassMatrixTranslation.multiply(jvTranslation.multiply(-1).less(this.bTranslation));
+		this.impulseTranslation = this.impulseTranslation.add(deltaLambdaTranslation);
 		
 		// Compute the impulse P=J^T * lambda of body 1
-		final Vector3f linearImpulseBody1 = deltaLambdaTranslation.multiplyNew(-1);
+		final Vector3f linearImpulseBody1 = deltaLambdaTranslation.multiply(-1);
 		Vector3f angularImpulseBody1 = deltaLambdaTranslation.cross(this.r1World);
 		
 		// Apply the impulse to the body 1
-		v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		v1 = v1.add(linearImpulseBody1.multiply(inverseMassBody1));
+		w1 = w1.add(this.i1.multiply(angularImpulseBody1));
 		
 		// Compute the impulse P=J^T * lambda of body 2
-		Vector3f angularImpulseBody2 = deltaLambdaTranslation.cross(this.r2World).multiplyNew(-1);
+		Vector3f angularImpulseBody2 = deltaLambdaTranslation.cross(this.r2World).multiply(-1);
 		
 		// Apply the impulse to the body 2
-		v2.add(deltaLambdaTranslation.multiplyNew(inverseMassBody2));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		v2 = v2.add(deltaLambdaTranslation.multiply(inverseMassBody2));
+		w2 = w2.add(this.i2.multiply(angularImpulseBody2));
 		
 		// --------------- Rotation Constraints --------------- //
 		
 		// Compute J*v for the 2 rotation raints
-		final Vector2f JvRotation = new Vector2f(-this.b2CrossA1.dot(w1) + this.b2CrossA1.dot(w2), -this.c2CrossA1.dot(w1) + this.c2CrossA1.dot(w2));
+		final Vector2f jvRotation = new Vector2f(-this.b2CrossA1.dot(w1) + this.b2CrossA1.dot(w2), -this.c2CrossA1.dot(w1) + this.c2CrossA1.dot(w2));
 		
 		// Compute the Lagrange multiplier lambda for the 2 rotation raints
-		final Vector2f deltaLambdaRotation = this.inverseMassMatrixRotation.multiplyNew(JvRotation.multiplyNew(-1).less(this.bRotation));
-		this.impulseRotation.add(deltaLambdaRotation);
+		final Vector2f deltaLambdaRotation = this.inverseMassMatrixRotation.multiplyNew(jvRotation.multiply(-1).less(this.bRotation));
+		this.impulseRotation = this.impulseRotation.add(deltaLambdaRotation);
 		
 		// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 1
-		angularImpulseBody1 = this.b2CrossA1.multiplyNew(-1).multiply(deltaLambdaRotation.x).less(this.c2CrossA1.multiplyNew(deltaLambdaRotation.y));
+		angularImpulseBody1 = this.b2CrossA1.multiply(-1).multiply(deltaLambdaRotation.x()).less(this.c2CrossA1.multiply(deltaLambdaRotation.y()));
 		
 		// Apply the impulse to the body 1
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		w1 = w1.add(this.i1.multiply(angularImpulseBody1));
 		
 		// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 2
-		angularImpulseBody2 = this.b2CrossA1.multiplyNew(deltaLambdaRotation.x).add(this.c2CrossA1.multiplyNew(deltaLambdaRotation.y));
+		angularImpulseBody2 = this.b2CrossA1.multiply(deltaLambdaRotation.x()).add(this.c2CrossA1.multiply(deltaLambdaRotation.y()));
 		
 		// Apply the impulse to the body 2
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		w2 = w2.add(this.i2.multiply(angularImpulseBody2));
 		
 		// --------------- Limits Constraints --------------- //
 		
@@ -726,50 +724,50 @@ public class HingeJoint extends Joint {
 			if (this.isLowerLimitViolated) {
 				
 				// Compute J*v for the lower limit raint
-				final float JvLowerLimit = w2.lessNew(w1).dot(this.mA1);
+				final float jvLowerLimit = w2.less(w1).dot(this.mA1);
 				
 				// Compute the Lagrange multiplier lambda for the lower limit raint
-				float deltaLambdaLower = this.inverseMassMatrixLimitMotor * (-JvLowerLimit - this.bLowerLimit);
+				float deltaLambdaLower = this.inverseMassMatrixLimitMotor * (-jvLowerLimit - this.bLowerLimit);
 				final float lambdaTemp = this.impulseLowerLimit;
 				this.impulseLowerLimit = FMath.max(this.impulseLowerLimit + deltaLambdaLower, 0.0f);
 				deltaLambdaLower = this.impulseLowerLimit - lambdaTemp;
 				
 				// Compute the impulse P=J^T * lambda for the lower limit raint of body 1
-				final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(-deltaLambdaLower);
+				final Vector3f angularImpulseBody1Tmp = this.mA1.multiply(-deltaLambdaLower);
 				
 				// Apply the impulse to the body 1
-				w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp));
+				w1 = w1.add(this.i1.multiply(angularImpulseBody1Tmp));
 				
 				// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
-				final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(deltaLambdaLower);
+				final Vector3f angularImpulseBody2Tmp = this.mA1.multiply(deltaLambdaLower);
 				
 				// Apply the impulse to the body 2
-				w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp));
+				w2 = w2.add(this.i2.multiply(angularImpulseBody2Tmp));
 			}
 			
 			// If the upper limit is violated
 			if (this.isUpperLimitViolated) {
 				
 				// Compute J*v for the upper limit raint
-				final float JvUpperLimit = w2.lessNew(w1).multiplyNew(-1).dot(this.mA1);
+				final float jvUpperLimit = w2.less(w1).multiply(-1).dot(this.mA1);
 				
 				// Compute the Lagrange multiplier lambda for the upper limit raint
-				float deltaLambdaUpper = this.inverseMassMatrixLimitMotor * (-JvUpperLimit - this.bUpperLimit);
+				float deltaLambdaUpper = this.inverseMassMatrixLimitMotor * (-jvUpperLimit - this.bUpperLimit);
 				final float lambdaTemp = this.impulseUpperLimit;
 				this.impulseUpperLimit = FMath.max(this.impulseUpperLimit + deltaLambdaUpper, 0.0f);
 				deltaLambdaUpper = this.impulseUpperLimit - lambdaTemp;
 				
 				// Compute the impulse P=J^T * lambda for the upper limit raint of body 1
-				final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(deltaLambdaUpper);
+				final Vector3f angularImpulseBody1Tmp = this.mA1.multiply(deltaLambdaUpper);
 				
 				// Apply the impulse to the body 1
-				w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp));
+				w1 = w1.add(this.i1.multiply(angularImpulseBody1Tmp));
 				
 				// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
-				final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(-deltaLambdaUpper);
+				final Vector3f angularImpulseBody2Tmp = this.mA1.multiply(-deltaLambdaUpper);
 				
 				// Apply the impulse to the body 2
-				w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp));
+				w2 = w2.add(this.i2.multiply(angularImpulseBody2Tmp));
 			}
 		}
 		
@@ -779,26 +777,26 @@ public class HingeJoint extends Joint {
 		if (this.isMotorEnabled) {
 			
 			// Compute J*v for the motor
-			final float JvMotor = this.mA1.dot(w1.lessNew(w2));
+			final float jvMotor = this.mA1.dot(w1.less(w2));
 			
 			// Compute the Lagrange multiplier lambda for the motor
 			final float maxMotorImpulse = this.maxMotorTorque * raintSolverData.timeStep;
-			float deltaLambdaMotor = this.inverseMassMatrixLimitMotor * (-JvMotor - this.motorSpeed);
+			float deltaLambdaMotor = this.inverseMassMatrixLimitMotor * (-jvMotor - this.motorSpeed);
 			final float lambdaTemp = this.impulseMotor;
 			this.impulseMotor = FMath.clamp(this.impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
 			deltaLambdaMotor = this.impulseMotor - lambdaTemp;
 			
 			// Compute the impulse P=J^T * lambda for the motor of body 1
-			final Vector3f angularImpulseBody1_tmp = this.mA1.multiplyNew(-deltaLambdaMotor);
+			final Vector3f angularImpulseBody1Tmp = this.mA1.multiply(-deltaLambdaMotor);
 			
 			// Apply the impulse to the body 1
-			w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp));
+			w1 = w1.add(this.i1.multiply(angularImpulseBody1Tmp));
 			
 			// Compute the impulse P=J^T * lambda for the motor of body 2
-			final Vector3f angularImpulseBody2_tmp = this.mA1.multiplyNew(deltaLambdaMotor);
+			final Vector3f angularImpulseBody2Tmp = this.mA1.multiply(deltaLambdaMotor);
 			
 			// Apply the impulse to the body 2
-			w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp));
+			w2 = w2.add(this.i2.multiply(angularImpulseBody2Tmp));
 		}
 		
 	}
@@ -807,56 +805,56 @@ public class HingeJoint extends Joint {
 	public void warmstart(final ConstraintSolverData raintSolverData) {
 		
 		// Get the velocities
-		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
 		
 		// Get the inverse mass and inverse inertia tensors of the bodies
 		final float inverseMassBody1 = this.body1.massInverse;
 		final float inverseMassBody2 = this.body2.massInverse;
 		
 		// Compute the impulse P=J^T * lambda for the 2 rotation raints
-		final Vector3f rotationImpulse = this.b2CrossA1.multiplyNew(-1).multiply(this.impulseRotation.x).less(this.c2CrossA1.multiplyNew(this.impulseRotation.y));
+		final Vector3f rotationImpulse = this.b2CrossA1.multiply(-1).multiply(this.impulseRotation.x()).less(this.c2CrossA1.multiply(this.impulseRotation.y()));
 		
 		// Compute the impulse P=J^T * lambda for the lower and upper limits raints
-		final Vector3f limitsImpulse = this.mA1.multiplyNew(this.impulseUpperLimit - this.impulseLowerLimit);
+		final Vector3f limitsImpulse = this.mA1.multiply(this.impulseUpperLimit - this.impulseLowerLimit);
 		
 		// Compute the impulse P=J^T * lambda for the motor raint
-		final Vector3f motorImpulse = this.mA1.multiplyNew(-this.impulseMotor);
+		final Vector3f motorImpulse = this.mA1.multiply(-this.impulseMotor);
 		
 		// Compute the impulse P=J^T * lambda for the 3 translation raints of body 1
-		final Vector3f linearImpulseBody1 = this.impulseTranslation.multiplyNew(-1);
-		final Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
+		final Vector3f linearImpulseBody1 = this.impulseTranslation.multiply(-1);
+		Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
 		
 		// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 1
-		angularImpulseBody1.add(rotationImpulse);
+		angularImpulseBody1 = angularImpulseBody1.add(rotationImpulse);
 		
 		// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1
-		angularImpulseBody1.add(limitsImpulse);
+		angularImpulseBody1 = angularImpulseBody1.add(limitsImpulse);
 		
 		// Compute the impulse P=J^T * lambda for the motor raint of body 1
-		angularImpulseBody1.add(motorImpulse);
+		angularImpulseBody1 = angularImpulseBody1.add(motorImpulse);
 		
 		// Apply the impulse to the body 1
-		v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		v1 = v1.add(linearImpulseBody1.multiply(inverseMassBody1));
+		w1 = w1.add(this.i1.multiply(angularImpulseBody1));
 		
 		// Compute the impulse P=J^T * lambda for the 3 translation raints of body 2
-		final Vector3f angularImpulseBody2 = this.impulseTranslation.cross(this.r2World).multiplyNew(-1);
+		Vector3f angularImpulseBody2 = this.impulseTranslation.cross(this.r2World).multiply(-1);
 		
 		// Compute the impulse P=J^T * lambda for the 2 rotation raints of body 2
-		angularImpulseBody2.add(rotationImpulse.multiplyNew(-1));
+		angularImpulseBody2 = angularImpulseBody2.add(rotationImpulse.multiply(-1));
 		
 		// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 2
-		angularImpulseBody2.add(limitsImpulse.multiplyNew(-1));
+		angularImpulseBody2 = angularImpulseBody2.add(limitsImpulse.multiply(-1));
 		
 		// Compute the impulse P=J^T * lambda for the motor raint of body 2
-		angularImpulseBody2.add(motorImpulse.multiplyNew(-1));
+		angularImpulseBody2 = angularImpulseBody2.add(motorImpulse.multiply(-1));
 		
 		// Apply the impulse to the body 2
-		v2.add(this.impulseTranslation.multiplyNew(inverseMassBody2));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		v2 = v2.add(this.impulseTranslation.multiply(inverseMassBody2));
+		w2 = w2.add(this.i2.multiply(angularImpulseBody2));
 	}
 	
 }

src/org/atriasoft/ephysics/constraint/HingeJointInfo.java

@@ -1,8 +1,7 @@
 package org.atriasoft.ephysics.constraint;
 
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.body.RigidBody;
+import org.atriasoft.etk.math.Vector3f;
 
 /**
  *  It is used to gather the information needed to create a hinge joint.
@@ -11,25 +10,25 @@ import org.atriasoft.ephysics.body.RigidBody;
 public class HingeJointInfo extends JointInfo {
 	
 	public Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
-	public Vector3f rotationAxisWorld; //!< Hinge rotation axis (in world-space coordinates)
 	public boolean isLimitEnabled; //!< True if the hinge joint limits are enabled
 	public boolean isMotorEnabled; //!< True if the hinge joint motor is enabled
-	public float minAngleLimit; //!< Minimum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [-2*pi, 0]
 	public float maxAngleLimit; //!< Maximum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [0, 2*pi]
-	public float motorSpeed; //!< Motor speed (in radian/second)
 	public float maxMotorTorque; //!< Maximum motor torque (in Newtons * meters) that can be applied to reach to desired motor speed
+	public float minAngleLimit; //!< Minimum allowed rotation angle (in radian) if limits are enabled. The angle must be in the range [-2*pi, 0]
+	public float motorSpeed; //!< Motor speed (in radian/second)
+	public Vector3f rotationAxisWorld; //!< Hinge rotation axis (in world-space coordinates)
 	
 	/**
 	 *  Constructor without limits and without motor
-	 * @param _rigidBody1 The first body of the joint
-	 * @param _rigidBody2 The second body of the joint
-	 * @param _initAnchorPointWorldSpace The initial anchor point in world-space coordinates
-	 * @param _initRotationAxisWorld The initial rotation axis in world-space coordinates
+	 * @param rigidBody1 The first body of the joint
+	 * @param rigidBody2 The second body of the joint
+	 * @param initAnchorPointWorldSpace The initial anchor point in world-space coordinates
+	 * @param initRotationAxisWorld The initial rotation axis in world-space coordinates
 	 */
-	public HingeJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initRotationAxisWorld) {
-		super(_rigidBody1, _rigidBody2, JointType.HINGEJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
-		this.rotationAxisWorld = _initRotationAxisWorld;
+	public HingeJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace, final Vector3f initRotationAxisWorld) {
+		super(rigidBody1, rigidBody2, JointType.HINGEJOINT);
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
+		this.rotationAxisWorld = initRotationAxisWorld;
 		this.isLimitEnabled = false;
 		this.isMotorEnabled = false;
 		this.minAngleLimit = -1;
@@ -41,22 +40,22 @@ public class HingeJointInfo extends JointInfo {
 	
 	/**
 	 *  Constructor with limits but without motor
-	 * @param _rigidBody1 The first body of the joint
-	 * @param _rigidBody2 The second body of the joint
-	 * @param _initAnchorPointWorldSpace The initial anchor point in world-space coordinates
-	 * @param _initRotationAxisWorld The intial rotation axis in world-space coordinates
-	 * @param _initMinAngleLimit The initial minimum limit angle (in radian)
-	 * @param _initMaxAngleLimit The initial maximum limit angle (in radian)
+	 * @param rigidBody1 The first body of the joint
+	 * @param rigidBody2 The second body of the joint
+	 * @param initAnchorPointWorldSpace The initial anchor point in world-space coordinates
+	 * @param initRotationAxisWorld The intial rotation axis in world-space coordinates
+	 * @param initMinAngleLimit The initial minimum limit angle (in radian)
+	 * @param initMaxAngleLimit The initial maximum limit angle (in radian)
 	 */
-	public HingeJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initRotationAxisWorld, final float _initMinAngleLimit,
-			final float _initMaxAngleLimit) {
-		super(_rigidBody1, _rigidBody2, JointType.HINGEJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
-		this.rotationAxisWorld = _initRotationAxisWorld;
+	public HingeJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace, final Vector3f initRotationAxisWorld, final float initMinAngleLimit,
+			final float initMaxAngleLimit) {
+		super(rigidBody1, rigidBody2, JointType.HINGEJOINT);
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
+		this.rotationAxisWorld = initRotationAxisWorld;
 		this.isLimitEnabled = true;
 		this.isMotorEnabled = false;
-		this.minAngleLimit = _initMinAngleLimit;
-		this.maxAngleLimit = _initMaxAngleLimit;
+		this.minAngleLimit = initMinAngleLimit;
+		this.maxAngleLimit = initMaxAngleLimit;
 		this.motorSpeed = 0;
 		this.maxMotorTorque = 0;
 		
@@ -64,26 +63,26 @@ public class HingeJointInfo extends JointInfo {
 	
 	/**
 	 *  Constructor with limits and motor
-	 * @param _rigidBody1 The first body of the joint
-	 * @param _rigidBody2 The second body of the joint
-	 * @param _initAnchorPointWorldSpace The initial anchor point in world-space
-	 * @param _initRotationAxisWorld The initial rotation axis in world-space
-	 * @param _initMinAngleLimit The initial minimum limit angle (in radian)
-	 * @param _initMaxAngleLimit The initial maximum limit angle (in radian)
-	 * @param _initMotorSpeed The initial motor speed of the joint (in radian per second)
-	 * @param _initMaxMotorTorque The initial maximum motor torque (in Newtons)
+	 * @param rigidBody1 The first body of the joint
+	 * @param rigidBody2 The second body of the joint
+	 * @param initAnchorPointWorldSpace The initial anchor point in world-space
+	 * @param initRotationAxisWorld The initial rotation axis in world-space
+	 * @param initMinAngleLimit The initial minimum limit angle (in radian)
+	 * @param initMaxAngleLimit The initial maximum limit angle (in radian)
+	 * @param initMotorSpeed The initial motor speed of the joint (in radian per second)
+	 * @param initMaxMotorTorque The initial maximum motor torque (in Newtons)
 	 */
-	public HingeJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initRotationAxisWorld, final float _initMinAngleLimit,
-			final float _initMaxAngleLimit, final float _initMotorSpeed, final float _initMaxMotorTorque) {
-		super(_rigidBody1, _rigidBody2, JointType.HINGEJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
-		this.rotationAxisWorld = _initRotationAxisWorld;
+	public HingeJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace, final Vector3f initRotationAxisWorld, final float initMinAngleLimit,
+			final float initMaxAngleLimit, final float initMotorSpeed, final float initMaxMotorTorque) {
+		super(rigidBody1, rigidBody2, JointType.HINGEJOINT);
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
+		this.rotationAxisWorld = initRotationAxisWorld;
 		this.isLimitEnabled = true;
 		this.isMotorEnabled = false;
-		this.minAngleLimit = _initMinAngleLimit;
-		this.maxAngleLimit = _initMaxAngleLimit;
-		this.motorSpeed = _initMotorSpeed;
-		this.maxMotorTorque = _initMaxMotorTorque;
+		this.minAngleLimit = initMinAngleLimit;
+		this.maxAngleLimit = initMaxAngleLimit;
+		this.motorSpeed = initMotorSpeed;
+		this.maxMotorTorque = initMaxMotorTorque;
 		
 	}
-};
+}

src/org/atriasoft/ephysics/constraint/JointInfo.java

@@ -10,27 +10,27 @@ public class JointInfo {
 	public final RigidBody body1;
 	//!< Second rigid body of the joint
 	public final RigidBody body2;
-	//!< Type of the joint
-	public final JointType type;
-	//!< Position correction technique used for the raint (used for joints). By default, the BAUMGARTE technique is used
-	public final JointsPositionCorrectionTechnique positionCorrectionTechnique;
 	//!< True if the two bodies of the joint are allowed to collide with each other
 	public final boolean isCollisionEnabled;
+	//!< Position correction technique used for the raint (used for joints). By default, the BAUMGARTE technique is used
+	public final JointsPositionCorrectionTechnique positionCorrectionTechnique;
+	//!< Type of the joint
+	public final JointType type;
 	
 	/// Constructor
-	JointInfo(final JointType _raintType) {
+	JointInfo(final JointType raintType) {
 		this.body1 = null;
 		this.body2 = null;
-		this.type = _raintType;
+		this.type = raintType;
 		this.positionCorrectionTechnique = JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL;
 		this.isCollisionEnabled = true;
 	}
 	
 	/// Constructor
-	JointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final JointType _raintType) {
-		this.body1 = _rigidBody1;
-		this.body2 = _rigidBody2;
-		this.type = _raintType;
+	JointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final JointType raintType) {
+		this.body1 = rigidBody1;
+		this.body2 = rigidBody2;
+		this.type = raintType;
 		this.positionCorrectionTechnique = JointsPositionCorrectionTechnique.NON_LINEAR_GAUSS_SEIDEL;
 		this.isCollisionEnabled = true;
 	}

src/org/atriasoft/ephysics/constraint/JointListElement.java

@@ -8,9 +8,9 @@ public class JointListElement {
 	/**
 	 * Constructor
 	 */
-	public JointListElement(final Joint _initJoint, final JointListElement _initNext) {
-		this.joint = _initJoint;
-		this.next = _initNext;
+	public JointListElement(final Joint initJoint, final JointListElement initNext) {
+		this.joint = initJoint;
+		this.next = initNext;
 		
 	}
 }

src/org/atriasoft/ephysics/constraint/SliderJoint.java

@@ -1,5 +1,8 @@
 package org.atriasoft.ephysics.constraint;
 
+import org.atriasoft.ephysics.body.BodyType;
+import org.atriasoft.ephysics.engine.ConstraintSolverData;
+import org.atriasoft.ephysics.mathematics.Matrix2f;
 import org.atriasoft.etk.math.FMath;
 import org.atriasoft.etk.math.Matrix3f;
 import org.atriasoft.etk.math.Quaternion;
@@ -7,51 +10,47 @@ import org.atriasoft.etk.math.Transform3D;
 import org.atriasoft.etk.math.Vector2f;
 import org.atriasoft.etk.math.Vector3f;
 
-import org.atriasoft.ephysics.body.BodyType;
-import org.atriasoft.ephysics.engine.ConstraintSolverData;
-import org.atriasoft.ephysics.mathematics.Matrix2f;
-
 public class SliderJoint extends Joint {
 	private static final float BETA = 0.2f; //!< Beta value for the position correction bias factor
-	private final Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
-	private final Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
-	private final Vector3f sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1)
+	private float bLowerLimit; //!< Bias of the lower limit raint
+	private Vector3f bRotation; //!< Bias of the 3 rotation raints
+	private Vector2f bTranslation; //!< Bias of the 2 translation raints
+	private float bUpperLimit; //!< Bias of the upper limit raint
 	private Matrix3f i1; //!< Inertia tensor of body 1 (in world-space coordinates)
 	private Matrix3f i2; //!< Inertia tensor of body 2 (in world-space coordinates)
-	private final Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies
-	private Vector3f N1; //!< First vector orthogonal to the slider axis local-space of body 1
-	private Vector3f N2; //!< Second vector orthogonal to the slider axis and N1 in local-space of body 1
-	private Vector3f R1; //!< Vector r1 in world-space coordinates
-	private Vector3f R2; //!< Vector r2 in world-space coordinates
-	private Vector3f R2CrossN1; //!< Cross product of r2 and n1
-	private Vector3f R2CrossN2; //!< Cross product of r2 and n2
-	private Vector3f R2CrossSliderAxis; //!< Cross product of r2 and the slider axis
-	private Vector3f R1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1
-	private Vector3f R1PlusUCrossN2; //!< Cross product of vector (r1 + u) and n2
-	private Vector3f R1PlusUCrossSliderAxis; //!< Cross product of vector (r1 + u) and the slider axis
-	private Vector2f bTranslation; //!< Bias of the 2 translation raints
-	private Vector3f bRotation; //!< Bias of the 3 rotation raints
-	private float bLowerLimit; //!< Bias of the lower limit raint
-	private float bUpperLimit; //!< Bias of the upper limit raint
-	private Matrix2f inverseMassMatrixTranslationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the translation raint (2x2 matrix)
-	private Matrix3f inverseMassMatrixRotationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the rotation raint (3x3 matrix)
+	private float impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
+	private float impulseMotor; //!< Accumulated impulse for the motor
+	private Vector3f impulseRotation; //!< Accumulated impulse for the 3 rotation raints
+	private Vector2f impulseTranslation; //!< Accumulated impulse for the 2 translation raints
+	private float impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
+	private Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies
 	private float inverseMassMatrixLimit; //!< Inverse of mass matrix K=JM^-1J^t for the upper and lower limit raints (1x1 matrix)
 	private float inverseMassMatrixMotor; //!< Inverse of mass matrix K=JM^-1J^t for the motor
-	private final Vector2f impulseTranslation; //!< Accumulated impulse for the 2 translation raints
-	private final Vector3f impulseRotation; //!< Accumulated impulse for the 3 rotation raints
-	private float impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
-	private float impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
-	private float impulseMotor; //!< Accumulated impulse for the motor
+	private Matrix3f inverseMassMatrixRotationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the rotation raint (3x3 matrix)
+	private Matrix2f inverseMassMatrixTranslationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the translation raint (2x2 matrix)
 	private boolean isLimitEnabled; //!< True if the slider limits are enabled
-	private boolean isMotorEnabled; //!< True if the motor of the joint in enabled
-	private Vector3f sliderAxisWorld; //!< Slider axis in world-space coordinates
-	private float lowerLimit; //!< Lower limit (minimum translation distance)
-	private float upperLimit; //!< Upper limit (maximum translation distance)
 	private boolean isLowerLimitViolated; //!< True if the lower limit is violated
+	private boolean isMotorEnabled; //!< True if the motor of the joint in enabled
 	private boolean isUpperLimitViolated; //!< True if the upper limit is violated
-	private float motorSpeed; //!< Motor speed (in m/s)
+	private final Vector3f localAnchorPointBody1; //!< Anchor point of body 1 (in local-space coordinates of body 1)
+	private final Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
+	private float lowerLimit; //!< Lower limit (minimum translation distance)
 	private float maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
 	/// Constructor
+	private float motorSpeed; //!< Motor speed (in m/s)
+	private Vector3f n1; //!< First vector orthogonal to the slider axis local-space of body 1
+	private Vector3f n2; //!< Second vector orthogonal to the slider axis and N1 in local-space of body 1
+	private Vector3f r1; //!< Vector r1 in world-space coordinates
+	private Vector3f r1PlusUCrossN1; //!< Cross product of vector (r1 + u) and n1
+	private Vector3f r1PlusUCrossN2; //!< Cross product of vector (r1 + u) and n2
+	private Vector3f r1PlusUCrossSliderAxis; //!< Cross product of vector (r1 + u) and the slider axis
+	private Vector3f r2; //!< Vector r2 in world-space coordinates
+	private Vector3f r2CrossN1; //!< Cross product of r2 and n1
+	private Vector3f r2CrossN2; //!< Cross product of r2 and n2
+	private Vector3f r2CrossSliderAxis; //!< Cross product of r2 and the slider axis
+	private Vector3f sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1)
+	private Vector3f sliderAxisWorld; //!< Slider axis in world-space coordinates
+	private float upperLimit; //!< Upper limit (maximum translation distance)
 	
 	public SliderJoint(final SliderJointInfo jointInfo) {
 		super(jointInfo);
@@ -80,13 +79,13 @@ public class SliderJoint extends Joint {
 		this.localAnchorPointBody2 = transform2.inverseNew().multiply(jointInfo.anchorPointWorldSpace);
 		
 		// Compute the inverse of the initial orientation difference between the two bodies
-		this.initOrientationDifferenceInv = transform2.getOrientation().multiplyNew(transform1.getOrientation().inverseNew());
-		this.initOrientationDifferenceInv.normalize();
-		this.initOrientationDifferenceInv.inverse();
+		this.initOrientationDifferenceInv = transform2.getOrientation().multiply(transform1.getOrientation().inverse());
+		this.initOrientationDifferenceInv = this.initOrientationDifferenceInv.normalize();
+		this.initOrientationDifferenceInv = this.initOrientationDifferenceInv.inverse();
 		
 		// Compute the slider axis in local-space of body 1
-		this.sliderAxisBody1 = this.body1.getTransform().getOrientation().inverseNew().multiply(jointInfo.sliderAxisWorldSpace);
-		this.sliderAxisBody1.normalize();
+		this.sliderAxisBody1 = this.body1.getTransform().getOrientation().inverse().multiply(jointInfo.sliderAxisWorldSpace);
+		this.sliderAxisBody1 = this.sliderAxisBody1.normalize();
 	}
 	
 	/// Enable/Disable the limits of the joint
@@ -167,7 +166,7 @@ public class SliderJoint extends Joint {
 	 */
 	public float getTranslation() {
 		
-		// TODO : Check if we need to compare rigid body position or center of mass here
+		// TODO Check if we need to compare rigid body position or center of mass here
 		
 		// Get the bodies positions and orientations
 		final Vector3f x1 = this.body1.getTransform().getPosition();
@@ -176,15 +175,15 @@ public class SliderJoint extends Joint {
 		final Quaternion q2 = this.body2.getTransform().getOrientation();
 		
 		// Compute the two anchor points in world-space coordinates
-		final Vector3f anchorBody1 = x1.addNew(q1.multiply(this.localAnchorPointBody1));
-		final Vector3f anchorBody2 = x2.addNew(q2.multiply(this.localAnchorPointBody2));
+		final Vector3f anchorBody1 = x1.add(q1.multiply(this.localAnchorPointBody1));
+		final Vector3f anchorBody2 = x2.add(q2.multiply(this.localAnchorPointBody2));
 		
 		// Compute the vector u (difference between anchor points)
-		final Vector3f u = anchorBody2.lessNew(anchorBody1);
+		final Vector3f u = anchorBody2.less(anchorBody1);
 		
 		// Compute the slider axis in world-space
-		final Vector3f sliderAxisWorld = q1.multiply(this.sliderAxisBody1);
-		sliderAxisWorld.normalize();
+		Vector3f sliderAxisWorld = q1.multiply(this.sliderAxisBody1);
+		sliderAxisWorld = sliderAxisWorld.normalize();
 		
 		// Compute and return the translation value
 		return u.dot(sliderAxisWorld);
@@ -209,17 +208,17 @@ public class SliderJoint extends Joint {
 			this.i2 = this.body2.getInertiaTensorInverseWorld();
 			
 			// Vector from body center to the anchor point
-			this.R1 = orientationBody1.multiply(this.localAnchorPointBody1);
-			this.R2 = orientationBody2.multiply(this.localAnchorPointBody2);
+			this.r1 = orientationBody1.multiply(this.localAnchorPointBody1);
+			this.r2 = orientationBody2.multiply(this.localAnchorPointBody2);
 			
 			// Compute the vector u (difference between anchor points)
-			final Vector3f u = x2.addNew(this.R2).less(x1).less(this.R1);
+			final Vector3f u = x2.add(this.r2).less(x1).less(this.r1);
 			
 			// Compute the two orthogonal vectors to the slider axis in world-space
 			this.sliderAxisWorld = orientationBody1.multiply(this.sliderAxisBody1);
-			this.sliderAxisWorld.normalize();
-			this.N1 = this.sliderAxisWorld.getOrthoVector();
-			this.N2 = this.sliderAxisWorld.cross(this.N1);
+			this.sliderAxisWorld = this.sliderAxisWorld.normalize();
+			this.n1 = this.sliderAxisWorld.getOrthoVector();
+			this.n2 = this.sliderAxisWorld.cross(this.n1);
 			
 			// Check if the limit raints are violated or not
 			final float uDotSliderAxis = u.dot(this.sliderAxisWorld);
@@ -237,25 +236,25 @@ public class SliderJoint extends Joint {
 			}
 			
 			// Compute the cross products used in the Jacobians
-			this.R2CrossN1 = this.R2.cross(this.N1);
-			this.R2CrossN2 = this.R2.cross(this.N2);
-			this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld);
-			final Vector3f r1PlusU = this.R1.addNew(u);
-			this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1);
-			this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2);
-			this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld);
+			this.r2CrossN1 = this.r2.cross(this.n1);
+			this.r2CrossN2 = this.r2.cross(this.n2);
+			this.r2CrossSliderAxis = this.r2.cross(this.sliderAxisWorld);
+			final Vector3f r1PlusU = this.r1.add(u);
+			this.r1PlusUCrossN1 = (r1PlusU).cross(this.n1);
+			this.r1PlusUCrossN2 = (r1PlusU).cross(this.n2);
+			this.r1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld);
 			
 			// Compute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
 			// raints (2x2 matrix)
 			final float sumInverseMass = this.body1.massInverse + this.body2.massInverse;
-			final Vector3f I1R1PlusUCrossN1 = this.i1.multiplyNew(this.R1PlusUCrossN1);
-			final Vector3f I1R1PlusUCrossN2 = this.i1.multiplyNew(this.R1PlusUCrossN2);
-			final Vector3f I2R2CrossN1 = this.i2.multiplyNew(this.R2CrossN1);
-			final Vector3f I2R2CrossN2 = this.i2.multiplyNew(this.R2CrossN2);
-			final float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + this.R2CrossN1.dot(I2R2CrossN1);
-			final float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) + this.R2CrossN1.dot(I2R2CrossN2);
-			final float el21 = this.R1PlusUCrossN2.dot(I1R1PlusUCrossN1) + this.R2CrossN2.dot(I2R2CrossN1);
-			final float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + this.R2CrossN2.dot(I2R2CrossN2);
+			final Vector3f i1r1PlusUCrossN1 = this.i1.multiply(this.r1PlusUCrossN1);
+			final Vector3f i1r1PlusUCrossN2 = this.i1.multiply(this.r1PlusUCrossN2);
+			final Vector3f i2r2CrossN1 = this.i2.multiply(this.r2CrossN1);
+			final Vector3f i2r2CrossN2 = this.i2.multiply(this.r2CrossN2);
+			final float el11 = sumInverseMass + this.r1PlusUCrossN1.dot(i1r1PlusUCrossN1) + this.r2CrossN1.dot(i2r2CrossN1);
+			final float el12 = this.r1PlusUCrossN1.dot(i1r1PlusUCrossN2) + this.r2CrossN1.dot(i2r2CrossN2);
+			final float el21 = this.r1PlusUCrossN2.dot(i1r1PlusUCrossN1) + this.r2CrossN2.dot(i2r2CrossN1);
+			final float el22 = sumInverseMass + this.r1PlusUCrossN2.dot(i1r1PlusUCrossN2) + this.r2CrossN2.dot(i2r2CrossN2);
 			
 			final Matrix2f matrixKTranslation = new Matrix2f(el11, el12, el21, el22);
 			this.inverseMassMatrixTranslationConstraint.setZero();
@@ -264,34 +263,33 @@ public class SliderJoint extends Joint {
 			}
 			
 			// Compute the bias "b" of the translation raint
-			this.bTranslation.setZero();
-			final float biasFactor = (BETA / raintSolverData.timeStep);
+			this.bTranslation = Vector2f.ZERO;
+			final float biasFactor = (SliderJoint.BETA / raintSolverData.timeStep);
 			if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
-				this.bTranslation.setX(u.dot(this.N1));
-				this.bTranslation.setY(u.dot(this.N2));
-				this.bTranslation.multiply(biasFactor);
+				this.bTranslation = new Vector2f(u.dot(this.n1), u.dot(this.n2));
+				this.bTranslation = this.bTranslation.multiply(biasFactor);
 			}
 			
 			// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
 			// contraints (3x3 matrix)
-			this.inverseMassMatrixRotationConstraint = this.i1.addNew(this.i2);
+			this.inverseMassMatrixRotationConstraint = this.i1.add(this.i2);
 			if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
-				this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.inverseNew();
+				this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.inverse();
 			}
 			
 			// Compute the bias "b" of the rotation raint
-			this.bRotation.setZero();
+			this.bRotation = Vector3f.ZERO;
 			if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
-				final Quaternion currentOrientationDifference = orientationBody2.multiplyNew(orientationBody1.inverseNew());
-				currentOrientationDifference.normalize();
-				final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv);
-				this.bRotation = qError.getVectorV().multiplyNew(biasFactor * 2.0f);
+				Quaternion currentOrientationDifference = orientationBody2.multiply(orientationBody1.inverse());
+				currentOrientationDifference = currentOrientationDifference.normalize();
+				final Quaternion qError = currentOrientationDifference.multiply(this.initOrientationDifferenceInv);
+				this.bRotation = qError.getVectorV().multiply(biasFactor * 2.0f);
 			}
 			// If the limits are enabled
 			if (this.isLimitEnabled && (this.isLowerLimitViolated || this.isUpperLimitViolated)) {
 				// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
-				this.inverseMassMatrixLimit = this.body1.massInverse + this.body2.massInverse + this.R1PlusUCrossSliderAxis.dot(this.i1.multiplyNew(this.R1PlusUCrossSliderAxis))
-						+ this.R2CrossSliderAxis.dot(this.i2.multiplyNew(this.R2CrossSliderAxis));
+				this.inverseMassMatrixLimit = this.body1.massInverse + this.body2.massInverse + this.r1PlusUCrossSliderAxis.dot(this.i1.multiply(this.r1PlusUCrossSliderAxis))
+						+ this.r2CrossSliderAxis.dot(this.i2.multiply(this.r2CrossSliderAxis));
 				this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0f) ? 1.0f / this.inverseMassMatrixLimit : 0.0f;
 				// Compute the bias "b" of the lower limit raint
 				this.bLowerLimit = 0.0f;
@@ -315,8 +313,8 @@ public class SliderJoint extends Joint {
 			// If warm-starting is not enabled
 			if (!raintSolverData.isWarmStartingActive) {
 				// Reset all the accumulated impulses
-				this.impulseTranslation.setZero();
-				this.impulseRotation.setZero();
+				this.impulseTranslation = Vector2f.ZERO;
+				this.impulseRotation = Vector3f.ZERO;
 				this.impulseLowerLimit = 0.0f;
 				this.impulseUpperLimit = 0.0f;
 				this.impulseMotor = 0.0f;
@@ -429,10 +427,10 @@ public class SliderJoint extends Joint {
 		}
 		
 		// Get the bodies positions and orientations
-		final Vector3f x1 = raintSolverData.positions[this.indexBody1];
-		final Vector3f x2 = raintSolverData.positions[this.indexBody2];
-		final Quaternion q1 = raintSolverData.orientations[this.indexBody1];
-		final Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+		Vector3f x1 = raintSolverData.positions[this.indexBody1];
+		Vector3f x2 = raintSolverData.positions[this.indexBody2];
+		Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+		Quaternion q2 = raintSolverData.orientations[this.indexBody2];
 		
 		// Get the inverse mass and inverse inertia tensors of the bodies
 		final float inverseMassBody1 = this.body1.massInverse;
@@ -443,17 +441,17 @@ public class SliderJoint extends Joint {
 		this.i2 = this.body2.getInertiaTensorInverseWorld();
 		
 		// Vector from body center to the anchor point
-		this.R1 = q1.multiply(this.localAnchorPointBody1);
-		this.R2 = q2.multiply(this.localAnchorPointBody2);
+		this.r1 = q1.multiply(this.localAnchorPointBody1);
+		this.r2 = q2.multiply(this.localAnchorPointBody2);
 		
 		// Compute the vector u (difference between anchor points)
-		final Vector3f u = x2.addNew(this.R2).less(x1).less(this.R1);
+		final Vector3f u = x2.add(this.r2).less(x1).less(this.r1);
 		
 		// Compute the two orthogonal vectors to the slider axis in world-space
 		this.sliderAxisWorld = q1.multiply(this.sliderAxisBody1);
-		this.sliderAxisWorld.normalize();
-		this.N1 = this.sliderAxisWorld.getOrthoVector();
-		this.N2 = this.sliderAxisWorld.cross(this.N1);
+		this.sliderAxisWorld = this.sliderAxisWorld.normalize();
+		this.n1 = this.sliderAxisWorld.getOrthoVector();
+		this.n2 = this.sliderAxisWorld.cross(this.n1);
 		
 		// Check if the limit raints are violated or not
 		final float uDotSliderAxis = u.dot(this.sliderAxisWorld);
@@ -463,27 +461,27 @@ public class SliderJoint extends Joint {
 		this.isUpperLimitViolated = upperLimitError <= 0;
 		
 		// Compute the cross products used in the Jacobians
-		this.R2CrossN1 = this.R2.cross(this.N1);
-		this.R2CrossN2 = this.R2.cross(this.N2);
-		this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld);
-		final Vector3f r1PlusU = this.R1.addNew(u);
-		this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1);
-		this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2);
-		this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld);
+		this.r2CrossN1 = this.r2.cross(this.n1);
+		this.r2CrossN2 = this.r2.cross(this.n2);
+		this.r2CrossSliderAxis = this.r2.cross(this.sliderAxisWorld);
+		final Vector3f r1PlusU = this.r1.add(u);
+		this.r1PlusUCrossN1 = (r1PlusU).cross(this.n1);
+		this.r1PlusUCrossN2 = (r1PlusU).cross(this.n2);
+		this.r1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld);
 		
 		// --------------- Translation Constraints --------------- //
 		
 		// Recompute the inverse of the mass matrix K=JM^-1J^t for the 2 translation
 		// raints (2x2 matrix)
 		final float sumInverseMass = this.body1.massInverse + this.body2.massInverse;
-		final Vector3f I1R1PlusUCrossN1 = this.i1.multiplyNew(this.R1PlusUCrossN1);
-		final Vector3f I1R1PlusUCrossN2 = this.i1.multiplyNew(this.R1PlusUCrossN2);
-		final Vector3f I2R2CrossN1 = this.i2.multiplyNew(this.R2CrossN1);
-		final Vector3f I2R2CrossN2 = this.i2.multiplyNew(this.R2CrossN2);
-		final float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + this.R2CrossN1.dot(I2R2CrossN1);
-		final float el12 = this.R1PlusUCrossN1.dot(I1R1PlusUCrossN2) + this.R2CrossN1.dot(I2R2CrossN2);
-		final float el21 = this.R1PlusUCrossN2.dot(I1R1PlusUCrossN1) + this.R2CrossN2.dot(I2R2CrossN1);
-		final float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + this.R2CrossN2.dot(I2R2CrossN2);
+		final Vector3f i1r1PlusUCrossN1 = this.i1.multiply(this.r1PlusUCrossN1);
+		final Vector3f i1r1PlusUCrossN2 = this.i1.multiply(this.r1PlusUCrossN2);
+		final Vector3f i2r2CrossN1 = this.i2.multiply(this.r2CrossN1);
+		final Vector3f i2r2CrossN2 = this.i2.multiply(this.r2CrossN2);
+		final float el11 = sumInverseMass + this.r1PlusUCrossN1.dot(i1r1PlusUCrossN1) + this.r2CrossN1.dot(i2r2CrossN1);
+		final float el12 = this.r1PlusUCrossN1.dot(i1r1PlusUCrossN2) + this.r2CrossN1.dot(i2r2CrossN2);
+		final float el21 = this.r1PlusUCrossN2.dot(i1r1PlusUCrossN1) + this.r2CrossN2.dot(i2r2CrossN1);
+		final float el22 = sumInverseMass + this.r1PlusUCrossN2.dot(i1r1PlusUCrossN2) + this.r2CrossN2.dot(i2r2CrossN2);
 		
 		final Matrix2f matrixKTranslation = new Matrix2f(el11, el12, el21, el22);
 		this.inverseMassMatrixTranslationConstraint.setZero();
@@ -492,74 +490,74 @@ public class SliderJoint extends Joint {
 		}
 		
 		// Compute the position error for the 2 translation raints
-		final Vector2f translationError = new Vector2f(u.dot(this.N1), u.dot(this.N2));
+		final Vector2f translationError = new Vector2f(u.dot(this.n1), u.dot(this.n2));
 		
 		// Compute the Lagrange multiplier lambda for the 2 translation raints
-		final Vector2f lambdaTranslation = this.inverseMassMatrixTranslationConstraint.multiplyNew(translationError.multiplyNew(-1));
+		final Vector2f lambdaTranslation = this.inverseMassMatrixTranslationConstraint.multiplyNew(translationError.multiply(-1));
 		
 		// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
-		final Vector3f linearImpulseBody1 = this.N1.multiplyNew(-1).multiplyNew(lambdaTranslation.x).lessNew(this.N2.multiplyNew(lambdaTranslation.y));
-		Vector3f angularImpulseBody1 = this.R1PlusUCrossN1.multiplyNew(-1).multiplyNew(lambdaTranslation.x).lessNew(this.R1PlusUCrossN2.multiplyNew(lambdaTranslation.y));
+		final Vector3f linearImpulseBody1 = this.n1.multiply(-1).multiply(lambdaTranslation.x()).less(this.n2.multiply(lambdaTranslation.y()));
+		Vector3f angularImpulseBody1 = this.r1PlusUCrossN1.multiply(-1).multiply(lambdaTranslation.x()).less(this.r1PlusUCrossN2.multiply(lambdaTranslation.y()));
 		
 		// Apply the impulse to the body 1
-		final Vector3f v1 = linearImpulseBody1.multiplyNew(inverseMassBody1);
-		Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1);
+		final Vector3f v1 = linearImpulseBody1.multiply(inverseMassBody1);
+		Vector3f w1 = this.i1.multiply(angularImpulseBody1);
 		
 		// Update the body position/orientation of body 1
-		x1.add(v1);
-		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
-		q1.normalize();
+		x1 = x1.add(v1);
+		q1 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1 = q1.normalize();
 		
 		// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
-		final Vector3f linearImpulseBody2 = this.N1.multiplyNew(lambdaTranslation.x).addNew(this.N2.multiplyNew(lambdaTranslation.y));
-		Vector3f angularImpulseBody2 = this.R2CrossN1.multiplyNew(lambdaTranslation.x).add(this.R2CrossN2.multiplyNew(lambdaTranslation.y));
+		final Vector3f linearImpulseBody2 = this.n1.multiply(lambdaTranslation.x()).add(this.n2.multiply(lambdaTranslation.y()));
+		Vector3f angularImpulseBody2 = this.r2CrossN1.multiply(lambdaTranslation.x()).add(this.r2CrossN2.multiply(lambdaTranslation.y()));
 		
 		// Apply the impulse to the body 2
-		final Vector3f v2 = linearImpulseBody2.multiplyNew(inverseMassBody2);
-		Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2);
+		final Vector3f v2 = linearImpulseBody2.multiply(inverseMassBody2);
+		Vector3f w2 = this.i2.multiply(angularImpulseBody2);
 		
 		// Update the body position/orientation of body 2
-		x2.add(v2);
-		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
-		q2.normalize();
+		x2 = x2.add(v2);
+		q2 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
+		q2 = q2.normalize();
 		
 		// --------------- Rotation Constraints --------------- //
 		
 		// Compute the inverse of the mass matrix K=JM^-1J^t for the 3 rotation
 		// contraints (3x3 matrix)
-		this.inverseMassMatrixRotationConstraint = this.i1.addNew(this.i2);
+		this.inverseMassMatrixRotationConstraint = this.i1.add(this.i2);
 		if (this.body1.getType() == BodyType.DYNAMIC || this.body2.getType() == BodyType.DYNAMIC) {
-			this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.inverseNew();
+			this.inverseMassMatrixRotationConstraint = this.inverseMassMatrixRotationConstraint.inverse();
 		}
 		
 		// Compute the position error for the 3 rotation raints
-		final Quaternion currentOrientationDifference = q2.multiplyNew(q1.inverseNew());
-		currentOrientationDifference.normalize();
-		final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv);
+		Quaternion currentOrientationDifference = q2.multiply(q1.inverse());
+		currentOrientationDifference = currentOrientationDifference.normalize();
+		final Quaternion qError = currentOrientationDifference.multiply(this.initOrientationDifferenceInv);
 		final Vector3f errorRotation = qError.getVectorV().multiply(2.0f);
 		
 		// Compute the Lagrange multiplier lambda for the 3 rotation raints
-		final Vector3f lambdaRotation = this.inverseMassMatrixRotationConstraint.multiplyNew(errorRotation.multiplyNew(-1));
+		final Vector3f lambdaRotation = this.inverseMassMatrixRotationConstraint.multiply(errorRotation.multiply(-1));
 		
 		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
-		angularImpulseBody1 = lambdaRotation.multiplyNew(-1);
+		angularImpulseBody1 = lambdaRotation.multiply(-1);
 		
 		// Apply the impulse to the body 1
-		w1 = this.i1.multiplyNew(angularImpulseBody1);
+		w1 = this.i1.multiply(angularImpulseBody1);
 		
 		// Update the body position/orientation of body 1
-		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
-		q1.normalize();
+		q1 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1 = q1.normalize();
 		
 		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2
 		angularImpulseBody2 = lambdaRotation;
 		
 		// Apply the impulse to the body 2
-		w2 = this.i2.multiplyNew(angularImpulseBody2);
+		w2 = this.i2.multiply(angularImpulseBody2);
 		
 		// Update the body position/orientation of body 2
-		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
-		q2.normalize();
+		q2 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
+		q2 = q2.normalize();
 		
 		// --------------- Limits Constraints --------------- //
 		
@@ -568,8 +566,8 @@ public class SliderJoint extends Joint {
 			if (this.isLowerLimitViolated || this.isUpperLimitViolated) {
 				
 				// Compute the inverse of the mass matrix K=JM^-1J^t for the limits (1x1 matrix)
-				this.inverseMassMatrixLimit = this.body1.massInverse + this.body2.massInverse + this.R1PlusUCrossSliderAxis.dot(this.i1.multiplyNew(this.R1PlusUCrossSliderAxis))
-						+ this.R2CrossSliderAxis.dot(this.i2.multiplyNew(this.R2CrossSliderAxis));
+				this.inverseMassMatrixLimit = this.body1.massInverse + this.body2.massInverse + this.r1PlusUCrossSliderAxis.dot(this.i1.multiply(this.r1PlusUCrossSliderAxis))
+						+ this.r2CrossSliderAxis.dot(this.i2.multiply(this.r2CrossSliderAxis));
 				this.inverseMassMatrixLimit = (this.inverseMassMatrixLimit > 0.0f) ? 1.0f / this.inverseMassMatrixLimit : 0.0f;
 			}
 			
@@ -580,30 +578,30 @@ public class SliderJoint extends Joint {
 				final float lambdaLowerLimit = this.inverseMassMatrixLimit * (-lowerLimitError);
 				
 				// Compute the impulse P=J^T * lambda for the lower limit raint of body 1
-				final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(-lambdaLowerLimit);
-				final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(-lambdaLowerLimit);
+				final Vector3f linearImpulseBody1Tmp = this.sliderAxisWorld.multiply(-lambdaLowerLimit);
+				final Vector3f angularImpulseBody1Tmp = this.r1PlusUCrossSliderAxis.multiply(-lambdaLowerLimit);
 				
 				// Apply the impulse to the body 1
-				final Vector3f v1_tmp = linearImpulseBody1_tmp.multiplyNew(inverseMassBody1);
-				final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp);
+				final Vector3f v1Tmp = linearImpulseBody1Tmp.multiply(inverseMassBody1);
+				final Vector3f w1Tmp = this.i1.multiply(angularImpulseBody1Tmp);
 				
 				// Update the body position/orientation of body 1
-				x1.add(v1_tmp);
-				q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f));
-				q1.normalize();
+				x1 = x1.add(v1Tmp);
+				q1 = q1.add((new Quaternion(0, w1Tmp)).multiply(q1).multiply(0.5f));
+				q1 = q1.normalize();
 				
 				// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
-				final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(lambdaLowerLimit);
-				final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(lambdaLowerLimit);
+				final Vector3f linearImpulseBody2Tmp = this.sliderAxisWorld.multiply(lambdaLowerLimit);
+				final Vector3f angularImpulseBody2Tmp = this.r2CrossSliderAxis.multiply(lambdaLowerLimit);
 				
 				// Apply the impulse to the body 2
-				final Vector3f v2_tmp = linearImpulseBody2_tmp.multiplyNew(inverseMassBody2);
-				final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp);
+				final Vector3f v2Tmp = linearImpulseBody2Tmp.multiply(inverseMassBody2);
+				final Vector3f w2Tmp = this.i2.multiply(angularImpulseBody2Tmp);
 				
 				// Update the body position/orientation of body 2
-				x2.add(v2_tmp);
-				q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f));
-				q2.normalize();
+				x2 = x2.add(v2Tmp);
+				q2 = q2.add((new Quaternion(0, w2Tmp)).multiply(q2).multiply(0.5f));
+				q2 = q2.normalize();
 			}
 			
 			// If the upper limit is violated
@@ -613,30 +611,30 @@ public class SliderJoint extends Joint {
 				final float lambdaUpperLimit = this.inverseMassMatrixLimit * (-upperLimitError);
 				
 				// Compute the impulse P=J^T * lambda for the upper limit raint of body 1
-				final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(lambdaUpperLimit);
-				final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(lambdaUpperLimit);
+				final Vector3f linearImpulseBody1Tmp = this.sliderAxisWorld.multiply(lambdaUpperLimit);
+				final Vector3f angularImpulseBody1Tmp = this.r1PlusUCrossSliderAxis.multiply(lambdaUpperLimit);
 				
 				// Apply the impulse to the body 1
-				final Vector3f v1_tmp = linearImpulseBody1_tmp.multiplyNew(inverseMassBody1);
-				final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp);
+				final Vector3f v1Tmp = linearImpulseBody1Tmp.multiply(inverseMassBody1);
+				final Vector3f w1Tmp = this.i1.multiply(angularImpulseBody1Tmp);
 				
 				// Update the body position/orientation of body 1
-				x1.add(v1_tmp);
-				q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f));
-				q1.normalize();
+				x1 = x1.add(v1Tmp);
+				q1 = q1.add((new Quaternion(0, w1Tmp)).multiply(q1).multiply(0.5f));
+				q1 = q1.normalize();
 				
 				// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
-				final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(-lambdaUpperLimit);
-				final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(-lambdaUpperLimit);
+				final Vector3f linearImpulseBody2Tmp = this.sliderAxisWorld.multiply(-lambdaUpperLimit);
+				final Vector3f angularImpulseBody2Tmp = this.r2CrossSliderAxis.multiply(-lambdaUpperLimit);
 				
 				// Apply the impulse to the body 2
-				final Vector3f v2_tmp = linearImpulseBody2_tmp.multiplyNew(inverseMassBody2);
-				final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp);
+				final Vector3f v2Tmp = linearImpulseBody2Tmp.multiply(inverseMassBody2);
+				final Vector3f w2Tmp = this.i2.multiply(angularImpulseBody2Tmp);
 				
 				// Update the body position/orientation of body 2
-				x2.add(v2_tmp);
-				q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f));
-				q2.normalize();
+				x2 = x2.add(v2Tmp);
+				q2 = q2.add((new Quaternion(0, w2Tmp)).multiply(q2).multiply(0.5f));
+				q2 = q2.normalize();
 			}
 		}
 	}
@@ -645,10 +643,10 @@ public class SliderJoint extends Joint {
 	public void solveVelocityConstraint(final ConstraintSolverData raintSolverData) {
 		
 		// Get the velocities
-		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
 		
 		// Get the inverse mass and inverse inertia tensors of the bodies
 		final float inverseMassBody1 = this.body1.massInverse;
@@ -657,51 +655,51 @@ public class SliderJoint extends Joint {
 		// --------------- Translation Constraints --------------- //
 		
 		// Compute J*v for the 2 translation raints
-		final float el1 = -this.N1.dot(v1) - w1.dot(this.R1PlusUCrossN1) + this.N1.dot(v2) + w2.dot(this.R2CrossN1);
-		final float el2 = -this.N2.dot(v1) - w1.dot(this.R1PlusUCrossN2) + this.N2.dot(v2) + w2.dot(this.R2CrossN2);
+		final float el1 = -this.n1.dot(v1) - w1.dot(this.r1PlusUCrossN1) + this.n1.dot(v2) + w2.dot(this.r2CrossN1);
+		final float el2 = -this.n2.dot(v1) - w1.dot(this.r1PlusUCrossN2) + this.n2.dot(v2) + w2.dot(this.r2CrossN2);
 		
-		final Vector2f JvTranslation = new Vector2f(el1, el2);
+		final Vector2f jvTranslation = new Vector2f(el1, el2);
 		
 		// Compute the Lagrange multiplier lambda for the 2 translation raints
-		final Vector2f deltaLambda = this.inverseMassMatrixTranslationConstraint.multiplyNew(JvTranslation.multiplyNew(-1).less(this.bTranslation));
-		this.impulseTranslation.add(deltaLambda);
+		final Vector2f deltaLambda = this.inverseMassMatrixTranslationConstraint.multiplyNew(jvTranslation.multiply(-1).less(this.bTranslation));
+		this.impulseTranslation = this.impulseTranslation.add(deltaLambda);
 		
 		// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
-		final Vector3f linearImpulseBody1 = this.N1.multiplyNew(-1).multiply(deltaLambda.x).less(this.N2.multiplyNew(deltaLambda.y));
-		Vector3f angularImpulseBody1 = this.R1PlusUCrossN1.multiplyNew(-1).multiply(deltaLambda.x).less(this.R1PlusUCrossN2.multiplyNew(deltaLambda.y));
+		final Vector3f linearImpulseBody1 = this.n1.multiply(-1).multiply(deltaLambda.x()).less(this.n2.multiply(deltaLambda.y()));
+		Vector3f angularImpulseBody1 = this.r1PlusUCrossN1.multiply(-1).multiply(deltaLambda.x()).less(this.r1PlusUCrossN2.multiply(deltaLambda.y()));
 		
 		// Apply the impulse to the body 1
-		v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		v1 = v1.add(linearImpulseBody1.multiply(inverseMassBody1));
+		w1 = w1.add(this.i1.multiply(angularImpulseBody1));
 		
 		// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
-		final Vector3f linearImpulseBody2 = this.N1.multiplyNew(deltaLambda.x).add(this.N2.multiplyNew(deltaLambda.y));
-		Vector3f angularImpulseBody2 = this.R2CrossN1.multiplyNew(deltaLambda.x).add(this.R2CrossN2.multiplyNew(deltaLambda.y));
+		final Vector3f linearImpulseBody2 = this.n1.multiply(deltaLambda.x()).add(this.n2.multiply(deltaLambda.y()));
+		Vector3f angularImpulseBody2 = this.r2CrossN1.multiply(deltaLambda.x()).add(this.r2CrossN2.multiply(deltaLambda.y()));
 		
 		// Apply the impulse to the body 2
-		v2.add(linearImpulseBody2.multiplyNew(inverseMassBody2));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		v2 = v2.add(linearImpulseBody2.multiply(inverseMassBody2));
+		w2 = w2.add(this.i2.multiply(angularImpulseBody2));
 		
 		// --------------- Rotation Constraints --------------- //
 		
 		// Compute J*v for the 3 rotation raints
-		final Vector3f JvRotation = w2.lessNew(w1);
+		final Vector3f jvRotation = w2.less(w1);
 		
 		// Compute the Lagrange multiplier lambda for the 3 rotation raints
-		final Vector3f deltaLambda2 = this.inverseMassMatrixRotationConstraint.multiplyNew(JvRotation.multiplyNew(-1).less(this.bRotation));
-		this.impulseRotation.add(deltaLambda2);
+		final Vector3f deltaLambda2 = this.inverseMassMatrixRotationConstraint.multiply(jvRotation.multiply(-1).less(this.bRotation));
+		this.impulseRotation = this.impulseRotation.add(deltaLambda2);
 		
 		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
-		angularImpulseBody1 = deltaLambda2.multiplyNew(-1);
+		angularImpulseBody1 = deltaLambda2.multiply(-1);
 		
 		// Apply the impulse to the body to body 1
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		w1 = w1.add(this.i1.multiply(angularImpulseBody1));
 		
 		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2
 		angularImpulseBody2 = deltaLambda2;
 		
 		// Apply the impulse to the body 2
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		w2 = w2.add(this.i2.multiply(angularImpulseBody2));
 		
 		// --------------- Limits Constraints --------------- //
 		
@@ -711,58 +709,58 @@ public class SliderJoint extends Joint {
 			if (this.isLowerLimitViolated) {
 				
 				// Compute J*v for the lower limit raint
-				final float JvLowerLimit = this.sliderAxisWorld.dot(v2) + this.R2CrossSliderAxis.dot(w2) - this.sliderAxisWorld.dot(v1) - this.R1PlusUCrossSliderAxis.dot(w1);
+				final float jvLowerLimit = this.sliderAxisWorld.dot(v2) + this.r2CrossSliderAxis.dot(w2) - this.sliderAxisWorld.dot(v1) - this.r1PlusUCrossSliderAxis.dot(w1);
 				
 				// Compute the Lagrange multiplier lambda for the lower limit raint
-				float deltaLambdaLower = this.inverseMassMatrixLimit * (-JvLowerLimit - this.bLowerLimit);
+				float deltaLambdaLower = this.inverseMassMatrixLimit * (-jvLowerLimit - this.bLowerLimit);
 				final float lambdaTemp = this.impulseLowerLimit;
 				this.impulseLowerLimit = FMath.max(this.impulseLowerLimit + deltaLambdaLower, 0.0f);
 				deltaLambdaLower = this.impulseLowerLimit - lambdaTemp;
 				
 				// Compute the impulse P=J^T * lambda for the lower limit raint of body 1
-				final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(-deltaLambdaLower);
-				final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(-deltaLambdaLower);
+				final Vector3f linearImpulseBody1Tmp = this.sliderAxisWorld.multiply(-deltaLambdaLower);
+				final Vector3f angularImpulseBody1Tmp = this.r1PlusUCrossSliderAxis.multiply(-deltaLambdaLower);
 				
 				// Apply the impulse to the body 1
-				v1.add(linearImpulseBody1_tmp.multiplyNew(inverseMassBody1));
-				w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp));
+				v1 = v1.add(linearImpulseBody1Tmp.multiply(inverseMassBody1));
+				w1 = w1.add(this.i1.multiply(angularImpulseBody1Tmp));
 				
 				// Compute the impulse P=J^T * lambda for the lower limit raint of body 2
-				final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(deltaLambdaLower);
-				final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(deltaLambdaLower);
+				final Vector3f linearImpulseBody2Tmp = this.sliderAxisWorld.multiply(deltaLambdaLower);
+				final Vector3f angularImpulseBody2Tmp = this.r2CrossSliderAxis.multiply(deltaLambdaLower);
 				
 				// Apply the impulse to the body 2
-				v2.add(linearImpulseBody2_tmp.multiplyNew(inverseMassBody2));
-				w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp));
+				v2 = v2.add(linearImpulseBody2Tmp.multiply(inverseMassBody2));
+				w2 = w2.add(this.i2.multiply(angularImpulseBody2Tmp));
 			}
 			
 			// If the upper limit is violated
 			if (this.isUpperLimitViolated) {
 				
 				// Compute J*v for the upper limit raint
-				final float JvUpperLimit = this.sliderAxisWorld.dot(v1) + this.R1PlusUCrossSliderAxis.dot(w1) - this.sliderAxisWorld.dot(v2) - this.R2CrossSliderAxis.dot(w2);
+				final float jvUpperLimit = this.sliderAxisWorld.dot(v1) + this.r1PlusUCrossSliderAxis.dot(w1) - this.sliderAxisWorld.dot(v2) - this.r2CrossSliderAxis.dot(w2);
 				
 				// Compute the Lagrange multiplier lambda for the upper limit raint
-				float deltaLambdaUpper = this.inverseMassMatrixLimit * (-JvUpperLimit - this.bUpperLimit);
+				float deltaLambdaUpper = this.inverseMassMatrixLimit * (-jvUpperLimit - this.bUpperLimit);
 				final float lambdaTemp = this.impulseUpperLimit;
 				this.impulseUpperLimit = FMath.max(this.impulseUpperLimit + deltaLambdaUpper, 0.0f);
 				deltaLambdaUpper = this.impulseUpperLimit - lambdaTemp;
 				
 				// Compute the impulse P=J^T * lambda for the upper limit raint of body 1
-				final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(deltaLambdaUpper);
-				final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(deltaLambdaUpper);
+				final Vector3f linearImpulseBody1Tmp = this.sliderAxisWorld.multiply(deltaLambdaUpper);
+				final Vector3f angularImpulseBody1Tmp = this.r1PlusUCrossSliderAxis.multiply(deltaLambdaUpper);
 				
 				// Apply the impulse to the body 1
-				v1.add(linearImpulseBody1_tmp.multiplyNew(inverseMassBody1));
-				w1.add(this.i1.multiplyNew(angularImpulseBody1_tmp));
+				v1 = v1.add(linearImpulseBody1Tmp.multiply(inverseMassBody1));
+				w1 = w1.add(this.i1.multiply(angularImpulseBody1Tmp));
 				
 				// Compute the impulse P=J^T * lambda for the upper limit raint of body 2
-				final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(-deltaLambdaUpper);
-				final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(-deltaLambdaUpper);
+				final Vector3f linearImpulseBody2Tmp = this.sliderAxisWorld.multiply(-deltaLambdaUpper);
+				final Vector3f angularImpulseBody2Tmp = this.r2CrossSliderAxis.multiply(-deltaLambdaUpper);
 				
 				// Apply the impulse to the body 2
-				v2.add(linearImpulseBody2_tmp.multiplyNew(inverseMassBody2));
-				w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp));
+				v2 = v2.add(linearImpulseBody2Tmp.multiply(inverseMassBody2));
+				w2 = w2.add(this.i2.multiply(angularImpulseBody2Tmp));
 			}
 		}
 		
@@ -771,83 +769,83 @@ public class SliderJoint extends Joint {
 		if (this.isMotorEnabled) {
 			
 			// Compute J*v for the motor
-			final float JvMotor = this.sliderAxisWorld.dot(v1) - this.sliderAxisWorld.dot(v2);
+			final float jvMotor = this.sliderAxisWorld.dot(v1) - this.sliderAxisWorld.dot(v2);
 			
 			// Compute the Lagrange multiplier lambda for the motor
 			final float maxMotorImpulse = this.maxMotorForce * raintSolverData.timeStep;
-			float deltaLambdaMotor = this.inverseMassMatrixMotor * (-JvMotor - this.motorSpeed);
+			float deltaLambdaMotor = this.inverseMassMatrixMotor * (-jvMotor - this.motorSpeed);
 			final float lambdaTemp = this.impulseMotor;
 			this.impulseMotor = FMath.clamp(this.impulseMotor + deltaLambdaMotor, -maxMotorImpulse, maxMotorImpulse);
 			deltaLambdaMotor = this.impulseMotor - lambdaTemp;
 			
 			// Compute the impulse P=J^T * lambda for the motor of body 1
-			final Vector3f linearImpulseBody1_tmp = this.sliderAxisWorld.multiplyNew(deltaLambdaMotor);
+			final Vector3f linearImpulseBody1Tmp = this.sliderAxisWorld.multiply(deltaLambdaMotor);
 			
 			// Apply the impulse to the body 1
-			v1.add(linearImpulseBody1_tmp.multiplyNew(inverseMassBody1));
+			v1 = v1.add(linearImpulseBody1Tmp.multiply(inverseMassBody1));
 			
 			// Compute the impulse P=J^T * lambda for the motor of body 2
-			final Vector3f linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(-deltaLambdaMotor);
+			final Vector3f linearImpulseBody2Tmp = this.sliderAxisWorld.multiply(-deltaLambdaMotor);
 			
 			// Apply the impulse to the body 2
-			v2.add(linearImpulseBody2_tmp.multiplyNew(inverseMassBody2));
+			v2 = v2.add(linearImpulseBody2Tmp.multiply(inverseMassBody2));
 		}
 	}
 	
 	@Override
 	public void warmstart(final ConstraintSolverData raintSolverData) {
 		// Get the velocities
-		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
 		
 		// Get the inverse mass and inverse inertia tensors of the bodies
-		final float inverseMassBody1 = this.body1.massInverse;
-		final float inverseMassBody2 = this.body2.massInverse;
+		float inverseMassBody1 = this.body1.massInverse;
+		float inverseMassBody2 = this.body2.massInverse;
 		
 		// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1
 		final float impulseLimits = this.impulseUpperLimit - this.impulseLowerLimit;
-		final Vector3f linearImpulseLimits = this.sliderAxisWorld.multiplyNew(impulseLimits);
+		final Vector3f linearImpulseLimits = this.sliderAxisWorld.multiply(impulseLimits);
 		
 		// Compute the impulse P=J^T * lambda for the motor raint of body 1
-		final Vector3f impulseMotor = this.sliderAxisWorld.multiplyNew(this.impulseMotor);
+		final Vector3f impulseMotor = this.sliderAxisWorld.multiply(this.impulseMotor);
 		
 		// Compute the impulse P=J^T * lambda for the 2 translation raints of body 1
-		final Vector3f linearImpulseBody1 = this.N1.multiplyNew(-1).multiply(this.impulseTranslation.x).less(this.N2.multiplyNew(this.impulseTranslation.y));
-		final Vector3f angularImpulseBody1 = this.R1PlusUCrossN1.multiplyNew(-1).multiply(this.impulseTranslation.x).less(this.R1PlusUCrossN2.multiplyNew(this.impulseTranslation.y));
+		Vector3f linearImpulseBody1 = this.n1.multiply(-1).multiply(this.impulseTranslation.x()).less(this.n2.multiply(this.impulseTranslation.y()));
+		Vector3f angularImpulseBody1 = this.r1PlusUCrossN1.multiply(-1).multiply(this.impulseTranslation.x()).less(this.r1PlusUCrossN2.multiply(this.impulseTranslation.y()));
 		
 		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 1
-		angularImpulseBody1.add(this.impulseRotation.multiplyNew(-1));
+		angularImpulseBody1 = angularImpulseBody1.add(this.impulseRotation.multiply(-1));
 		
 		// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 1
-		linearImpulseBody1.add(linearImpulseLimits);
-		angularImpulseBody1.add(this.R1PlusUCrossSliderAxis.multiplyNew(impulseLimits));
+		linearImpulseBody1 = linearImpulseBody1.add(linearImpulseLimits);
+		angularImpulseBody1 = angularImpulseBody1.add(this.r1PlusUCrossSliderAxis.multiply(impulseLimits));
 		
 		// Compute the impulse P=J^T * lambda for the motor raint of body 1
-		linearImpulseBody1.add(impulseMotor);
+		linearImpulseBody1 = linearImpulseBody1.add(impulseMotor);
 		
 		// Apply the impulse to the body 1
-		v1.add(linearImpulseBody1.multiplyNew(inverseMassBody1));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		v1 = v1.add(linearImpulseBody1.multiply(inverseMassBody1));
+		w1 = w1.add(this.i1.multiply(angularImpulseBody1));
 		
 		// Compute the impulse P=J^T * lambda for the 2 translation raints of body 2
-		final Vector3f linearImpulseBody2 = this.N1.multiplyNew(this.impulseTranslation.x).add(this.N2.multiplyNew(this.impulseTranslation.y));
-		final Vector3f angularImpulseBody2 = this.R2CrossN1.multiplyNew(this.impulseTranslation.x).add(this.R2CrossN2.multiplyNew(this.impulseTranslation.y));
+		Vector3f linearImpulseBody2 = this.n1.multiply(this.impulseTranslation.x()).add(this.n2.multiply(this.impulseTranslation.y()));
+		Vector3f angularImpulseBody2 = this.r2CrossN1.multiply(this.impulseTranslation.x()).add(this.r2CrossN2.multiply(this.impulseTranslation.y()));
 		
 		// Compute the impulse P=J^T * lambda for the 3 rotation raints of body 2
-		angularImpulseBody2.add(this.impulseRotation);
+		angularImpulseBody2 = angularImpulseBody2.add(this.impulseRotation);
 		
 		// Compute the impulse P=J^T * lambda for the lower and upper limits raints of body 2
-		linearImpulseBody2.less(linearImpulseLimits);
-		angularImpulseBody2.add(this.R2CrossSliderAxis.multiplyNew(-impulseLimits));
+		linearImpulseBody2 = linearImpulseBody2.less(linearImpulseLimits);
+		angularImpulseBody2 = angularImpulseBody2.add(this.r2CrossSliderAxis.multiply(-impulseLimits));
 		
 		// Compute the impulse P=J^T * lambda for the motor raint of body 2
-		linearImpulseBody2.add(impulseMotor.multiplyNew(-1));
+		linearImpulseBody2 = linearImpulseBody2.add(impulseMotor.multiply(-1));
 		
 		// Apply the impulse to the body 2
-		v2.add(linearImpulseBody2.multiplyNew(inverseMassBody2));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		v2 = v2.add(linearImpulseBody2.multiply(inverseMassBody2));
+		w2 = w2.add(this.i2.multiply(angularImpulseBody2));
 	}
 	
 }

src/org/atriasoft/ephysics/constraint/SliderJointInfo.java

@@ -1,8 +1,7 @@
 package org.atriasoft.ephysics.constraint;
 
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.body.RigidBody;
+import org.atriasoft.etk.math.Vector3f;
 
 /**
  * This structure is used to gather the information needed to create a slider
@@ -11,25 +10,25 @@ import org.atriasoft.ephysics.body.RigidBody;
 public class SliderJointInfo extends JointInfo {
 	
 	public Vector3f anchorPointWorldSpace; //!< Anchor point (in world-space coordinates)
-	public Vector3f sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates)
 	public boolean isLimitEnabled; //!< True if the slider limits are enabled
 	public boolean isMotorEnabled; //!< True if the slider motor is enabled
-	public float minTranslationLimit; //!< Mininum allowed translation if limits are enabled
-	public float maxTranslationLimit; //!< Maximum allowed translation if limits are enabled
-	public float motorSpeed; //!< Motor speed
 	public float maxMotorForce; //!< Maximum motor force (in Newtons) that can be applied to reach to desired motor speed
+	public float maxTranslationLimit; //!< Maximum allowed translation if limits are enabled
+	public float minTranslationLimit; //!< Mininum allowed translation if limits are enabled
+	public float motorSpeed; //!< Motor speed
+	public Vector3f sliderAxisWorldSpace; //!< Slider axis (in world-space coordinates)
 	
 	/** 
 		 *  Constructor without limits and without motor
-		 * @param _rigidBody1 The first body of the joint
-		 * @param _rigidBody2 The second body of the joint
-		 * @param _initAnchorPointWorldSpace The initial anchor point in world-space
-		 * @param _initSliderAxisWorldSpace The initial slider axis in world-space
+		 * @param rigidBody1 The first body of the joint
+		 * @param rigidBody2 The second body of the joint
+		 * @param initAnchorPointWorldSpace The initial anchor point in world-space
+		 * @param initSliderAxisWorldSpace The initial slider axis in world-space
 		 */
-	public SliderJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initSliderAxisWorldSpace) {
-		super(_rigidBody1, _rigidBody2, JointType.SLIDERJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
-		this.sliderAxisWorldSpace = _initSliderAxisWorldSpace;
+	public SliderJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace, final Vector3f initSliderAxisWorldSpace) {
+		super(rigidBody1, rigidBody2, JointType.SLIDERJOINT);
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
+		this.sliderAxisWorldSpace = initSliderAxisWorldSpace;
 		this.isLimitEnabled = false;
 		this.isMotorEnabled = false;
 		this.minTranslationLimit = -1.0f;
@@ -41,22 +40,22 @@ public class SliderJointInfo extends JointInfo {
 	
 	/**
 		 *  Constructor with limits and no motor
-		 * @param _rigidBody1 The first body of the joint
-		 * @param _rigidBody2 The second body of the joint
-		 * @param _initAnchorPointWorldSpace The initial anchor point in world-space
-		 * @param _initSliderAxisWorldSpace The initial slider axis in world-space
-		 * @param _initMinTranslationLimit The initial minimum translation limit (in meters)
-		 * @param _initMaxTranslationLimit The initial maximum translation limit (in meters)
+		 * @param rigidBody1 The first body of the joint
+		 * @param rigidBody2 The second body of the joint
+		 * @param initAnchorPointWorldSpace The initial anchor point in world-space
+		 * @param initSliderAxisWorldSpace The initial slider axis in world-space
+		 * @param initMinTranslationLimit The initial minimum translation limit (in meters)
+		 * @param initMaxTranslationLimit The initial maximum translation limit (in meters)
 		 */
-	public SliderJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initSliderAxisWorldSpace,
-			final float _initMinTranslationLimit, final float _initMaxTranslationLimit) {
-		super(_rigidBody1, _rigidBody2, JointType.SLIDERJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
-		this.sliderAxisWorldSpace = _initSliderAxisWorldSpace;
+	public SliderJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace, final Vector3f initSliderAxisWorldSpace,
+			final float initMinTranslationLimit, final float initMaxTranslationLimit) {
+		super(rigidBody1, rigidBody2, JointType.SLIDERJOINT);
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
+		this.sliderAxisWorldSpace = initSliderAxisWorldSpace;
 		this.isLimitEnabled = true;
 		this.isMotorEnabled = false;
-		this.minTranslationLimit = _initMinTranslationLimit;
-		this.maxTranslationLimit = _initMaxTranslationLimit;
+		this.minTranslationLimit = initMinTranslationLimit;
+		this.maxTranslationLimit = initMaxTranslationLimit;
 		this.motorSpeed = 0;
 		this.maxMotorForce = 0;
 		
@@ -64,26 +63,26 @@ public class SliderJointInfo extends JointInfo {
 	
 	/**
 		 *  Constructor with limits and motor
-		 * @param _rigidBody1 The first body of the joint
-		 * @param _rigidBody2 The second body of the joint
-		 * @param _initAnchorPointWorldSpace The initial anchor point in world-space
-		 * @param _initSliderAxisWorldSpace The initial slider axis in world-space
-		 * @param _initMinTranslationLimit The initial minimum translation limit (in meters)
-		 * @param _initMaxTranslationLimit The initial maximum translation limit (in meters)
-		 * @param _initMotorSpeed The initial speed of the joint motor (in meters per second)
-		 * @param _initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters)
+		 * @param rigidBody1 The first body of the joint
+		 * @param rigidBody2 The second body of the joint
+		 * @param initAnchorPointWorldSpace The initial anchor point in world-space
+		 * @param initSliderAxisWorldSpace The initial slider axis in world-space
+		 * @param initMinTranslationLimit The initial minimum translation limit (in meters)
+		 * @param initMaxTranslationLimit The initial maximum translation limit (in meters)
+		 * @param initMotorSpeed The initial speed of the joint motor (in meters per second)
+		 * @param initMaxMotorForce The initial maximum motor force of the joint (in Newtons x meters)
 		 */
-	public SliderJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace, final Vector3f _initSliderAxisWorldSpace,
-			final float _initMinTranslationLimit, final float _initMaxTranslationLimit, final float _initMotorSpeed, final float _initMaxMotorForce) {
-		super(_rigidBody1, _rigidBody2, JointType.SLIDERJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
-		this.sliderAxisWorldSpace = _initSliderAxisWorldSpace;
+	public SliderJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace, final Vector3f initSliderAxisWorldSpace,
+			final float initMinTranslationLimit, final float initMaxTranslationLimit, final float initMotorSpeed, final float initMaxMotorForce) {
+		super(rigidBody1, rigidBody2, JointType.SLIDERJOINT);
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
+		this.sliderAxisWorldSpace = initSliderAxisWorldSpace;
 		this.isLimitEnabled = true;
 		this.isMotorEnabled = true;
-		this.minTranslationLimit = _initMinTranslationLimit;
-		this.maxTranslationLimit = _initMaxTranslationLimit;
-		this.motorSpeed = _initMotorSpeed;
-		this.maxMotorForce = _initMaxMotorForce;
+		this.minTranslationLimit = initMinTranslationLimit;
+		this.maxTranslationLimit = initMaxTranslationLimit;
+		this.motorSpeed = initMotorSpeed;
+		this.maxMotorForce = initMaxMotorForce;
 		
 	}
 }

src/org/atriasoft/ephysics/engine/CollisionCallback.java

@@ -11,7 +11,7 @@ import org.atriasoft.ephysics.constraint.ContactPointInfo;
 public interface CollisionCallback {
 	/**
 	 *  This method will be called for contact.
-	 * @param _contactPointInfo Contact information property.
+	 * @param contactPointInfo Contact information property.
 	 */
-	void notifyContact(ContactPointInfo _contactPointInfo);
+	void notifyContact(ContactPointInfo contactPointInfo);
 }

src/org/atriasoft/ephysics/engine/CollisionWorld.java

@@ -21,12 +21,12 @@ import org.atriasoft.etk.math.Transform3D;
  */
 public class CollisionWorld {
 	
-	public CollisionDetection collisionDetection; //!< Reference to the collision detection
 	protected Set<CollisionBody> bodies = new Set<CollisionBody>(Set.getCollisionBodyCoparator()); //!< All the bodies (rigid and soft) of the world
+	public CollisionDetection collisionDetection; //!< Reference to the collision detection
 	protected int currentBodyID; //!< Current body ID
-	protected List<Integer> freeBodiesIDs = new ArrayList<>(); //!< List of free ID for rigid bodies
 	public EventListener eventListener; //!< Pointer to an event listener object
 	/// Return the next available body ID
+	protected List<Integer> freeBodiesIDs = new ArrayList<>(); //!< List of free ID for rigid bodies
 	
 	/// Constructor
 	public CollisionWorld() {
@@ -106,16 +106,16 @@ public class CollisionWorld {
 	
 	/**
 	 *  Ray cast method
-	 * @param _ray Ray to use for raycasting
-	 * @param _raycastCallback Pointer to the class with the callback method
-	 * @param _raycastWithCategoryMaskBits Bits mask corresponding to the category of bodies to be raycasted
+	 * @param ray Ray to use for raycasting
+	 * @param raycastCallback Pointer to the class with the callback method
+	 * @param raycastWithCategoryMaskBits Bits mask corresponding to the category of bodies to be raycasted
 	 */
-	public void raycast(final Ray _ray, final RaycastCallback _raycastCallback) {
-		raycast(_ray, _raycastCallback, 0xFFFF);
+	public void raycast(final Ray ray, final RaycastCallback raycastCallback) {
+		raycast(ray, raycastCallback, 0xFFFF);
 	}
 	
-	public void raycast(final Ray _ray, final RaycastCallback _raycastCallback, final int _raycastWithCategoryMaskBits) {
-		this.collisionDetection.raycast(_raycastCallback, _ray, _raycastWithCategoryMaskBits);
+	public void raycast(final Ray ray, final RaycastCallback raycastCallback, final int raycastWithCategoryMaskBits) {
+		this.collisionDetection.raycast(raycastCallback, ray, raycastWithCategoryMaskBits);
 	}
 	
 	/// Reset all the contact manifolds linked list of each body
@@ -131,124 +131,124 @@ public class CollisionWorld {
 	 *  Set the collision dispatch configuration
 	 * This can be used to replace default collision detection algorithms by your
 	 * custom algorithm for instance.
-	 * @param _CollisionDispatch Pointer to a collision dispatch object describing
+	 * @param CollisionDispatch Pointer to a collision dispatch object describing
 	 * which collision detection algorithm to use for two given collision shapes
 	 */
-	public void setCollisionDispatch(final CollisionDispatch _collisionDispatch) {
-		this.collisionDetection.setCollisionDispatch(_collisionDispatch);
+	public void setCollisionDispatch(final CollisionDispatch collisionDispatch) {
+		this.collisionDetection.setCollisionDispatch(collisionDispatch);
 	}
 	
 	/**
 	 *  Test if the AABBs of two bodies overlap
-	 * @param _body1 Pointer to the first body to test
-	 * @param _body2 Pointer to the second body to test
+	 * @param body1 Pointer to the first body to test
+	 * @param body2 Pointer to the second body to test
 	 * @return True if the AABBs of the two bodies overlap and false otherwise
 	 */
-	public boolean testAABBOverlap(final CollisionBody _body1, final CollisionBody _body2) {
+	public boolean testAABBOverlap(final CollisionBody body1, final CollisionBody body2) {
 		// If one of the body is not active, we return no overlap
-		if (!_body1.isActive() || !_body2.isActive()) {
+		if (!body1.isActive() || !body2.isActive()) {
 			return false;
 		}
 		// Compute the AABBs of both bodies
-		final AABB body1AABB = _body1.getAABB();
-		final AABB body2AABB = _body2.getAABB();
+		final AABB body1AABB = body1.getAABB();
+		final AABB body2AABB = body2.getAABB();
 		// Return true if the two AABBs overlap
 		return body1AABB.testCollision(body2AABB);
 	}
 	
 	/**
 	 *  Test if the AABBs of two proxy shapes overlap
-	 * @param _shape1 Pointer to the first proxy shape to test
-	 * @param _shape2 Pointer to the second proxy shape to test
+	 * @param shape1 Pointer to the first proxy shape to test
+	 * @param shape2 Pointer to the second proxy shape to test
 	 */
-	public boolean testAABBOverlap(final ProxyShape _shape1, final ProxyShape _shape2) {
-		return this.collisionDetection.testAABBOverlap(_shape1, _shape2);
+	public boolean testAABBOverlap(final ProxyShape shape1, final ProxyShape shape2) {
+		return this.collisionDetection.testAABBOverlap(shape1, shape2);
 	}
 	
 	/**
 	 *  Test and report collisions between two bodies
-	 * @param _body1 Pointer to the first body to test
-	 * @param _body2 Pointer to the second body to test
-	 * @param _callback Pointer to the object with the callback method
+	 * @param body1 Pointer to the first body to test
+	 * @param body2 Pointer to the second body to test
+	 * @param callback Pointer to the object with the callback method
 	 */
-	public void testCollision(final CollisionBody _body1, final CollisionBody _body2, final CollisionCallback _callback) {
+	public void testCollision(final CollisionBody body1, final CollisionBody body2, final CollisionCallback callback) {
 		// Reset all the contact manifolds lists of each body
 		resetContactManifoldListsOfBodies();
 		// Create the sets of shapes
 		final Set<DTree> shapes1 = new Set<DTree>(Set.getDTreeCoparator());
-		for (ProxyShape shape = _body1.getProxyShapesList(); shape != null; shape = shape.getNext()) {
+		for (ProxyShape shape = body1.getProxyShapesList(); shape != null; shape = shape.getNext()) {
 			shapes1.add(shape.broadPhaseID);
 		}
 		final Set<DTree> shapes2 = new Set<DTree>(Set.getDTreeCoparator());
-		for (ProxyShape shape = _body2.getProxyShapesList(); shape != null; shape = shape.getNext()) {
+		for (ProxyShape shape = body2.getProxyShapesList(); shape != null; shape = shape.getNext()) {
 			shapes2.add(shape.broadPhaseID);
 		}
 		// Perform the collision detection and report contacts
-		this.collisionDetection.testCollisionBetweenShapes(_callback, shapes1, shapes2);
+		this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
 	}
 	
 	/**
 	 *  Test and report collisions between a body and all the others bodies of the world.
-	 * @param _body Pointer to the first body to test
-	 * @param _callback Pointer to the object with the callback method
+	 * @param body Pointer to the first body to test
+	 * @param callback Pointer to the object with the callback method
 	 */
-	public void testCollision(final CollisionBody _body, final CollisionCallback _callback) {
+	public void testCollision(final CollisionBody body, final CollisionCallback callback) {
 		// Reset all the contact manifolds lists of each body
 		resetContactManifoldListsOfBodies();
 		// Create the sets of shapes
 		final Set<DTree> shapes1 = new Set<DTree>(Set.getDTreeCoparator());
 		// For each shape of the body
-		for (ProxyShape shape = _body.getProxyShapesList(); shape != null; shape = shape.getNext()) {
+		for (ProxyShape shape = body.getProxyShapesList(); shape != null; shape = shape.getNext()) {
 			shapes1.add(shape.broadPhaseID);
 		}
 		final Set<DTree> emptySet = new Set<DTree>(Set.getDTreeCoparator());
 		// Perform the collision detection and report contacts
-		this.collisionDetection.testCollisionBetweenShapes(_callback, shapes1, emptySet);
+		this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, emptySet);
 	}
 	
 	/**
 	 *  Test and report collisions between all shapes of the world
-	 * @param _callback Pointer to the object with the callback method
+	 * @param callback Pointer to the object with the callback method
 	 */
-	public void testCollision(final CollisionCallback _callback) {
+	public void testCollision(final CollisionCallback callback) {
 		// Reset all the contact manifolds lists of each body
 		resetContactManifoldListsOfBodies();
 		final Set<DTree> emptySet = new Set<DTree>(Set.getDTreeCoparator());
 		// Perform the collision detection and report contacts
-		this.collisionDetection.testCollisionBetweenShapes(_callback, emptySet, emptySet);
+		this.collisionDetection.testCollisionBetweenShapes(callback, emptySet, emptySet);
 	}
 	
 	/**
 	 *  Test and report collisions between a given shape and all the others shapes of the world.
-	 * @param _shape Pointer to the proxy shape to test
-	 * @param _callback Pointer to the object with the callback method
+	 * @param shape Pointer to the proxy shape to test
+	 * @param callback Pointer to the object with the callback method
 	 */
-	public void testCollision(final ProxyShape _shape, final CollisionCallback _callback) {
+	public void testCollision(final ProxyShape shape, final CollisionCallback callback) {
 		// Reset all the contact manifolds lists of each body
 		resetContactManifoldListsOfBodies();
 		// Create the sets of shapes
 		final Set<DTree> shapes = new Set<DTree>(Set.getDTreeCoparator());
-		shapes.add(_shape.broadPhaseID);
+		shapes.add(shape.broadPhaseID);
 		final Set<DTree> emptySet = new Set<DTree>(Set.getDTreeCoparator());
 		// Perform the collision detection and report contacts
-		this.collisionDetection.testCollisionBetweenShapes(_callback, shapes, emptySet);
+		this.collisionDetection.testCollisionBetweenShapes(callback, shapes, emptySet);
 	}
 	
 	/**
 	 * Test and report collisions between two given shapes
-	 * @param _shape1 Pointer to the first proxy shape to test
-	 * @param _shape2 Pointer to the second proxy shape to test
-	 * @param _callback Pointer to the object with the callback method
+	 * @param shape1 Pointer to the first proxy shape to test
+	 * @param shape2 Pointer to the second proxy shape to test
+	 * @param callback Pointer to the object with the callback method
 	 */
-	public void testCollision(final ProxyShape _shape1, final ProxyShape _shape2, final CollisionCallback _callback) {
+	public void testCollision(final ProxyShape shape1, final ProxyShape shape2, final CollisionCallback callback) {
 		// Reset all the contact manifolds lists of each body
 		resetContactManifoldListsOfBodies();
 		// Create the sets of shapes
 		final Set<DTree> shapes1 = new Set<DTree>(Set.getDTreeCoparator());
-		shapes1.add(_shape1.broadPhaseID);
+		shapes1.add(shape1.broadPhaseID);
 		final Set<DTree> shapes2 = new Set<DTree>(Set.getDTreeCoparator());
-		shapes2.add(_shape2.broadPhaseID);
+		shapes2.add(shape2.broadPhaseID);
 		// Perform the collision detection and report contacts
-		this.collisionDetection.testCollisionBetweenShapes(_callback, shapes1, shapes2);
+		this.collisionDetection.testCollisionBetweenShapes(callback, shapes1, shapes2);
 	}
 }

src/org/atriasoft/ephysics/engine/ContactManifoldSolver.java

@@ -12,8 +12,8 @@ public class ContactManifoldSolver {
 	int indexBody2; //!< Index of body 2 in the raint solver
 	float massInverseBody1; //!< Inverse of the mass of body 1
 	float massInverseBody2; //!< Inverse of the mass of body 2
-	Matrix3f inverseInertiaTensorBody1 = new Matrix3f(); //!< Inverse inertia tensor of body 1
-	Matrix3f inverseInertiaTensorBody2 = new Matrix3f(); //!< Inverse inertia tensor of body 2
+	Matrix3f inverseInertiaTensorBody1 = Matrix3f.IDENTITY; //!< Inverse inertia tensor of body 1
+	Matrix3f inverseInertiaTensorBody2 = Matrix3f.IDENTITY; //!< Inverse inertia tensor of body 2
 	ContactPointSolver[] contacts = new ContactPointSolver[ContactManifold.MAX_CONTACT_POINTS_IN_MANIFOLD]; //!< Contact point raints
 	int nbContacts; //!< Number of contact points
 	boolean isBody1DynamicType; //!< True if the body 1 is of type dynamic
@@ -23,27 +23,27 @@ public class ContactManifoldSolver {
 	float rollingResistanceFactor; //!< Rolling resistance factor between the two bodies
 	ContactManifold externalContactManifold; //!< Pointer to the external contact manifold
 	// - Variables used when friction raints are apply at the center of the manifold-//
-	Vector3f normal = new Vector3f(0, 0, 0); //!< Average normal vector of the contact manifold
-	Vector3f frictionPointBody1 = new Vector3f(0, 0, 0); //!< Point on body 1 where to apply the friction raints
-	Vector3f frictionPointBody2 = new Vector3f(0, 0, 0); //!< Point on body 2 where to apply the friction raints
-	Vector3f r1Friction = new Vector3f(0, 0, 0); //!< R1 vector for the friction raints
-	Vector3f r2Friction = new Vector3f(0, 0, 0); //!< R2 vector for the friction raints
-	Vector3f r1CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 1st friction vector
-	Vector3f r1CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 2nd friction vector
-	Vector3f r2CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 1st friction vector
-	Vector3f r2CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 2nd friction vector
+	Vector3f normal = Vector3f.ZERO; //!< Average normal vector of the contact manifold
+	Vector3f frictionPointBody1 = Vector3f.ZERO; //!< Point on body 1 where to apply the friction raints
+	Vector3f frictionPointBody2 = Vector3f.ZERO; //!< Point on body 2 where to apply the friction raints
+	Vector3f r1Friction = Vector3f.ZERO; //!< R1 vector for the friction raints
+	Vector3f r2Friction = Vector3f.ZERO; //!< R2 vector for the friction raints
+	Vector3f r1CrossT1 = Vector3f.ZERO; //!< Cross product of r1 with 1st friction vector
+	Vector3f r1CrossT2 = Vector3f.ZERO; //!< Cross product of r1 with 2nd friction vector
+	Vector3f r2CrossT1 = Vector3f.ZERO; //!< Cross product of r2 with 1st friction vector
+	Vector3f r2CrossT2 = Vector3f.ZERO; //!< Cross product of r2 with 2nd friction vector
 	float inverseFriction1Mass; //!< Matrix K for the first friction raint
 	float inverseFriction2Mass; //!< Matrix K for the second friction raint
 	float inverseTwistFrictionMass; //!< Matrix K for the twist friction raint
-	Matrix3f inverseRollingResistance = new Matrix3f(); //!< Matrix K for the rolling resistance raint
-	Vector3f frictionVector1 = new Vector3f(0, 0, 0); //!< First friction direction at contact manifold center
-	Vector3f frictionvec2 = new Vector3f(0, 0, 0); //!< Second friction direction at contact manifold center
-	Vector3f oldFrictionVector1 = new Vector3f(0, 0, 0); //!< Old 1st friction direction at contact manifold center
-	Vector3f oldFrictionvec2 = new Vector3f(0, 0, 0); //!< Old 2nd friction direction at contact manifold center
+	Matrix3f inverseRollingResistance = Matrix3f.IDENTITY; //!< Matrix K for the rolling resistance raint
+	Vector3f frictionVector1 = Vector3f.ZERO; //!< First friction direction at contact manifold center
+	Vector3f frictionvec2 = Vector3f.ZERO; //!< Second friction direction at contact manifold center
+	Vector3f oldFrictionVector1 = Vector3f.ZERO; //!< Old 1st friction direction at contact manifold center
+	Vector3f oldFrictionvec2 = Vector3f.ZERO; //!< Old 2nd friction direction at contact manifold center
 	float friction1Impulse; //!< First friction direction impulse at manifold center
 	float friction2Impulse; //!< Second friction direction impulse at manifold center
 	float frictionTwistImpulse; //!< Twist friction impulse at contact manifold center
-	Vector3f rollingResistanceImpulse = new Vector3f(0, 0, 0); //!< Rolling resistance impulse
+	Vector3f rollingResistanceImpulse = Vector3f.ZERO; //!< Rolling resistance impulse
 	
 	public ContactManifoldSolver() {
 		for (int iii = 0; iii < ContactManifold.MAX_CONTACT_POINTS_IN_MANIFOLD; iii++) {

src/org/atriasoft/ephysics/engine/ContactPointSolver.java

@@ -1,8 +1,7 @@
 package org.atriasoft.ephysics.engine;
 
-import org.atriasoft.etk.math.Vector3f;
-
 import org.atriasoft.ephysics.constraint.ContactPoint;
+import org.atriasoft.etk.math.Vector3f;
 
 /**
  * Contact solver internal data structure that to store all the
@@ -13,20 +12,20 @@ public class ContactPointSolver {
 	float friction1Impulse; //!< Accumulated impulse in the 1st friction direction
 	float friction2Impulse; //!< Accumulated impulse in the 2nd friction direction
 	float penetrationSplitImpulse; //!< Accumulated split impulse for penetration correction
-	Vector3f rollingResistanceImpulse = new Vector3f(0, 0, 0); //!< Accumulated rolling resistance impulse
-	Vector3f normal = new Vector3f(0, 0, 0); //!< Normal vector of the contact
-	Vector3f frictionVector1 = new Vector3f(0, 0, 0); //!< First friction vector in the tangent plane
-	Vector3f frictionvec2 = new Vector3f(0, 0, 0); //!< Second friction vector in the tangent plane
-	Vector3f oldFrictionVector1 = new Vector3f(0, 0, 0); //!< Old first friction vector in the tangent plane
-	Vector3f oldFrictionvec2 = new Vector3f(0, 0, 0); //!< Old second friction vector in the tangent plane
-	Vector3f r1 = new Vector3f(0, 0, 0); //!< Vector from the body 1 center to the contact point
-	Vector3f r2 = new Vector3f(0, 0, 0); //!< Vector from the body 2 center to the contact point
-	Vector3f r1CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 1st friction vector
-	Vector3f r1CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 2nd friction vector
-	Vector3f r2CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 1st friction vector
-	Vector3f r2CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 2nd friction vector
-	Vector3f r1CrossN = new Vector3f(0, 0, 0); //!< Cross product of r1 with the contact normal
-	Vector3f r2CrossN = new Vector3f(0, 0, 0); //!< Cross product of r2 with the contact normal
+	Vector3f rollingResistanceImpulse = Vector3f.ZERO; //!< Accumulated rolling resistance impulse
+	Vector3f normal = Vector3f.ZERO; //!< Normal vector of the contact
+	Vector3f frictionVector1 = Vector3f.ZERO; //!< First friction vector in the tangent plane
+	Vector3f frictionvec2 = Vector3f.ZERO; //!< Second friction vector in the tangent plane
+	Vector3f oldFrictionVector1 = Vector3f.ZERO; //!< Old first friction vector in the tangent plane
+	Vector3f oldFrictionvec2 = Vector3f.ZERO; //!< Old second friction vector in the tangent plane
+	Vector3f r1 = Vector3f.ZERO; //!< Vector from the body 1 center to the contact point
+	Vector3f r2 = Vector3f.ZERO; //!< Vector from the body 2 center to the contact point
+	Vector3f r1CrossT1 = Vector3f.ZERO; //!< Cross product of r1 with 1st friction vector
+	Vector3f r1CrossT2 = Vector3f.ZERO; //!< Cross product of r1 with 2nd friction vector
+	Vector3f r2CrossT1 = Vector3f.ZERO; //!< Cross product of r2 with 1st friction vector
+	Vector3f r2CrossT2 = Vector3f.ZERO; //!< Cross product of r2 with 2nd friction vector
+	Vector3f r1CrossN = Vector3f.ZERO; //!< Cross product of r1 with the contact normal
+	Vector3f r2CrossN = Vector3f.ZERO; //!< Cross product of r2 with the contact normal
 	float penetrationDepth; //!< Penetration depth
 	float restitutionBias; //!< Velocity restitution bias
 	float inversePenetrationMass; //!< Inverse of the matrix K for the penenetration

src/org/atriasoft/ephysics/engine/Impulse.java

@@ -5,24 +5,12 @@ import org.atriasoft.etk.math.Vector3f;
 /**
  *  Represents an impulse that we can apply to bodies in the contact or raint solver.
  */
-public class Impulse {
-	public final Vector3f linearImpulseBody1; //!< Linear impulse applied to the first body
-	public final Vector3f angularImpulseBody1; //!< Angular impulse applied to the first body
-	public final Vector3f linearImpulseBody2; //!< Linear impulse applied to the second body
-	public final Vector3f angularImpulseBody2; //!< Angular impulse applied to the second body
+@SuppressWarnings("preview")
+public record Impulse(
+		Vector3f linearImpulseBody1, //!< Linear impulse applied to the first body
+		Vector3f angularImpulseBody1, //!< Angular impulse applied to the first body
+		Vector3f linearImpulseBody2, //!< Linear impulse applied to the second body
+		Vector3f angularImpulseBody2 //!< Angular impulse applied to the second body
+) {
 	
-	public Impulse(final Impulse impulse) {
-		this.linearImpulseBody1 = impulse.linearImpulseBody1.clone();
-		this.angularImpulseBody1 = impulse.angularImpulseBody1.clone();
-		this.linearImpulseBody2 = impulse.linearImpulseBody2.clone();
-		this.angularImpulseBody2 = impulse.angularImpulseBody2.clone();
-		
-	}
-	
-	public Impulse(final Vector3f _initLinearImpulseBody1, final Vector3f _initAngularImpulseBody1, final Vector3f _initLinearImpulseBody2, final Vector3f _initAngularImpulseBody2) {
-		this.linearImpulseBody1 = _initLinearImpulseBody1.clone();
-		this.angularImpulseBody1 = _initAngularImpulseBody1.clone();
-		this.linearImpulseBody2 = _initLinearImpulseBody2.clone();
-		this.angularImpulseBody2 = _initAngularImpulseBody2.clone();
-	}
 }

src/org/atriasoft/ephysics/engine/Island.java

@@ -7,96 +7,98 @@ import org.atriasoft.ephysics.body.BodyType;
 import org.atriasoft.ephysics.body.RigidBody;
 import org.atriasoft.ephysics.collision.ContactManifold;
 import org.atriasoft.ephysics.constraint.Joint;
-import org.atriasoft.ephysics.internal.Log;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
 
 /**
  *  An island represent an isolated group of awake bodies that are connected with each other by
  * some contraints (contacts or joints).
  */
 public class Island {
+	static final Logger LOGGER = LoggerFactory.getLogger(Island.class);
 	private final List<RigidBody> bodies = new ArrayList<>(); //!< Array with all the bodies of the island
 	private final List<ContactManifold> contactManifolds = new ArrayList<>(); //!< Array with all the contact manifolds between bodies of the island
 	private final List<Joint> joints = new ArrayList<>(); //!< Array with all the joints between bodies of the island
-	
+
 	/**
 	 *  Constructor
 	 */
 	public Island(final int nbMaxBodies, final int nbMaxContactManifolds, final int nbMaxJoints) {
 		// Allocate memory for the arrays
-		//this.bodies.reserve(_nbMaxBodies);
-		//this.contactManifolds.reserve(_nbMaxContactManifolds);
-		//this.joints.reserve(_nbMaxJoints);
+		//this.bodies.reserve(nbMaxBodies);
+		//this.contactManifolds.reserve(nbMaxContactManifolds);
+		//this.joints.reserve(nbMaxJoints);
 	}
-	
-	/** 
+
+	/**
 		 * Add a body.
 		 */
-	public void addBody(final RigidBody _body) {
-		if (_body.isSleeping() == true) {
-			Log.error("Try to add a body that is sleeping ...");
+	public void addBody(final RigidBody body) {
+		if (body.isSleeping()) {
+			LOGGER.error("Try to add a body that is sleeping ...");
 			return;
 		}
-		this.bodies.add(_body);
+		this.bodies.add(body);
 	}
-	
-	/** 
+
+	/**
 		 * Add a contact manifold.
 		 */
-	public void addContactManifold(final ContactManifold _contactManifold) {
-		this.contactManifolds.add(_contactManifold);
+	public void addContactManifold(final ContactManifold contactManifold) {
+		this.contactManifolds.add(contactManifold);
 	}
-	
-	/** 
+
+	/**
 		 * Add a joint.
 		 */
-	public void addJoint(final Joint _joint) {
-		this.joints.add(_joint);
+	public void addJoint(final Joint joint) {
+		this.joints.add(joint);
 	}
-	
-	/** 
+
+	/**
 	 * Return a pointer to the array of bodies
 	 */
 	public List<RigidBody> getBodies() {
 		return this.bodies;
 	}
-	
-	/** 
+
+	/**
 	 * Return a pointer to the array of contact manifolds
 	 */
 	public List<ContactManifold> getContactManifold() {
 		return this.contactManifolds;
 	}
-	
-	/** 
+
+	/**
 	 * Return a pointer to the array of joints
 	 */
 	public List<Joint> getJoints() {
 		return this.joints;
 	}
-	
-	/** 
+
+	/**
 	 *  Get the number of body
 	 * @return Number of bodies.
 	 */
 	public int getNbBodies() {
 		return this.bodies.size();
 	}
-	
-	/** 
+
+	/**
 	 * @ Get the number of contact manifolds
 	 * Return the number of contact manifolds in the island
 	 */
 	public int getNbContactManifolds() {
 		return this.contactManifolds.size();
 	}
-	
-	/** 
+
+	/**
 	 * Return the number of joints in the island
 	 */
 	public int getNbJoints() {
 		return this.joints.size();
 	}
-	
+
 	/**
 	 *  Reset the isAlreadyIsland variable of the static bodies so that they can also be included in the other islands
 	 */
@@ -107,5 +109,5 @@ public class Island {
 			}
 		}
 	}
-	
+
 }

src/org/atriasoft/ephysics/engine/OverlappingPair.java

@@ -20,7 +20,7 @@ public class OverlappingPair {
 	public static PairInt computeBodiesIndexPair(final CollisionBody body1, final CollisionBody body2) {
 		// Construct the pair of body index
 		final PairInt indexPair = body1.getID() < body2.getID() ? new PairInt(body1.getID(), body2.getID()) : new PairInt(body2.getID(), body1.getID());
-		assert (indexPair.first != indexPair.second);
+		assert (indexPair.first() != indexPair.second());
 		return indexPair;
 	}
 	
@@ -31,10 +31,10 @@ public class OverlappingPair {
 		return new PairDTree(shape1.getBroadPhaseID(), shape2.getBroadPhaseID());
 	}
 	
-	private final ContactManifoldSet contactManifoldSet; //!< Set of persistent contact manifolds
-	
 	private Vector3f cachedSeparatingAxis; //!< Cached previous separating axis
 	
+	private final ContactManifoldSet contactManifoldSet; //!< Set of persistent contact manifolds
+	
 	/// Constructor
 	public OverlappingPair(final ProxyShape shape1, final ProxyShape shape2, final int nbMaxContactManifolds) {
 		this.contactManifoldSet = new ContactManifoldSet(shape1, shape2, nbMaxContactManifolds);