[DEV] update to the records

.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

@@ -5,7 +5,7 @@
 			<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">
+	<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER/org.eclipse.jdt.internal.debug.ui.launcher.StandardVMType/JavaSE-15">
 		<attributes>
 			<attribute name="module" value="true"/>
 		</attributes>

.project

@@ -11,8 +11,14 @@
 			<arguments>
 			</arguments>
 		</buildCommand>
+		<buildCommand>
+			<name>net.sf.eclipsecs.core.CheckstyleBuilder</name>
+			<arguments>
+			</arguments>
+		</buildCommand>
 	</buildSpec>
 	<natures>
 		<nature>org.eclipse.jdt.core.javanature</nature>
+		<nature>net.sf.eclipsecs.core.CheckstyleNature</nature>
 	</natures>
 </projectDescription>

ephysics.iml

@@ -0,0 +1,13 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="JAVA_MODULE" version="4">
+  <component name="NewModuleRootManager" inherit-compiler-output="true">
+    <exclude-output />
+    <content url="file://$MODULE_DIR$">
+      <sourceFolder url="file://$MODULE_DIR$/src" isTestSource="false" />
+    </content>
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+    <orderEntry type="library" scope="TEST" name="org.junit.jupiter:junit-jupiter-api:5.7.1" level="project" />
+    <orderEntry type="module" module-name="etk" />
+  </component>
+</module>

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
+	/// True if the spleeping technique is enabled
-	public static final int DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS = 10;
+	public static final boolean SPLEEPING_ENABLED = true;
 	
-	/// Number of iterations when solving the position raints of the Sequential Impulse technique
+	private Configuration() {}
-	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)
 }

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 worldPoint = Vector3f.ZERO; //!< Hit point in world-space coordinates
-	public Vector3f worldNormal = new Vector3f(); //!< Surface normal at 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) {
+	public RaycastTest(final RaycastCallback callback) {
-		this.userCallback = _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
+	 * Constructor
-	 * @param[in] _id ID of the new body
+	 * @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
+	 * Set whether or not the body is allowed to go to sleep
-	 * @param[in] _isAllowedToSleep True if the body is allowed 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
+	 * Set whether or not the body is active
-	 * @param[in] _isActive True if you want to activate the body
+	 * @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
+	 * Set the variable to know whether or not the body is sleeping
-	 * @param[in] _isSleeping Set the new status
+	 * @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
+	 * Attach user data to this body
-	 * @param[in] _userData A pointer to the user data you want to attach to the 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

@@ -38,59 +38,58 @@ import org.atriasoft.etk.math.Vector3f;
 /**
  * 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;
 	// 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
+	 * Constructor
-	 * @param[in] _transform The transform of the body
+	 * @param transform The transform of the body
-	 * @param[in] _world The physics world where the body is created
+	 * @param world The physics world where the body is created
-	 * @param[in] _id ID of the body
+	 * @param id ID of the body
 	 */
-	public CollisionBody(final Transform3D _transform, final CollisionWorld _world, final int _id) {
+	public CollisionBody(final Transform3D transform, final CollisionWorld world, final int id) {
-		super(_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;
+		this.world = world;
 		
-		//Log.debug("         set transform: " + _transform);
+		//Log.debug("         set transform: " + transform);
-		if (Float.isNaN(_transform.getPosition().x)) {
+		if (Float.isNaN(transform.getPosition().x())) {
-			Log.critical("         set transform: " + _transform);
+			Log.critical("         set transform: " + transform);
 		}
-		if (Float.isInfinite(_transform.getOrientation().z)) {
+		if (Float.isInfinite(transform.getOrientation().z())) {
-			Log.critical("         set transform: " + _transform);
+			Log.critical("         set transform: " + transform);
 		}
 	}
 	
 	/**
-	 * @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 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 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,14 +98,14 @@ 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()) {
@@ -115,7 +114,7 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @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() {
@@ -126,10 +125,10 @@ public class CollisionBody extends Body {
 		//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()));
+		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);
@@ -138,7 +137,7 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @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() {
@@ -146,25 +145,25 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @brief Get the body local-space coordinates of a point given in the world-space coordinates
+	 * Get the body local-space coordinates of a point given in the world-space coordinates
-	 * @param[in] _worldPoint A point in 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) {
+	public Vector3f getLocalPoint(final Vector3f worldPoint) {
-		return this.transform.inverseNew().multiply(_worldPoint);
+		return this.transform.inverseNew().multiply(worldPoint);
 	}
 	
 	/**
-	 * @brief Get the body local-space coordinates of a vector given in the world-space coordinates
+	 * Get the body local-space coordinates of a vector given in the world-space coordinates
-	 * @param[in] _worldVector A vector in 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) {
+	public Vector3f getLocalVector(final Vector3f worldVector) {
-		return this.transform.getOrientation().inverseNew().multiply(_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
 	 */
@@ -173,15 +172,15 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @brief Return the current position and orientation
+	 * 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 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() {
@@ -193,41 +192,41 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @brief Get the world-space coordinates of a point given the local-space coordinates of the body
+	 * 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
+	 * @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) {
+	public Vector3f getWorldPoint(final Vector3f localPoint) {
-		return this.transform.multiplyNew(_localPoint);
+		return this.transform.multiply(localPoint);
 	}
 	
 	/**
-	 * @brief Get the world-space vector of a vector given in local-space coordinates of the body
+	 * 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
+	 * @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) {
+	public Vector3f getWorldVector(final Vector3f localVector) {
-		return this.transform.getOrientation().multiply(_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 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 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) {
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo) {
-		if (this.isActive == false) {
+		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)) {
+			if (shape.raycast(rayTemp, raycastInfo)) {
-				rayTemp.maxFraction = _raycastInfo.hitFraction;
+				rayTemp.maxFraction = raycastInfo.hitFraction;
 				isHit = true;
 			}
 		}
@@ -235,7 +234,7 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @brief Remove all the collision shapes
+	 * Remove all the collision shapes
 	 */
 	public void removeAllCollisionShapes() {
 		ProxyShape current = this.proxyCollisionShapes;
@@ -253,14 +252,14 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @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 +271,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;
@@ -289,7 +288,7 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @brief Reset the contact manifold lists
+	 * Reset the contact manifold lists
 	 */
 	public void resetContactManifoldsList() {
 		// Delete the linked list of contact manifolds of that body
@@ -297,7 +296,7 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @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() {
@@ -314,21 +313,21 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @brief Set whether or not the body is active
+	 * Set whether or not the body is active
-	 * @param[in] _isActive True if you want to activate the body
+	 * @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 {
@@ -340,27 +339,27 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @brief Set the current position and orientation
+	 * 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
+	 * @param transform The transformation of the body that transforms the local-space of the body int32to world-space
 	 */
-	public void setTransform(final Transform3D _transform) {
+	public void setTransform(final Transform3D transform) {
-		//Log.info("         set transform: " + this.transform + " ==> " + _transform);
+		//Log.info("         set transform: " + this.transform + " ==> " + transform);
-		if (Float.isNaN(_transform.getPosition().x)) {
+		if (Float.isNaN(transform.getPosition().x())) {
-			Log.critical("         set transform: " + this.transform + " ==> " + _transform);
+			Log.critical("         set transform: " + this.transform + " ==> " + transform);
 		}
-		if (Float.isInfinite(_transform.getOrientation().z)) {
+		if (Float.isInfinite(transform.getOrientation().z())) {
-			Log.critical("         set transform: " + this.transform + " ==> " + _transform);
+			Log.critical("         set transform: " + this.transform + " ==> " + transform);
 		}
-		this.transform = _transform;
+		this.transform = transform;
 		updateBroadPhaseState();
 	}
 	
 	/**
-	 * @brief Set the type of the body
+	 * Set the type of the body
-	 * @param[in] type The type of the body (STATIC, KINEMATIC, DYNAMIC)
+	 * @param type The type of the body (STATIC, KINEMATIC, DYNAMIC)
 	 */
-	public void setType(final BodyType _type) {
+	public void setType(final BodyType type) {
-		this.type = _type;
+		this.type = type;
 		if (this.type == BodyType.STATIC) {
 			// Update the broad-phase state of the body
 			updateBroadPhaseState();
@@ -368,14 +367,14 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @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;
 			}
 		}
@@ -383,7 +382,7 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @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
@@ -394,11 +393,11 @@ public class CollisionBody extends Body {
 	}
 	
 	/**
-	 * @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()));
+		proxyShape.getCollisionShape().computeAABB(aabb, this.transform.multiply(proxyShape.getLocalToBodyTransform()));
-		this.world.getCollisionDetection().updateProxyCollisionShape(_proxyShape, aabb, new Vector3f(0, 0, 0), _forceReinsert);
+		this.world.getCollisionDetection().updateProxyCollisionShape(proxyShape, aabb, new Vector3f(0, 0, 0), forceReinsert);
 	}
 }

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

@@ -45,22 +45,22 @@ import org.atriasoft.etk.math.Vector3f;
  */
 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
 	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
@@ -70,7 +70,7 @@ 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;
 	}
 	
@@ -80,15 +80,15 @@ public class RigidBody extends CollisionBody {
 	 * 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 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 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 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) {
+	public ProxyShape addCollisionShape(final CollisionShape collisionShape, final Transform3D transform, final float mass) {
-		assert (_mass > 0.0f);
+		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,7 +98,7 @@ 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++;
@@ -114,18 +114,18 @@ 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 force The force to apply on the body
-	 * @param _point The point where the force is applied (in world-space coordinates)
+	 * @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.externalForce = this.externalForce.add(force);
-		this.externalTorque.add(_point.lessNew(this.centerOfMassWorld).cross(_force));
+		this.externalTorque = this.externalTorque.add(point.less(this.centerOfMassWorld).cross(force));
 	}
 	
 	/**
@@ -134,16 +134,16 @@ 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 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);
 	}
 	
 	/**
@@ -152,24 +152,24 @@ public class RigidBody extends CollisionBody {
 	 * 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) {
+	public void enableGravity(final boolean isEnabled) {
-		this.isGravityEnabled = _isEnabled;
+		this.isGravityEnabled = isEnabled;
 	}
 	
 	/**
@@ -190,9 +190,9 @@ public class RigidBody extends CollisionBody {
 	
 	/**
 	 *  Get the inverse of the inertia tensor in world coordinates.
-	 * The inertia tensor I_w in world coordinates is computed with the
+	 * The inertia tensor Iw in world coordinates is computed with the
-	 * local inverse inertia tensor I_b^-1 in body coordinates
+	 * local inverse inertia tensor Ib^-1 in body coordinates
-	 * by I_w = R * I_b^-1 * R^T
+	 * 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
@@ -205,7 +205,7 @@ public class RigidBody extends CollisionBody {
 		//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());
+		final Matrix3f tmp = this.transform.getOrientation().getMatrix().multiply(this.inertiaTensorLocalInverse).multiply(this.transform.getOrientation().getMatrix().transpose());
 		//Log.error("}}}    tmp=" + tmp);
 		return tmp;
 	}
@@ -220,16 +220,16 @@ public class RigidBody extends CollisionBody {
 	
 	/**
 	 *  Get the inertia tensor in world coordinates.
-	 * The inertia tensor I_w in world coordinates is computed
+	 * The inertia tensor Iw in world coordinates is computed
-	 * with the local inertia tensor I_b in body coordinates
+	 * with the local inertia tensor Ib in body coordinates
-	 * by I_w = R * I_b * R^T
+	 * 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());
 	}
 	
 	/**
@@ -290,9 +290,9 @@ public class RigidBody extends CollisionBody {
 	public void recomputeMassInformation() {
 		this.initMass = 0.0f;
 		this.massInverse = 0.0f;
-		this.inertiaTensorLocal.setZero();
+		this.inertiaTensorLocal = Matrix3f.ZERO;
-		this.inertiaTensorLocalInverse.setZero();
+		this.inertiaTensorLocalInverse = Matrix3f.ZERO;
-		this.centerOfMassLocal.setZero();
+		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 +302,7 @@ 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,37 +312,36 @@ public class RigidBody extends CollisionBody {
 		}
 		// Compute the center of mass
 		final Vector3f oldCenterOfMass = this.centerOfMassWorld;
-		this.centerOfMassLocal.multiply(this.massInverse);
+		this.centerOfMassLocal = this.centerOfMassLocal.multiply(this.massInverse);
-		this.centerOfMassWorld = this.transform.multiplyNew(this.centerOfMassLocal);
+		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();
+			Matrix3f inertiaTensor = shape.getCollisionShape().computeLocalInertiaTensor(shape.getMass());
-			shape.getCollisionShape().computeLocalInertiaTensor(inertiaTensor, 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 off1 = offset.multiply(-offset.x());
-			final Vector3f off2 = offset.multiplyNew(-offset.y);
+			final Vector3f off2 = offset.multiply(-offset.y());
-			final Vector3f off3 = offset.multiplyNew(-offset.z);
+			final Vector3f off3 = offset.multiply(-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);
+			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());
+			offsetMatrix = offsetMatrix.multiply(shape.getMass());
-			this.inertiaTensorLocal.add(inertiaTensor.addNew(offsetMatrix));
+			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) {
+	public void removeCollisionShape(final ProxyShape proxyShape) {
-		super.removeCollisionShape(_proxyShape);
+		super.removeCollisionShape(proxyShape);
 		recomputeMassInformation();
 	}
 	
@@ -373,11 +372,11 @@ 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) {
+	public void setAngularDamping(final float angularDamping) {
-		assert (_angularDamping >= 0.0f);
+		assert (angularDamping >= 0.0f);
-		this.angularDamping = _angularDamping;
+		this.angularDamping = angularDamping;
 	}
 	
 	public void setAngularReactionEnable(final boolean angularReactionEnable) {
@@ -386,13 +385,13 @@ public class RigidBody extends CollisionBody {
 	
 	/**
 	*  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);
 		}
@@ -400,7 +399,7 @@ public class RigidBody extends CollisionBody {
 	
 	/**
 	 *  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,55 +407,55 @@ public class RigidBody extends CollisionBody {
 		}
 		final Vector3f oldCenterOfMass = this.centerOfMassWorld;
 		this.centerOfMassLocal = centerOfMassLocal;
-		this.centerOfMassWorld = this.transform.multiplyNew(this.centerOfMassLocal);
+		this.centerOfMassWorld = this.transform.multiply(this.centerOfMassLocal);
-		this.linearVelocity.add(this.angularVelocity.cross(this.centerOfMassWorld.lessNew(oldCenterOfMass)));
+		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) {
+	public void setIsSleeping(final boolean isSleeping) {
-		if (_isSleeping) {
+		if (isSleeping) {
-			this.linearVelocity.setZero();
+			this.linearVelocity = Vector3f.ZERO;
-			this.angularVelocity.setZero();
+			this.angularVelocity = Vector3f.ZERO;
-			this.externalForce.setZero();
+			this.externalForce = Vector3f.ZERO;
-			this.externalTorque.setZero();
+			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) {
+	public void setLinearDamping(final float linearDamping) {
-		assert (_linearDamping >= 0.0f);
+		assert (linearDamping >= 0.0f);
-		this.linearDamping = _linearDamping;
+		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);
 		}
@@ -464,13 +463,13 @@ public class RigidBody extends CollisionBody {
 	
 	/**
 	 *  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 {
@@ -481,67 +480,67 @@ public class RigidBody extends CollisionBody {
 	
 	/**
 	 *  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) {
+	public void setMaterial(final Material material) {
-		this.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) {
+	public void setTransform(final Transform3D transform) {
-		//Log.info("         set transform: " + this.transform + " ==> " + _transform);
+		//Log.info("         set transform: " + this.transform + " ==> " + transform);
-		if (_transform.getPosition().x == Float.NaN) {
+		if (transform.getPosition().x() == Float.NaN) {
-			Log.critical("         set transform: " + this.transform + " ==> " + _transform);
+			Log.critical("         set transform: " + this.transform + " ==> " + transform);
 		}
-		if (Float.isInfinite(_transform.getOrientation().z)) {
+		if (Float.isInfinite(transform.getOrientation().z())) {
-			Log.critical("         set transform: " + this.transform + " ==> " + _transform);
+			Log.critical("         set transform: " + this.transform + " ==> " + transform);
 		}
-		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) {
+	public void setType(final BodyType type) {
-		if (this.type == _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.linearVelocity = Vector3f.ZERO;
-			this.angularVelocity.setZero();
+			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.inertiaTensorLocal = Matrix3f.ZERO;
-			this.inertiaTensorLocalInverse.setZero();
+			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.externalForce = Vector3f.ZERO;
-		this.externalTorque.setZero();
+		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) {
@@ -549,7 +548,7 @@ public class RigidBody extends CollisionBody {
 			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()));
+			shape.getCollisionShape().computeAABB(aabb, this.transform.multiply(shape.getLocalToBodyTransform()));
 			//Log.info("         : " + aabb.getMin() + " " + aabb.getMax());
 			// Update the broad-phase state for the proxy collision shape
 			//Log.warning("            ==> updateProxyCollisionShape");
@@ -562,11 +561,11 @@ public class RigidBody extends CollisionBody {
 	 */
 	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)));
+		this.transform = new Transform3D(this.centerOfMassWorld.less(this.transform.getOrientation().multiply(this.centerOfMassLocal)), this.transform.getOrientation());
-		if (Float.isNaN(this.transform.getPosition().x) == true) {
+		if (Float.isNaN(this.transform.getPosition().x())) {
 			Log.critical("updateTransformWithCenterOfMass: " + this.transform);
 		}
-		if (Float.isInfinite(this.transform.getOrientation().z) == true) {
+		if (Float.isInfinite(this.transform.getOrientation().z())) {
 			Log.critical("         set transform: " + this.transform);
 		}
 	}

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

@@ -33,31 +33,31 @@ import org.atriasoft.ephysics.mathematics.SetMultiple;
 import org.atriasoft.etk.math.Vector3f;
 
 /*
- * @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;
 	/// 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
+	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) {
+	public CollisionDetection(final CollisionWorld world) {
-		this.world = _world;
+		this.world = world;
 		this.broadPhaseAlgorithm = new BroadPhaseAlgorithm(this);
 		this.isCollisionShapesAdded = false;
 		this.narrowPhaseGJKAlgorithm = new GJKAlgorithm(this);
@@ -71,7 +71,7 @@ public class CollisionDetection implements NarrowPhaseCallback {
 	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);
 		}
@@ -79,11 +79,11 @@ public class CollisionDetection implements NarrowPhaseCallback {
 	
 	/// 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) {
+	private void addContactManifoldToBody(final OverlappingPair pair) {
-		assert (_pair != null);
+		assert (pair != null);
-		final CollisionBody body1 = _pair.getShape1().getBody();
+		final CollisionBody body1 = pair.getShape1().getBody();
-		final CollisionBody body2 = _pair.getShape2().getBody();
+		final CollisionBody body2 = pair.getShape2().getBody();
-		final ContactManifoldSet manifoldSet = _pair.getContactManifoldSet();
+		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);
@@ -100,53 +100,53 @@ public class CollisionDetection implements NarrowPhaseCallback {
 	}
 	
 	/// Add a pair of bodies that cannot collide with each other
-	public void addNoCollisionPair(final CollisionBody _body1, final CollisionBody _body2) {
+	public void addNoCollisionPair(final CollisionBody body1, final CollisionBody body2) {
-		this.noCollisionPairs.add(OverlappingPair.computeBodiesIndexPair(_body1, _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) {
+	public void askForBroadPhaseCollisionCheck(final ProxyShape shape) {
-		this.broadPhaseAlgorithm.addMovedCollisionShape(_shape.broadPhaseID);
+		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) {
+	public void broadPhaseNotifyOverlappingPair(final ProxyShape shape1, final ProxyShape shape2) {
-		assert (_shape1.broadPhaseID != _shape2.broadPhaseID);
+		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());
+		//Log.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) {
+		if ((shape1.getCollideWithMaskBits() & shape2.getCollisionCategoryBits()) == 0 || (shape1.getCollisionCategoryBits() & shape2.getCollideWithMaskBits()) == 0) {
 			Log.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());
+		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
+		// Create the overlapping pair and add it int32to the set of overlapping pairs
-		final OverlappingPair newPair = new OverlappingPair(_shape1, _shape2, nbMaxManifolds);
+		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);
+		shape1.getBody().setIsSleeping(false);
-		_shape2.getBody().setIsSleeping(false);
+		shape2.getBody().setIsSleeping(false);
 	}
 	
 	/// Delete all the contact points in the currently overlapping pairs
@@ -250,7 +250,7 @@ public class CollisionDetection implements NarrowPhaseCallback {
 	}
 	
 	/// 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,14 +262,14 @@ 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)
+			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)) {
+					&& (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
@@ -313,7 +313,7 @@ public class CollisionDetection implements NarrowPhaseCallback {
 			
 			final CollisionShapeInfo shape2Info = new CollisionShapeInfo(shape2, shape2.getCollisionShape(), shape2.getLocalToWorldTransform(), pair, shape2.getCachedCollisionData());
 			
-			final TestCollisionBetweenShapesCallback narrowPhaseCallback = new TestCollisionBetweenShapesCallback(_callback);
+			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);
@@ -323,14 +323,14 @@ public class CollisionDetection implements NarrowPhaseCallback {
 		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);
+		overlappingPair.addContact(contact);
-		// Add the overlapping pair int32_to the set of pairs in contact during narrow-phase
+		// Add the overlapping pair int32to the set of pairs in contact during narrow-phase
-		final PairDTree pairId = OverlappingPair.computeID(_overlappingPair.getShape1(), _overlappingPair.getShape2());
+		final PairDTree pairId = OverlappingPair.computeID(overlappingPair.getShape1(), overlappingPair.getShape2());
-		this.contactOverlappingPair.put(pairId, _overlappingPair);
+		this.contactOverlappingPair.put(pairId, overlappingPair);
 	}
 	
 	/// Fill-in the collision detection matrix
@@ -344,8 +344,8 @@ public class CollisionDetection implements NarrowPhaseCallback {
 	}
 	
 	/// Return the Narrow-phase collision detection algorithm to use between two types of shapes
-	public NarrowPhaseAlgorithm getCollisionAlgorithm(final CollisionShapeType _shape1Type, final CollisionShapeType _shape2Type) {
+	public NarrowPhaseAlgorithm getCollisionAlgorithm(final CollisionShapeType shape1Type, final CollisionShapeType shape2Type) {
-		return this.collisionMatrix[_shape1Type.value][_shape2Type.value];
+		return this.collisionMatrix[shape1Type.value][shape2Type.value];
 	}
 	
 	public GJKAlgorithm getNarrowPhaseGJKAlgorithm() {
@@ -364,55 +364,55 @@ public class CollisionDetection implements NarrowPhaseCallback {
 	
 	/// 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) {
 		Log.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) {
+	public void raycast(final RaycastCallback raycastCallback, final Ray ray, final int raycastWithCategoryMaskBits) {
-		final RaycastTest rayCastTest = new RaycastTest(_raycastCallback);
+		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) {
+	public void removeNoCollisionPair(final CollisionBody body1, final CollisionBody body2) {
-		this.noCollisionPairs.erase(this.noCollisionPairs.find(OverlappingPair.computeBodiesIndexPair(_body1, _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) {
+	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();
@@ -424,14 +424,14 @@ 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)
+			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)) {
+					&& (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
@@ -446,8 +446,8 @@ public class CollisionDetection implements NarrowPhaseCallback {
 							manifold.getShape2().getCollisionShape(), contactPoint.getNormal(), contactPoint.getPenetrationDepth(), contactPoint.getLocalPointOnBody1(),
 							contactPoint.getLocalPointOnBody2());
 					// Notify the collision callback about this new contact
-					if (_callback != null) {
+					if (callback != null) {
-						_callback.notifyContact(contactInfo);
+						callback.notifyContact(contactInfo);
 					}
 				}
 				
@@ -456,42 +456,42 @@ public class CollisionDetection implements NarrowPhaseCallback {
 	}
 	
 	/// Set the collision dispatch configuration
-	public void setCollisionDispatch(final CollisionDispatch _collisionDispatch) {
+	public void setCollisionDispatch(final CollisionDispatch collisionDispatch) {
-		this.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) {
+	public void updateProxyCollisionShape(final ProxyShape shape, final AABB aabb) {
-		updateProxyCollisionShape(_shape, _aabb, new Vector3f(0, 0, 0), false);
+		updateProxyCollisionShape(shape, aabb, new Vector3f(0, 0, 0), false);
 	}
 	
-	public void updateProxyCollisionShape(final ProxyShape _shape, final AABB _aabb, final Vector3f _displacement) {
+	public void updateProxyCollisionShape(final ProxyShape shape, final AABB aabb, final Vector3f displacement) {
-		updateProxyCollisionShape(_shape, _aabb, _displacement, false);
+		updateProxyCollisionShape(shape, aabb, displacement, false);
 	}
 	
-	public void updateProxyCollisionShape(final ProxyShape _shape, final AABB _aabb, final Vector3f _displacement, final boolean _forceReinsert) {
+	public void updateProxyCollisionShape(final ProxyShape shape, final AABB aabb, final Vector3f displacement, final boolean forceReinsert) {
-		this.broadPhaseAlgorithm.updateProxyCollisionShape(_shape, _aabb, _displacement);
+		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(
-	public final OverlappingPair overlappingPair; //!< Broadphase overlapping pair
+		ProxyShape proxyShape, //!< Proxy shape
-	public final ProxyShape proxyShape; //!< Proxy shape
+		CollisionShape collisionShape, //!< Pointer to the collision shape
-	public final CollisionShape collisionShape; //!< Pointer to the collision shape
+		Transform3D shapeToWorldTransform, //!< Transform3D that maps from collision shape local-space to world-space
-	public final Transform3D shapeToWorldTransform; //!< Transform3D that maps from collision shape local-space to world-space
+		OverlappingPair overlappingPair, //!< Broadphase overlapping pair
-	public final Object cachedCollisionData; //!< Cached collision data of the proxy shape
+		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) {
+	private final int normalDirectionId; //!< Normal direction Id (Unique Id representing the normal direction)
-		this.shape1 = _shape1;
+	private Vector3f rollingResistanceImpulse; //!< Accumulated rolling resistance impulse
-		this.shape2 = _shape2;
+	private final ProxyShape shape1; //!< Pointer to the first proxy shape of the contact
-		this.normalDirectionId = _normalDirectionId;
+	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 vector1 = newPoint.less(this.contactPoints[1].getLocalPointOnBody1());
-			final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().lessNew(this.contactPoints[2].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 vector1 = newPoint.less(this.contactPoints[0].getLocalPointOnBody1());
-			final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().lessNew(this.contactPoints[2].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 vector1 = newPoint.less(this.contactPoints[0].getLocalPointOnBody1());
-			final Vector3f vector2 = this.contactPoints[3].getLocalPointOnBody1().lessNew(this.contactPoints[1].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 vector1 = newPoint.less(this.contactPoints[0].getLocalPointOnBody1());
-			final Vector3f vector2 = this.contactPoints[2].getLocalPointOnBody1().lessNew(this.contactPoints[1].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) {
+			if (area1 < area3) {
 				return 3;
-			} else {
+			}
 			return 1;
 		}
-		} else if (area0 < area2) {
+		if (area0 < area2) {
 			if (area2 < area3) {
 				return 3;
-			} else {
+			}
 			return 2;
 		}
-		} else if (area0 < area3) {
+		if (area0 < area3) {
 			return 3;
-		} else {
-			return 0;
 		}
+		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].setWorldPointOnBody1(transform1.multiply(this.contactPoints[iii].getLocalPointOnBody1()));
-			this.contactPoints[iii].setWorldPointOnBody2(transform2.multiplyNew(this.contactPoints[iii].getLocalPointOnBody2()));
+			this.contactPoints[iii].setWorldPointOnBody2(transform2.multiply(this.contactPoints[iii].getLocalPointOnBody2()));
-			this.contactPoints[iii]
+			this.contactPoints[iii].setPenetrationDepth((this.contactPoints[iii].getWorldPointOnBody1().less(this.contactPoints[iii].getWorldPointOnBody2())).dot(this.contactPoints[iii].getNormal()));
-					.setPenetrationDepth((this.contactPoints[iii].getWorldPointOnBody1().lessNew(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 projOfPoint1 = this.contactPoints[iii].getNormal().multiply(distanceNormal).add(this.contactPoints[iii].getWorldPointOnBody1());
-				final Vector3f projDifference = this.contactPoints[iii].getWorldPointOnBody2().lessNew(projOfPoint1);
+				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) {
+	public ContactManifoldListElement(final ContactManifold initContactManifold, final ContactManifoldListElement initNext) {
-		this.contactManifold = _initContactManifold;
+		this.contactManifold = initContactManifold;
-		this.next = _initNext;
+		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) {
+	public ContactManifoldSet(final ProxyShape shape1, final ProxyShape shape2, final int nbMaxManifolds) {
-		this.nbMaxManifolds = _nbMaxManifolds;
+		this.nbMaxManifolds = nbMaxManifolds;
 		this.nbManifolds = 0;
-		this.shape1 = _shape1;
+		this.shape1 = shape1;
-		this.shape2 = _shape2;
+		this.shape2 = shape2;
-		assert (_nbMaxManifolds >= 1);
+		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 float max = FMath.max(FMath.abs(normal.x()), FMath.abs(normal.y()), FMath.abs(normal.z()));
-		final Vector3f normalScaled = normal.divideNew(max);
+		final Vector3f normalScaled = normal.divide(max);
-		if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
+		if (normalScaled.x() >= normalScaled.y() && normalScaled.x() >= normalScaled.z()) {
-			faceNo = normalScaled.x > 0 ? 0 : 1;
+			faceNo = normalScaled.x() > 0 ? 0 : 1;
-			u = normalScaled.y;
+			u = normalScaled.y();
-			v = normalScaled.z;
+			v = normalScaled.z();
-		} else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) {
+		} else if (normalScaled.y() >= normalScaled.x() && normalScaled.y() >= normalScaled.z()) {
-			faceNo = normalScaled.y > 0 ? 2 : 3;
+			faceNo = normalScaled.y() > 0 ? 2 : 3;
-			u = normalScaled.x;
+			u = normalScaled.x();
-			v = normalScaled.z;
+			v = normalScaled.z();
 		} else {
-			faceNo = normalScaled.z > 0 ? 4 : 5;
+			faceNo = normalScaled.z() > 0 ? 4 : 5;
-			u = normalScaled.x;
+			u = normalScaled.x();
-			v = normalScaled.y;
+			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.manifolds[iii].update(this.shape1.getBody().getTransform().multiply(this.shape1.getLocalToBodyTransform()),
-					this.shape2.getBody().getTransform().multiplyNew(this.shape2.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) {
+	public TestCollisionBetweenShapesCallback(final CollisionCallback callback) {
-		this.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) {
+	@Override
-		this.collisionCallback.notifyContact(_contactInfo);
+	public void notifyContact(final OverlappingPair overlappingPair, final ContactPointInfo contactInfo) {
+		this.collisionCallback.notifyContact(contactInfo);
 	}
 }

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

@@ -5,8 +5,8 @@ import org.atriasoft.etk.math.Vector3f;
 public class Triangle {
 	public final Vector3f[] value = new Vector3f[3];
 	
-	public Vector3f get(final int _id) {
+	public Vector3f get(final int id) {
-		return this.value[_id];
+		return this.value[id];
 	}
 	
 }

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) {
+	public void addSubpart(final TriangleVertexArray triangleVertexArray) {
-		this.triangleArrays.add(_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) {
+	public TriangleVertexArray getSubpart(final int indexSubpart) {
-		assert (_indexSubpart < this.triangleArrays.size());
+		assert (indexSubpart < this.triangleArrays.size());
-		return this.triangleArrays.get(_indexSubpart);
+		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) {
+	public TriangleVertexArray(final List<Vector3f> vertices, final List<Integer> triangles) {
-		this.vertices = _vertices.toArray(new Vector3f[0]);
+		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 vertices List Of all vertices
-	 * @param _triangles List of all linked points
+	 * @param triangles List of all linked points
 	 */
-	public TriangleVertexArray(final Vector3f[] _vertices, final int[] _triangles) {
+	public TriangleVertexArray(final Vector3f[] vertices, final int[] triangles) {
-		this.vertices = _vertices;
+		this.vertices = vertices;
-		this.triangles = _triangles;
+		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[0] = this.vertices[this.triangles[id * 3]];
-		out.value[1] = this.vertices[this.triangles[_id * 3 + 1]];
+		out.value[1] = this.vertices[this.triangles[id * 3 + 1]];
-		out.value[2] = this.vertices[this.triangles[_id * 3 + 2]];
+		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) {
+		public BroadPhaseRaycastCallback(final DynamicAABBTree dynamicAABBTree, final int raycastWithCategoryMaskBits, final RaycastTest raycastTest) {
-			this.dynamicAABBTree = _dynamicAABBTree;
+			this.dynamicAABBTree = dynamicAABBTree;
-			this.raycastWithCategoryMaskBits = _raycastWithCategoryMaskBits;
+			this.raycastWithCategoryMaskBits = raycastWithCategoryMaskBits;
-			this.raycastTest = _raycastTest;
+			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) {
+	public void addMovedCollisionShape(final DTree broadPhaseID) {
-		//Log.info("  ** Element that has moved ... = " + _broadPhaseID);
+		//Log.info("  ** Element that has moved ... = " + broadPhaseID);
-		this.movedShapes.add(_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
@@ -137,13 +137,13 @@ 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));
 				}
 			});
 		}
@@ -153,15 +153,15 @@ public class BroadPhaseAlgorithm {
 		// 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) {
+			public int compare(final PairDTree pair1, final PairDTree pair2) {
-				if (_pair1.first.uid < _pair2.first.uid) {
+				if (pair1.first().uid < pair2.first().uid) {
 					return -1;
 				}
-				if (_pair1.first.uid == _pair2.first.uid) {
+				if (pair1.first().uid == pair2.first().uid) {
-					if (_pair1.second.uid < _pair2.second.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 shape1 = (ProxyShape) (this.dynamicAABBTree.getNodeDataPointer(pair.first()));
-			final ProxyShape shape2 = (ProxyShape) (this.dynamicAABBTree.getNodeDataPointer(pair.second));
+			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) {
+	public void raycast(final Ray ray, final RaycastTest raycastTest, final int raycastWithCategoryMaskBits) {
-		final BroadPhaseRaycastCallback broadPhaseRaycastCallback = new BroadPhaseRaycastCallback(this.dynamicAABBTree, _raycastWithCategoryMaskBits, _raycastTest);
+		final BroadPhaseRaycastCallback broadPhaseRaycastCallback = new BroadPhaseRaycastCallback(this.dynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
-		this.dynamicAABBTree.raycast(_ray, broadPhaseRaycastCallback);
+		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) {
+	public void removeProxyCollisionShape(final ProxyShape proxyShape) {
-		final DTree broadPhaseID = _proxyShape.getBroadPhaseID();
+		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,26 +224,26 @@ public class BroadPhaseAlgorithm {
 	}
 	
 	/// Return true if the two broad-phase collision shapes are overlapping
-	public boolean testOverlappingShapes(final ProxyShape _shape1, final ProxyShape _shape2) {
+	public boolean testOverlappingShapes(final ProxyShape shape1, final ProxyShape shape2) {
-		if (_shape1 == _shape2) {
+		if (shape1 == shape2) {
 			return false;
 		}
 		// Get the two AABBs of the collision shapes
-		final AABB aabb1 = this.dynamicAABBTree.getFatAABB(_shape1.getBroadPhaseID());
+		final AABB aabb1 = this.dynamicAABBTree.getFatAABB(shape1.getBroadPhaseID());
-		final AABB aabb2 = this.dynamicAABBTree.getFatAABB(_shape2.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) {
+	public void updateProxyCollisionShape(final ProxyShape proxyShape, final AABB aabb, final Vector3f displacement) {
-		updateProxyCollisionShape(_proxyShape, _aabb, _displacement, false);
+		updateProxyCollisionShape(proxyShape, aabb, displacement, false);
 	}
 	
-	public void updateProxyCollisionShape(final ProxyShape _proxyShape, final AABB _aabb, final Vector3f _displacement, final boolean _forceReinsert) {
+	public void updateProxyCollisionShape(final ProxyShape proxyShape, final AABB aabb, final Vector3f displacement, final boolean forceReinsert) {
-		final DTree broadPhaseID = _proxyShape.getBroadPhaseID();
+		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);
 		// longo the tree).
@@ -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 dataInt0 = 0;
-	int dataInt_1 = 0;
+	int dataInt1 = 0;
 	
-	public DTreeLeafInt(final int dataInt_0, final int dataInt_1) {
+	public DTreeLeafInt(final int dataInt0, final int dataInt1) {
-		super();
+		this.dataInt0 = dataInt0;
-		this.dataInt_0 = dataInt_0;
+		this.dataInt1 = dataInt1;
-		this.dataInt_1 = dataInt_1;
 	}
 	
 	@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 childrenleft = null; //!< Left child of the node
-	DTree children_right = null; //!< Right child of the node
+	DTree childrenright = null; //!< Right child of the node
 }

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

@@ -19,11 +19,11 @@ import org.atriasoft.etk.math.Vector3f;
  */
 public class DynamicAABBTree {
 	
-	private DTree rootNode; //!< Pointer to the memory location of the nodes of the tree
-	
 	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 +51,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.setMin(aabb.getMin().less(this.extraAABBGap));
-		leafNode.aabb.setMax(aabb.getMax().addNew(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 +61,35 @@ public class DynamicAABBTree {
 	}
 	
 	/// Balance the sub-tree of a given node using left or right rotations.
-	private DTree balanceSubTreeAtNode(final DTree _node) {
+	private DTree balanceSubTreeAtNode(final DTree node) {
-		assert (_node != null);
+		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 nodeB = nodeA.childrenleft;
-		final DTree nodeC = nodeA.children_right;
+		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 nodeF = nodeCTree.childrenleft;
-			final DTree nodeG = nodeCTree.children_right;
+			final DTree nodeG = nodeCTree.childrenright;
-			nodeCTree.children_left = _node;
+			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) {
+				if (nodeCParent.childrenleft == node) {
-					nodeCParent.children_left = nodeC;
+					nodeCParent.childrenleft = nodeC;
 				} else {
-					assert (nodeCParent.children_right == _node);
+					assert (nodeCParent.childrenright == node);
-					nodeCParent.children_right = nodeC;
+					nodeCParent.childrenright = nodeC;
 				}
 			} else {
 				this.rootNode = nodeC;
@@ -98,9 +98,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;
+				nodeCTree.childrenright = nodeF;
-				nodeA.children_right = nodeG;
+				nodeA.childrenright = nodeG;
-				nodeG.parent = new WeakReference<DTree>(_node);
+				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 +111,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;
+				nodeCTree.childrenright = nodeG;
-				nodeA.children_right = nodeF;
+				nodeA.childrenright = nodeF;
-				nodeF.parent = new WeakReference<DTree>(_node);
+				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 +130,18 @@ public class DynamicAABBTree {
 		if (balanceFactor < -1) {
 			assert (!nodeB.isLeaf());
 			final DTreeNode nodeBTree = (DTreeNode) nodeB;
-			final DTree nodeF = nodeBTree.children_left;
+			final DTree nodeF = nodeBTree.childrenleft;
-			final DTree nodeG = nodeBTree.children_right;
+			final DTree nodeG = nodeBTree.childrenright;
-			nodeBTree.children_left = _node;
+			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) {
+				if (nodeBParent.childrenleft == node) {
-					nodeBParent.children_left = nodeB;
+					nodeBParent.childrenleft = nodeB;
 				} else {
-					assert (nodeBParent.children_right == _node);
+					assert (nodeBParent.childrenright == node);
-					nodeBParent.children_right = nodeB;
+					nodeBParent.childrenright = nodeB;
 				}
 			} else {
 				this.rootNode = nodeB;
@@ -150,9 +150,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;
+				nodeBTree.childrenright = nodeF;
-				nodeA.children_left = nodeG;
+				nodeA.childrenleft = nodeG;
-				nodeG.parent = new WeakReference<DTree>(_node);
+				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 +163,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;
+				nodeBTree.childrenright = nodeG;
-				nodeA.children_left = nodeF;
+				nodeA.childrenleft = nodeF;
-				nodeF.parent = new WeakReference<DTree>(_node);
+				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 +179,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 +188,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 leftHeight = computeHeight(nodeTree.childrenleft);
-		final int rightHeight = computeHeight(nodeTree.children_right);
+		final int rightHeight = computeHeight(nodeTree.childrenright);
 		// Return the height of the node
 		return 1 + Math.max(leftHeight, rightHeight);
 	}
@@ -207,14 +207,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 +234,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 leftChild = node.childrenleft;
-			final DTree rightChild = node.children_right;
+			final DTree rightChild = node.childrenright;
 			// Compute the merged AABB
 			final float volumeAABB = currentNode.aabb.getVolume();
 			final AABB mergedAABBs = new AABB();
@@ -299,20 +299,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) {
+			if (parentNode.childrenleft == siblingNode) {
-				parentNode.children_left = newParentNode;
+				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.childrenleft = siblingNode;
-		newParentNode.children_right = _node;
+		newParentNode.childrenright = inNode;
-		siblingNode.parent = new WeakReference<DTree>(newParentNode);
+		siblingNode.parent = new WeakReference<>(newParentNode);
-		_node.parent = new WeakReference<DTree>(newParentNode);
+		inNode.parent = new WeakReference<>(newParentNode);
 		
 		// Move up in the tree to change the AABBs that have changed
 		currentNode = newParentNode;
@@ -320,11 +320,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 leftChild = nodeDouble.childrenleft;
-			final DTree rightChild = nodeDouble.children_right;
+			final DTree rightChild = nodeDouble.childrenright;
 			assert (leftChild != null);
 			assert (rightChild != null);
 			// Recompute the height of the node in the tree
@@ -338,18 +338,18 @@ public class DynamicAABBTree {
 				currentNode = null;
 			}
 		}
-		assert (_node.isLeaf());
+		assert (inNode.isLeaf());
 	}
 	
 	/// Ray casting method
-	public void raycast(final Ray _ray, final CallbackRaycast _callback) {
+	public void raycast(final Ray ray, final CallbackRaycast callback) {
-		if (_callback == null) {
+		if (callback == null) {
 			Log.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 +360,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 +387,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.childrenleft);
-				stack.push(tmpNode.children_right);
+				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) {
+	private void removeLeafNode(final DTree node) {
-		assert (_node.isLeaf());
+		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) {
+		if (parentNode.childrenleft == node) {
-			siblingNode = parentNode.children_right;
+			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 +426,10 @@ public class DynamicAABBTree {
 		} else {
 			final DTreeNode grandParentNode = (DTreeNode) grandParentNodeWeak.get();
 			// Destroy the parent node
-			if (grandParentNode.children_left == parentNode) {
+			if (grandParentNode.childrenleft == parentNode) {
-				grandParentNode.children_left = siblingNode;
+				grandParentNode.childrenleft = siblingNode;
 			} else {
-				grandParentNode.children_right = siblingNode;
+				grandParentNode.childrenright = siblingNode;
 			}
 			siblingNode.parent = parentNode.parent;
 			releaseNode(parentNode);
@@ -442,8 +442,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 leftChild = currentTreeNode.childrenleft;
-				final DTree rightChild = currentTreeNode.children_right;
+				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,22 +458,22 @@ public class DynamicAABBTree {
 	}
 	
 	/// Remove an object from the tree
-	public void removeObject(final DTree _node) {
+	public void removeObject(final DTree node) {
-		assert (_node.isLeaf());
+		assert (node.isLeaf());
 		// Remove the node from the tree
-		removeLeafNode(_node);
+		removeLeafNode(node);
-		releaseNode(_node);
+		releaseNode(node);
 	}
 	
 	/// Report all shapes overlapping with the AABB given in parameter.
-	public void reportAllShapesOverlappingWithAABB(final AABB _aabb, final CallbackOverlapping _callback) {
+	public void reportAllShapesOverlappingWithAABB(final AABB aabb, final CallbackOverlapping callback) {
-		if (_callback == null) {
+		if (callback == null) {
 			Log.error("call with null callback");
 			return;
 		}
 		//Log.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);
@@ -489,24 +489,24 @@ public class DynamicAABBTree {
 			final DTree nodeToVisit = nodeIDToVisit;
 			//Log.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) {
+					if (aabb != nodeToVisit.aabb) {
 						Log.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.childrenleft);
-					stack.push(tmp.children_right);
+					stack.push(tmp.childrenright);
-					//Log.error("    add stack: " + tmp.children_left);
+					//Log.error("    add stack: " + tmp.childrenleft);
-					//Log.error("    add stack: " + tmp.children_right);
+					//Log.error("    add stack: " + tmp.childrenright);
 				}
 			}
 		}
@@ -525,51 +525,59 @@ 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) {
+	public boolean updateObject(final DTree node, final AABB newAABB, final Vector3f displacement) {
-		return updateObject(_node, _newAABB, _displacement, false);
+		return updateObject(node, newAABB, displacement, false);
 	}
 	
-	public boolean updateObject(final DTree _node, final AABB _newAABB, final Vector3f _displacement, final boolean _forceReinsert) {
+	public boolean updateObject(final DTree node, final AABB newAABB, final Vector3f displacement, final boolean forceReinsert) {
-		assert (_node.isLeaf());
+		assert (node.isLeaf());
-		assert (_node.height >= 0);
+		assert (node.height >= 0);
-		//Log.verbose(" compare : " + _node.aabb.getMin() + " " + _node.aabb.getMax());
+		//Log.verbose(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax());
-		//Log.verbose("         : " + _newAABB.getMin() + " " + _newAABB.getMax());
+		//Log.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.setMin(node.aabb.getMin().less(gap));
-		_node.aabb.getMax().add(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) {
+		float xmin = node.aabb.getMin().x();
-			_node.aabb.getMin().setX(_node.aabb.getMin().x + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.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) {
+		if (displacement.y() < 0.0f) {
-			_node.aabb.getMin().setY(_node.aabb.getMin().y + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.y);
+			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) {
+		if (displacement.z() < 0.0f) {
-			_node.aabb.getMin().setZ(_node.aabb.getMin().z + Configuration.DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER * _displacement.z);
+			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());
+		node.aabb.setMin(new Vector3f(xmin, ymin, zmin));
-		//Log.error("         : " + _newAABB.getMin() + " " + _newAABB.getMax());
+		node.aabb.setMax(new Vector3f(xmax, ymax, zmax));
-		if (_node.aabb.contains(_newAABB) == false) {
+		//Log.error(" compare : " + node.aabb.getMin() + " " + node.aabb.getMax());
+		//Log.error("         : " + newAABB.getMin() + " " + newAABB.getMax());
+		if (!node.aabb.contains(newAABB)) {
 			//Log.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,10 @@ package org.atriasoft.ephysics.collision.broadphase;
 
 import java.util.Set;
 
-import javafx.collections.transformation.SortedList;
+@SuppressWarnings("preview")
-
+public record PairDTree(
-public class 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 +20,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 +31,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 ProxyShape concaveProxyShape; //!< Proxy shape of the concave collision shape
-		protected ConvexShape convexShape; //!< Convex collision shape to test collision with
 		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) {
+		public void setCollisionDetection(final CollisionDetection collisionDetection) {
-			this.collisionDetection = _collisionDetection;
+			this.collisionDetection = collisionDetection;
 		}
 		
 		/// Set the concave collision shape
-		public void setConcaveShape(final ConcaveShape _concaveShape) {
+		public void setConcaveShape(final ConcaveShape concaveShape) {
-			this.concaveShape = _concaveShape;
+			this.concaveShape = concaveShape;
 		}
 		
 		/// Set the convex collision shape to test collision with
-		public void setConvexShape(final ConvexShape _convexShape) {
+		public void setConvexShape(final ConvexShape convexShape) {
-			this.convexShape = _convexShape;
+			this.convexShape = convexShape;
 		}
 		
 		/// Set the narrow-phase collision callback
-		public void setNarrowPhaseCallback(final NarrowPhaseCallback _callback) {
+		public void setNarrowPhaseCallback(final NarrowPhaseCallback callback) {
-			this.narrowPhaseCallback = _callback;
+			this.narrowPhaseCallback = callback;
 		}
 		
 		/// Set the broadphase overlapping pair
-		public void setOverlappingPair(final OverlappingPair _overlappingPair) {
+		public void setOverlappingPair(final OverlappingPair overlappingPair) {
-			this.overlappingPair = _overlappingPair;
+			this.overlappingPair = overlappingPair;
 		}
 		
 		/// Set the proxy shapes of the two collision shapes
-		public void setProxyShapes(final ProxyShape _convexProxyShape, final ProxyShape _concaveProxyShape) {
+		public void setProxyShapes(final ProxyShape convexProxyShape, final ProxyShape concaveProxyShape) {
-			this.convexProxyShape = _convexProxyShape;
+			this.convexProxyShape = convexProxyShape;
-			this.concaveProxyShape = _concaveProxyShape;
+			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) {
+		public SmoothCollisionNarrowPhaseCallback(final List<SmoothMeshContactInfo> contactPoints) {
-			this.contactPoints = new ArrayList<>(_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) {
+			if (contactInfo.collisionShape1.getType() == CollisionShapeType.TRIANGLE) {
-				assert (_contactInfo.collisionShape2.getType() != CollisionShapeType.TRIANGLE);
+				assert (contactInfo.collisionShape2.getType() != CollisionShapeType.TRIANGLE);
-				final TriangleShape triangleShape = (TriangleShape) _contactInfo.collisionShape1;
+				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);
+				assert (contactInfo.collisionShape2.getType() == CollisionShapeType.TRIANGLE);
-				final TriangleShape triangleShape = (TriangleShape) _contactInfo.collisionShape2;
+				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,
+		public SmoothMeshContactInfo(final ContactPointInfo contact, final boolean firstShapeTriangle, final Vector3f trianglePoint1, final Vector3f trianglePoint2, final Vector3f trianglePoint3) {
-				final Vector3f _trianglePoint3) {
+			this.contactInfo = new ContactPointInfo(contact);
-			this.contactInfo = new ContactPointInfo(_contact);
+			this.isFirstShapeTriangle = firstShapeTriangle;
-			this.isFirstShapeTriangle = _firstShapeTriangle;
+			this.triangleVertices[0] = trianglePoint1;
-			this.triangleVertices[0] = _trianglePoint1;
+			this.triangleVertices[1] = trianglePoint2;
-			this.triangleVertices[1] = _trianglePoint2;
+			this.triangleVertices[2] = trianglePoint3;
-			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) {
+	protected void addProcessedVertex(final List<PairIntVector3f> processTriangleVertices, final Vector3f vertex) {
-		_processTriangleVertices.add(new PairIntVector3f((int) (_vertex.x * _vertex.y * _vertex.z), _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);
+		int key = int(vertex.x * vertex.y * vertex.z);
-		auto range = _processTriangleVertices.equal_range(key);
+		auto range = processTriangleVertices.equalrange(key);
 		for (auto it = range.first; it != range.second; ++it) {
-			if (    _vertex.x == it.second.x
+			if (    vertex.x == it.second.x
-			     && _vertex.y == it.second.y
+			     && vertex.y == it.second.y
-			     && _vertex.z == it.second.z) {
+			     && vertex.z == it.second.z) {
 				return true;
 			}
 		}
 		return false;
 		*/
 		// TODO : This is not really the same ...
-		for (final PairIntVector3f it : _processTriangleVertices) {
+		for (final PairIntVector3f it : processTriangleVertices) {
-			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;
 			}
 		}
@@ -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) {
+			public int compare(final SmoothMeshContactInfo pair1, final SmoothMeshContactInfo pair2) {
-				if (_pair1.contactInfo.penetrationDepth < _pair2.contactInfo.penetrationDepth) {
+				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 isUZero = Mathematics.approxEqual(u, 0.0f, 0.0001f);
-			final boolean isVZero = Mathematics.ApproxEqual(v, 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 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();
+					firstShape = overlappingPair.getShape1();
-					secondShape = _overlappingPair.getShape2();
+					secondShape = overlappingPair.getShape2();
 				} else {
-					firstShape = _overlappingPair.getShape2();
+					firstShape = overlappingPair.getShape2();
-					secondShape = _overlappingPair.getShape1();
+					secondShape = overlappingPair.getShape1();
 				}
 				// We use the triangle normal as the contact normal
-				final Vector3f a = info.triangleVertices[1].lessNew(info.triangleVertices[0]);
+				final Vector3f a = info.triangleVertices[1].less(info.triangleVertices[0]);
-				final Vector3f b = info.triangleVertices[2].lessNew(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);
+				final Vector3f firstWorldPoint = firstShape.getLocalToWorldTransform().multiply(firstLocalPoint);
-				newContactInfo.normal.normalize();
+				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);
+					final Vector3f newSecondWorldPoint = firstWorldPoint.add(newContactInfo.normal);
-					newContactInfo.localPoint2 = worldToLocalSecondPoint.multiplyNew(newSecondWorldPoint);
+					newContactInfo.localPoint2 = worldToLocalSecondPoint.multiply(newSecondWorldPoint);
 				} else {
-					final Vector3f newSecondWorldPoint = firstWorldPoint.lessNew(newContactInfo.normal);
+					final Vector3f newSecondWorldPoint = firstWorldPoint.less(newContactInfo.normal);
-					newContactInfo.localPoint1 = worldToLocalSecondPoint.multiplyNew(newSecondWorldPoint);
+					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()) {
+		if (shape1Info.collisionShape().isConvex()) {
-			convexProxyShape = _shape1Info.proxyShape;
+			convexProxyShape = shape1Info.proxyShape();
-			convexShape = (ConvexShape) _shape1Info.collisionShape;
+			convexShape = (ConvexShape) shape1Info.collisionShape();
-			concaveProxyShape = _shape2Info.proxyShape;
+			concaveProxyShape = shape2Info.proxyShape();
-			concaveShape = (ConcaveShape) _shape2Info.collisionShape;
+			concaveShape = (ConcaveShape) shape2Info.collisionShape();
 		} else {
 			// Collision shape 2 is convex, collision shape 1 is concave
-			convexProxyShape = _shape2Info.proxyShape;
+			convexProxyShape = shape2Info.proxyShape();
-			convexShape = (ConvexShape) _shape2Info.collisionShape;
+			convexShape = (ConvexShape) shape2Info.collisionShape();
-			concaveProxyShape = _shape1Info.proxyShape;
+			concaveProxyShape = shape1Info.proxyShape();
-			concaveShape = (ConcaveShape) _shape1Info.collisionShape;
+			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.sphereVsSphereAlgorithm = new SphereVsSphereAlgorithm(collisionDetection);
-		this.GJKAlgorithm = new GJKAlgorithm(_collisionDetection);
+		this.gjkAlgorithm = new GJKAlgorithm(collisionDetection);
-		this.concaveVsConvexAlgorithm = new ConcaveVsConvexAlgorithm(_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,20 +33,20 @@ 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) {
+	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);
+		////Log.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.isClosestPointInternalToTriangle(): " + triangle.isClosestPointInternalToTriangle());
-		////Log.info("    _triangle.getDistSquare(): " + _triangle.getDistSquare());
+		////Log.info("    triangle.getDistSquare(): " + triangle.getDistSquare());
-		if (_triangle.isClosestPointInternalToTriangle() && _triangle.getDistSquare() <= _upperBoundSquarePenDepth) {
+		if (triangle.isClosestPointInternalToTriangle() && triangle.getDistSquare() <= upperBoundSquarePenDepth) {
 			// Add the triangle face to the list of candidates
-			_heap.add(_triangle);
+			heap.add(triangle);
 			////Log.info("add in heap:");
 			//int iii = 0;
-			//for (final TriangleEPA elem : _heap) {
+			//for (final TriangleEPA elem : heap) {
 			//	////Log.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,
+	public Vector3f computePenetrationDepthAndContactPoints(final Simplex simplex, final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info,
-			final Transform3D _transform2, final Vector3f _vector, final NarrowPhaseCallback _narrowPhaseCallback) {
+			final Transform3D transform2, Vector3f vector, final NarrowPhaseCallback narrowPhaseCallback) {
 		////Log.info("computePenetrationDepthAndContactPoints()");
-		assert (_shape1Info.collisionShape.isConvex());
+		assert (shape1Info.collisionShape().isConvex());
-		assert (_shape2Info.collisionShape.isConvex());
+		assert (shape2Info.collisionShape().isConvex());
-		final ConvexShape shape1 = (ConvexShape) (_shape1Info.collisionShape);
+		final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape());
-		final ConvexShape shape2 = (ConvexShape) (_shape2Info.collisionShape);
+		final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape());
-		final CacheData shape1CachedCollisionData = (CacheData) _shape1Info.cachedCollisionData;
+		final CacheData shape1CachedCollisionData = (CacheData) shape1Info.cachedCollisionData();
-		final CacheData shape2CachedCollisionData = (CacheData) _shape2Info.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[] 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[] 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 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) {
+			public int compare(final TriangleEPA face1, final TriangleEPA face2) {
-				final float d1 = _face1.getDistSquare();
+				final float d1 = face1.getDistSquare();
-				final float d2 = _face2.getDistSquare();
+				final float d2 = face2.getDistSquare();
 				if (d1 < d2) {
 					return -1;
 				}
@@ -91,14 +91,14 @@ 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
@@ -110,7 +110,7 @@ public class EPAAlgorithm {
 				// 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));
+				suppPointsB[2] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v1.multiply(-1)), shape2CachedCollisionData));
-				points[2] = suppPointsA[2].lessNew(suppPointsB[2]);
+				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));
+				suppPointsB[3] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v2.multiply(-1)), shape2CachedCollisionData));
-				points[3] = suppPointsA[3].lessNew(suppPointsB[3]);
+				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));
+				suppPointsB[4] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v3.multiply(-1)), shape2CachedCollisionData));
-				points[4] = suppPointsA[4].lessNew(suppPointsB[4]);
+				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;
@@ -190,7 +190,7 @@ public class EPAAlgorithm {
 					// 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,8 +224,8 @@ 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 v1 = points[1].less(points[0]);
-				final Vector3f v2 = points[2].lessNew(points[0]);
+				final Vector3f v2 = points[2].less(points[0]);
 				final Vector3f n = v1.cross(v2);
 				////Log.info(">>>>>>>>>>>>>>>>>>");
 				////Log.info("    v1 = " + v1);
@@ -233,14 +233,14 @@ public class EPAAlgorithm {
 				////Log.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));
+				suppPointsB[3] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n.multiply(-1)), shape2CachedCollisionData));
-				points[3] = suppPointsA[3].lessNew(suppPointsB[3]);
+				points[3] = suppPointsA[3].less(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);
+				suppPointsA[4] = shape1.getLocalSupportPointWithMargin(n.multiply(-1), shape1CachedCollisionData);
-				suppPointsB[4] = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n), shape2CachedCollisionData));
+				suppPointsB[4] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(n), shape2CachedCollisionData));
-				points[4] = suppPointsA[4].lessNew(suppPointsB[4]);
+				points[4] = suppPointsA[4].less(suppPointsB[4]);
 				////Log.info("    suppPointsA[4]= " + suppPointsA[4]);
 				////Log.info("    suppPointsB[4]= " + suppPointsB[4]);
 				////Log.info("    points[4]= " + points[4]);
@@ -274,12 +274,12 @@ public class EPAAlgorithm {
 					////Log.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,11 +296,13 @@ 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;
@@ -323,9 +325,8 @@ 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
+				suppPointsB[nbVertices] = body2Tobody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(triangle.getClosestPoint().multiply(-1)), shape2CachedCollisionData));
-						.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(triangle.getClosestPoint().multiplyNew(-1)), shape2CachedCollisionData));
+				points[nbVertices] = suppPointsA[nbVertices].less(suppPointsB[nbVertices]);
-				points[nbVertices] = suppPointsA[nbVertices].lessNew(suppPointsB[nbVertices]);
 				final int indexNewVertex = nbVertices;
 				nbVertices++;
 				// Update the upper bound of the penetration depth
@@ -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());
+		final Vector3f tmp = transform1.getOrientation().multiply(triangle.getClosestPoint());
-		_vector.set(tmp);
+		vector = tmp;
 		final Vector3f pALocal = triangle.computeClosestPointOfObject(suppPointsA);
 		final Vector3f pBLocal = body2Tobody1.inverseNew().multiply(triangle.computeClosestPointOfObject(suppPointsB));
-		final Vector3f normal = _vector.safeNormalizeNew();
+		final Vector3f normal = vector.safeNormalize();
-		final float penetrationDepth = _vector.length();
+		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,29 +391,29 @@ 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) {
+	private int isOriginInTetrahedron(final Vector3f p1, final Vector3f p2, final Vector3f p3, final Vector3f p4) {
-		////Log.error("isOriginInTetrahedron(" + _p1 + ", " + _p2 + ", " + _p3 + ", " + _p4 + ")");
+		////Log.error("isOriginInTetrahedron(" + p1 + ", " + p2 + ", " + p3 + ", " + p4 + ")");
 		// Check vertex 1
-		final Vector3f normal1 = _p2.lessNew(_p1).cross(_p3.lessNew(_p1));
+		final Vector3f normal1 = p2.less(p1).cross(p3.less(p1));
-		if ((normal1.dot(_p1) > 0.0f) == (normal1.dot(_p4) > 0.0)) {
+		if ((normal1.dot(p1) > 0.0f) == (normal1.dot(p4) > 0.0)) {
 			////Log.error("    ==> 4");
 			return 4;
 		}
 		// Check vertex 2
-		final Vector3f normal2 = _p4.lessNew(_p2).cross(_p3.lessNew(_p2));
+		final Vector3f normal2 = p4.less(p2).cross(p3.less(p2));
-		if ((normal2.dot(_p2) > 0.0f) == (normal2.dot(_p1) > 0.0)) {
+		if ((normal2.dot(p2) > 0.0f) == (normal2.dot(p1) > 0.0)) {
 			////Log.error("    ==> 1");
 			return 1;
 		}
 		// Check vertex 3
-		final Vector3f normal3 = _p4.lessNew(_p3).cross(_p1.lessNew(_p3));
+		final Vector3f normal3 = p4.less(p3).cross(p1.less(p3));
-		if ((normal3.dot(_p3) > 0.0f) == (normal3.dot(_p2) > 0.0)) {
+		if ((normal3.dot(p3) > 0.0f) == (normal3.dot(p2) > 0.0)) {
 			////Log.error("    ==> 2");
 			return 2;
 		}
 		// Check vertex 4
-		final Vector3f normal4 = _p2.lessNew(_p4).cross(_p1.lessNew(_p4));
+		final Vector3f normal4 = p2.less(p4).cross(p1.less(p4));
-		if ((normal4.dot(_p4) > 0.0f) == (normal4.dot(_p3) > 0.0)) {
+		if ((normal4.dot(p4) > 0.0f) == (normal4.dot(p3) > 0.0)) {
 			////Log.error("    ==> 3");
 			return 3;
 		}

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) {
@@ -76,11 +76,11 @@ 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)) {
+			if (!this.ownerTriangle.getAdjacentEdge(EdgeEPA.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());
@@ -90,7 +90,7 @@ public class EdgeEPA implements Cloneable {
 					return true;
 				}
 				return false;
-				} else if (!this.ownerTriangle.getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
+			} else if (!this.ownerTriangle.getAdjacentEdge(EdgeEPA.indexOfPreviousCounterClockwiseEdge(this.index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
 				this.ownerTriangle.setIsObsolete(false);
 				triangleStore.resize(backup);
 				//Log.error("EdgeEPA 3 befor call: " + indexNewVertex + ", " + getTargetVertexIndex() + ", " + getSourceVertexIndex());
@@ -102,7 +102,6 @@ public class EdgeEPA implements Cloneable {
 				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) {
+	public static void halfLink(final EdgeEPA edge0, final EdgeEPA edge1) {
-		assert (_edge0.getSourceVertexIndex() == _edge1.getTargetVertexIndex() && _edge0.getTargetVertexIndex() == _edge1.getSourceVertexIndex());
+		assert (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
-		_edge0.getOwnerTriangle().adjacentEdges[_edge0.getIndex()] = _edge1;
+		edge0.getOwnerTriangle().adjacentEdges[edge0.getIndex()] = edge1;
 	}
 	
-	public static boolean link(final EdgeEPA _edge0, final EdgeEPA _edge1) {
+	public static boolean link(final EdgeEPA edge0, final EdgeEPA edge1) {
-		if (_edge0.getSourceVertexIndex() == _edge1.getTargetVertexIndex() && _edge0.getTargetVertexIndex() == _edge1.getSourceVertexIndex()) {
+		if (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() && edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex()) {
-			_edge0.getOwnerTriangle().adjacentEdges[_edge0.getIndex()] = _edge1;
+			edge0.getOwnerTriangle().adjacentEdges[edge0.getIndex()] = edge1;
-			_edge1.getOwnerTriangle().adjacentEdges[_edge1.getIndex()] = _edge0;
+			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 determinant; //!< Determinant
-	
-	private float lambda2; //!< Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
-	
 	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[0] = indexVertex1;
-		this.indicesVertices[1] = _indexVertex2;
+		this.indicesVertices[1] = indexVertex2;
-		this.indicesVertices[2] = _indexVertex3;
+		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) {
+	public TriangleEPA(final TriangleEPA triangle) {
-		this.indicesVertices[0] = _triangle.indicesVertices[0];
+		this.indicesVertices[0] = triangle.indicesVertices[0];
-		this.indicesVertices[1] = _triangle.indicesVertices[1];
+		this.indicesVertices[1] = triangle.indicesVertices[1];
-		this.indicesVertices[2] = _triangle.indicesVertices[2];
+		this.indicesVertices[2] = triangle.indicesVertices[2];
-		this.adjacentEdges[0] = _triangle.adjacentEdges[0];
+		this.adjacentEdges[0] = triangle.adjacentEdges[0];
-		this.adjacentEdges[1] = _triangle.adjacentEdges[1];
+		this.adjacentEdges[1] = triangle.adjacentEdges[1];
-		this.adjacentEdges[2] = _triangle.adjacentEdges[2];
+		this.adjacentEdges[2] = triangle.adjacentEdges[2];
-		this.isObsolete = _triangle.isObsolete;
+		this.isObsolete = triangle.isObsolete;
-		this.determinant = _triangle.determinant;
+		this.determinant = triangle.determinant;
-		this.closestPoint = _triangle.closestPoint;
+		this.closestPoint = triangle.closestPoint;
-		this.lambda1 = _triangle.lambda1;
+		this.lambda1 = triangle.lambda1;
-		this.lambda2 = _triangle.lambda2;
+		this.lambda2 = triangle.lambda2;
-		this.distSquare = _triangle.distSquare;
+		this.distSquare = triangle.distSquare;
 	}
 	*/
 	
 	/// Compute the point v closest to the origin of this triangle
-	public boolean computeClosestPoint(final Vector3f[] _vertices) {
+	public boolean computeClosestPoint(final Vector3f[] vertices) {
-		final Vector3f p0 = _vertices[this.indicesVertices[0]];
+		final Vector3f p0 = vertices[this.indicesVertices[0]];
-		final Vector3f v1 = _vertices[this.indicesVertices[1]].lessNew(p0);
+		final Vector3f v1 = vertices[this.indicesVertices[1]].less(p0);
-		final Vector3f v2 = _vertices[this.indicesVertices[2]].lessNew(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) {
+	public Vector3f computeClosestPointOfObject(final Vector3f[] supportPointsOfObject) {
-		final Vector3f p0 = _supportPointsOfObject[this.indicesVertices[0]].clone();
+		final Vector3f p0 = supportPointsOfObject[this.indicesVertices[0]];
-		final Vector3f tmp1 = _supportPointsOfObject[this.indicesVertices[1]].lessNew(p0).multiply(this.lambda1);
+		final Vector3f tmp1 = supportPointsOfObject[this.indicesVertices[1]].less(p0).multiply(this.lambda1);
-		final Vector3f tmp2 = _supportPointsOfObject[this.indicesVertices[2]].lessNew(p0).multiply(this.lambda2);
+		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) {
+	public boolean computeSilhouette(final Vector3f[] vertices, final int indexNewVertex, final TrianglesStore triangleStore) {
-		final int first = _triangleStore.getNbTriangles();
+		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)
+		final boolean result = this.adjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) && this.adjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore)
-				&& this.adjacentEdges[1].computeSilhouette(_vertices, _indexNewVertex, _triangleStore) && this.adjacentEdges[2].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++) {
+			for (i = first, j = triangleStore.getNbTriangles() - 1; i != triangleStore.getNbTriangles(); j = i++) {
-				final TriangleEPA triangle = _triangleStore.get(i);
+				final TriangleEPA triangle = triangleStore.get(i);
-				halfLink(triangle.getAdjacentEdge(1), new EdgeEPA(triangle, 1));
+				TriangleEPA.halfLink(triangle.getAdjacentEdge(1), new EdgeEPA(triangle, 1));
-				if (!link(new EdgeEPA(triangle, 0), new EdgeEPA(_triangleStore.get(j), 2))) {
+				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) {
+	public int get(final int pos) {
-		assert (_pos >= 0 && _pos < 3);
+		assert (pos >= 0 && pos < 3);
-		return this.indicesVertices[_pos];
+		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) {
+	public EdgeEPA getAdjacentEdge(final int index) {
-		assert (_index >= 0 && _index < 3);
+		assert (index >= 0 && index < 3);
-		return this.adjacentEdges[_index];
+		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) {
+	public boolean isVisibleFromVertex(final Vector3f[] vertices, final int index) {
-		final Vector3f closestToVert = _vertices[_index].lessNew(this.closestPoint);
+		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[0] = indexVertex1;
-		this.indicesVertices[1] = _indexVertex2;
+		this.indicesVertices[1] = indexVertex2;
-		this.indicesVertices[2] = _indexVertex3;
+		this.indicesVertices[2] = indexVertex3;
 	}
 	
 	/// Private assignment operator
-	public TriangleEPA set(final TriangleEPA _triangle) {
+	public TriangleEPA set(final TriangleEPA triangle) {
-		this.indicesVertices[0] = _triangle.indicesVertices[0];
+		this.indicesVertices[0] = triangle.indicesVertices[0];
-		this.indicesVertices[1] = _triangle.indicesVertices[1];
+		this.indicesVertices[1] = triangle.indicesVertices[1];
-		this.indicesVertices[2] = _triangle.indicesVertices[2];
+		this.indicesVertices[2] = triangle.indicesVertices[2];
-		this.adjacentEdges[0] = _triangle.adjacentEdges[0];
+		this.adjacentEdges[0] = triangle.adjacentEdges[0];
-		this.adjacentEdges[1] = _triangle.adjacentEdges[1];
+		this.adjacentEdges[1] = triangle.adjacentEdges[1];
-		this.adjacentEdges[2] = _triangle.adjacentEdges[2];
+		this.adjacentEdges[2] = triangle.adjacentEdges[2];
-		this.isObsolete = _triangle.isObsolete;
+		this.isObsolete = triangle.isObsolete;
-		this.determinant = _triangle.determinant;
+		this.determinant = triangle.determinant;
-		this.closestPoint = _triangle.closestPoint;
+		this.closestPoint = triangle.closestPoint;
-		this.lambda1 = _triangle.lambda1;
+		this.lambda1 = triangle.lambda1;
-		this.lambda2 = _triangle.lambda2;
+		this.lambda2 = triangle.lambda2;
-		this.distSquare = _triangle.distSquare;
+		this.distSquare = triangle.distSquare;
 		return this;
 	}
 	
 	/// Set an adjacent edge of the triangle
-	public void setAdjacentEdge(final int _index, final EdgeEPA _edge) {
+	public void setAdjacentEdge(final int index, final EdgeEPA edge) {
-		assert (_index >= 0 && _index < 3);
+		assert (index >= 0 && index < 3);
-		this.adjacentEdges[_index] = _edge;
+		this.adjacentEdges[index] = edge;
 	}
 	
 	/// Set the isObsolete value
-	public void setIsObsolete(final boolean _isObsolete) {
+	public void setIsObsolete(final boolean isObsolete) {
-		this.isObsolete = _isObsolete;
+		this.isObsolete = isObsolete;
 	}
 	
 }

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

@@ -7,7 +7,7 @@ import org.atriasoft.ephysics.internal.Log;
 import org.atriasoft.etk.math.Vector3f;
 
 public class TrianglesStore {
-	private static int MAX_TRIANGLES = 200;
+	private static final int MAX_TRIANGLES = 200;
 	
 	public static void shrinkTo(final List list, final int newSize) {
 		final int size = list.size();
@@ -27,8 +27,8 @@ public class TrianglesStore {
 	}
 	
 	/// Access operator
-	public TriangleEPA get(final int _id) {
+	public TriangleEPA get(final int id) {
-		return this.triangles.get(_id);
+		return this.triangles.get(id);
 	}
 	
 	/// Return the number of triangles
@@ -42,13 +42,13 @@ public class TrianglesStore {
 	}
 	
 	/// Create a new triangle
-	public TriangleEPA newTriangle(final Vector3f[] _vertices, final int _v0, final int _v1, final int _v2) {
+	public TriangleEPA newTriangle(final Vector3f[] vertices, final int v0, final int v1, final int v2) {
-		//Log.info("newTriangle: " + _v0 + ", " + _v1 + ", " + _v2);
+		//Log.info("newTriangle: " + v0 + ", " + v1 + ", " + v2);
 		// If we have not reached the maximum number of triangles
-		if (this.triangles.size() < MAX_TRIANGLES) {
+		if (this.triangles.size() < TrianglesStore.MAX_TRIANGLES) {
 			
-			final TriangleEPA tmp = new TriangleEPA(_v0, _v1, _v2);
+			final TriangleEPA tmp = new TriangleEPA(v0, v1, v2);
-			if (!tmp.computeClosestPoint(_vertices)) {
+			if (!tmp.computeClosestPoint(vertices)) {
 				return null;
 			}
 			this.triangles.add(tmp);
@@ -61,10 +61,10 @@ public class TrianglesStore {
 	}
 	
 	/// Set the number of triangles
-	public void resize(final int _backup) {
+	public void resize(final int backup) {
-		if (_backup > this.triangles.size()) {
+		if (backup > this.triangles.size()) {
-			Log.error("RESIZE BIGGER : " + _backup + " > " + this.triangles.size());
+			Log.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,55 +50,57 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 		this.algoEPA.init();
 	}
 	
-	private void computePenetrationDepthForEnlargedObjects(final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info, final Transform3D transform2,
+	private Vector3f computePenetrationDepthForEnlargedObjects(final CollisionShapeInfo shape1Info, final Transform3D transform1, final CollisionShapeInfo shape2Info, final Transform3D transform2,
-			final NarrowPhaseCallback narrowPhaseCallback, final Vector3f v) {
+			final NarrowPhaseCallback narrowPhaseCallback, Vector3f v) {
 		//Log.info("computePenetrationDepthForEnlargedObjects...");
 		final Simplex simplex = new Simplex();
 		float distSquare = Float.MAX_VALUE;
 		float prevDistSquare;
-		assert (shape1Info.collisionShape.isConvex());
+		assert (shape1Info.collisionShape().isConvex());
-		assert (shape2Info.collisionShape.isConvex());
+		assert (shape2Info.collisionShape().isConvex());
-		final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape);
+		final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape());
-		final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape);
+		final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape());
-		final Object shape1CachedCollisionData = shape1Info.cachedCollisionData;
+		final Object shape1CachedCollisionData = shape1Info.cachedCollisionData();
-		final Object shape2CachedCollisionData = shape2Info.cachedCollisionData;
+		final Object shape2CachedCollisionData = shape2Info.cachedCollisionData();
 		//Log.info("    transform1=" + transform1);
 		//Log.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());
+		final Matrix3f rotateToBody2 = transform2.getOrientation().getMatrix().transpose().multiply(transform1.getOrientation().getMatrix());
 		//Log.info("    body2ToBody1=" + body2ToBody1);
 		//Log.info("    rotateToBody2=" + rotateToBody2);
 		//Log.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);
+			final Vector3f suppA = shape1.getLocalSupportPointWithMargin(v.multiply(-1), (CacheData) shape1CachedCollisionData);
 			//Log.info("    suppA=" + suppA);
-			final Vector3f suppB = body2ToBody1.multiplyNew(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiplyNew(v), (CacheData) shape2CachedCollisionData));
+			final Vector3f suppB = body2ToBody1.multiply(shape2.getLocalSupportPointWithMargin(rotateToBody2.multiply(v), (CacheData) shape2CachedCollisionData));
 			//Log.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);
 			// If the enlarge objects do not intersect
 			if (vDotw > 0.0f) {
 				//Log.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;
+				return v;
 			}
-			if (!simplex.computeClosestPoint(v)) {
+			Vector3f v2 = simplex.computeClosestPoint(v);
+			if (v2 == null) {
 				//Log.info("        ==> ret 3");
-				return;
+				return v;
 			}
+			v = v2;
 			// Store and update the square distance
 			prevDistSquare = distSquare;
 			//Log.info("    distSquare=" + distSquare);
@@ -106,14 +108,15 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 			//Log.info("    distSquare=" + distSquare);
 			if (prevDistSquare - distSquare <= Constant.FLOAT_EPSILON * prevDistSquare) {
 				//Log.info("        ==> ret 4");
-				return;
+				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);
+		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);
+			suppB = shape.getLocalSupportPointWithoutMargin(v.value, (CacheData) shapeCachedCollisionData);
-			w = suppA.lessNew(suppB);
+			w = suppA.less(suppB);
-			vDotW = v.dot(w);
+			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);
+				suppA = rayDirection.multiply(lambda).add(ray.point1);
-					w = suppA.lessNew(suppB);
+				w = suppA.less(suppB);
-					n = v;
+				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)) {
+			Vector3f v2 = simplex.computeClosestPoint(v.value);
-				distSquare = v.length2();
+			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();
+		var pointA = new Object() {
-		final Vector3f pointB = new Vector3f();
+			Vector3f value = Vector3f.ZERO;
-		simplex.computeClosestPointsOfAandB(pointA, pointB);
+		};
+		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) {
@@ -207,28 +219,34 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 		//Log.error("=================================================");
 		//Log.error(" shape1Info=" + shape1Info.shapeToWorldTransform);
 		//Log.error(" shape2Info=" + shape2Info.shapeToWorldTransform);
-		Vector3f suppA = new Vector3f(); // Support point of object A
+		Vector3f suppA = Vector3f.ZERO; // Support point of object A
-		Vector3f suppB = new Vector3f(); // Support point of object B
+		Vector3f suppB = Vector3f.ZERO; // Support point of object B
-		Vector3f w = new Vector3f(); // Support point of Minkowski difference A-B
+		Vector3f w = Vector3f.ZERO; // Support point of Minkowski difference A-B
-		Vector3f pA = new Vector3f(); // Closest point of object A
+		var pA = new Object() {
-		Vector3f pB = new Vector3f(); // Closest point of object B
+			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 (shape1Info.collisionShape().isConvex());
-		assert (shape2Info.collisionShape.isConvex());
+		assert (shape2Info.collisionShape().isConvex());
-		final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape);
+		final ConvexShape shape1 = (ConvexShape) (shape1Info.collisionShape());
-		final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape);
+		final ConvexShape shape2 = (ConvexShape) (shape2Info.collisionShape());
-		final CacheData shape1CachedCollisionData = (CacheData) shape1Info.cachedCollisionData;
+		final CacheData shape1CachedCollisionData = (CacheData) shape1Info.cachedCollisionData();
-		final CacheData shape2CachedCollisionData = (CacheData) shape2Info.cachedCollisionData;
+		final CacheData shape2CachedCollisionData = (CacheData) shape2Info.cachedCollisionData();
 		// Get the local-space to world-space transforms
-		final Transform3D transform1 = shape1Info.shapeToWorldTransform.clone();
+		final Transform3D transform1 = shape1Info.shapeToWorldTransform();
-		final Transform3D transform2 = shape2Info.shapeToWorldTransform.clone();
+		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,7 +254,9 @@ 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;
 		
@@ -248,43 +268,43 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 		do {
 			//Log.error("------------------");
 			// Compute the support points for original objects (without margins) A and B
-			suppA = shape1.getLocalSupportPointWithoutMargin(cacheSeparatingAxis.multiplyNew(-1.0f), shape1CachedCollisionData);
+			suppA = shape1.getLocalSupportPointWithoutMargin(cacheSeparatingAxis.value.multiply(-1.0f), shape1CachedCollisionData);
-			suppB = body2Tobody1.multiplyNew(shape2.getLocalSupportPointWithoutMargin(rotateToBody2.multiplyNew(cacheSeparatingAxis), shape2CachedCollisionData));
+			suppB = body2Tobody1.multiply(shape2.getLocalSupportPointWithoutMargin(rotateToBody2.multiply(cacheSeparatingAxis.value), shape2CachedCollisionData));
 			// Compute the support point for the Minkowski difference A-B
-			w = suppA.lessNew(suppB);
+			w = suppA.less(suppB);
-			vDotw = cacheSeparatingAxis.dot(w);
+			vDotw = cacheSeparatingAxis.value.dot(w);
 			//Log.error(" suppA=" + suppA);
 			//Log.error(" suppB=" + suppB);
 			//Log.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) {
+			if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * GJKAlgorithm.REL_ERROR_SQUARE) {
 				//Log.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));
+				pA.value = pA.value.less(cacheSeparatingAxis.value.multiply(shape1.getMargin() / dist));
-				pB = body2Tobody1.inverseNew().multiplyNew(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB));
+				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,
+				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape(), shape2Info.proxyShape(), shape1Info.collisionShape(), shape2Info.collisionShape(), normal,
-						pA, pB);
+						penetrationDepth, pA.value, pB.value);
-				callback.notifyContact(shape1Info.overlappingPair, contactInfo);
+				callback.notifyContact(shape1Info.overlappingPair(), contactInfo);
 				// There is an intersection, therefore we return
 				return;
 			}
@@ -294,15 +314,15 @@ public class GJKAlgorithm extends NarrowPhaseAlgorithm {
 			if (simplex.isAffinelyDependent()) {
 				//Log.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));
+				pA.value = pA.value.less(cacheSeparatingAxis.value.multiply(shape1.getMargin() / dist));
-				pB = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB));
+				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)
@@ -312,26 +332,27 @@ 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,
+				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape(), shape2Info.proxyShape(), shape1Info.collisionShape(), shape2Info.collisionShape(), normal,
-						pA, pB);
+						penetrationDepth, pA.value, pB.value);
-				callback.notifyContact(shape1Info.overlappingPair, contactInfo);
+				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)) {
+			Vector3f tmp = simplex.computeClosestPoint(cacheSeparatingAxis.value);
+			if (tmp == null) {
 				//Log.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));
+				pA.value = pA.value.less(cacheSeparatingAxis.value.multiply(shape1.getMargin() / dist));
-				pB = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB));
+				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,
+				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape(), shape2Info.proxyShape(), shape1Info.collisionShape(), shape2Info.collisionShape(), normal,
-						pA, pB);
+						penetrationDepth, pA.value, pB.value);
-				callback.notifyContact(shape1Info.overlappingPair, contactInfo);
+				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);
+				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));
+				pA.value = pA.value.less(cacheSeparatingAxis.value.multiply(shape1.getMargin() / dist));
-				pB = body2Tobody1.inverseNew().multiply(cacheSeparatingAxis.multiplyNew(shape2.getMargin() / dist).add(pB));
+				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,
+				final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape(), shape2Info.proxyShape(), shape1Info.collisionShape(), shape2Info.collisionShape(), normal,
-						pA, pB);
+						penetrationDepth, pA.value, pB.value);
-				callback.notifyContact(shape1Info.overlappingPair, contactInfo);
+				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 suppA = Vector3f.ZERO; // Support point of object A
-		Vector3f w = new Vector3f(); // Support point of Minkowski difference A-B
+		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;
 
 /**
@@ -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 computeClosestPointForSubset(this.bitsCurrentSimplex); // Compute the closest point in the simplex
-				return true;
 			}
 		}
 		
@@ -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 this.points[this.lastFound]; // The closest point of the simplex "v" is the last added point
-			return true;
 		}
 		
 		// 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,24 +224,26 @@ 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);
+		Vector3f pA = Vector3f.ZERO;
-		pB.set(0.0f, 0.0f, 0.0f);
+		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)));
+				pA = pA.add(this.suppPointsA[iii].multiply(this.det.get(this.bitsCurrentSimplex, iii)));
-				pB.add(this.suppPointsB[iii].multiplyNew(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);
+		pA = pA.multiply(factor);
-		pB.multiply(factor);
+		pB = pB.multiply(factor);
+		outA.accept(pA);
+		outB.accept(pB);
 	}
 	
 	/// Compute the cached determinant values
@@ -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();
+				suppPointsA[nbVertices] = this.suppPointsA[nbVertices];
-				suppPointsB[nbVertices] = this.suppPointsB[nbVertices].clone();
+				suppPointsB[nbVertices] = this.suppPointsB[nbVertices];
-				points[nbVertices] = this.points[nbVertices].clone();
+				points[nbVertices] = this.points[nbVertices];
 				nbVertices++;
 			}
 		}
@@ -428,9 +432,9 @@ public class Simplex {
 			if (overlap(this.bitsCurrentSimplex, bit)) {
 				//Log.warning("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 sphereShape1 = (SphereShape) shape1Info.collisionShape();
-		final SphereShape sphereShape2 = (SphereShape) _shape2Info.collisionShape;
+		final SphereShape sphereShape2 = (SphereShape) shape2Info.collisionShape();
 		// Get the local-space to world-space transforms
-		final Transform3D transform1 = _shape1Info.shapeToWorldTransform;
+		final Transform3D transform1 = shape1Info.shapeToWorldTransform();
-		final Transform3D transform2 = _shape2Info.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 intersectionOnBody1 = centerSphere2InBody1LocalSpace.safeNormalize().multiply(sphereShape1.getRadius());
-			final Vector3f intersectionOnBody2 = centerSphere1InBody2LocalSpace.safeNormalizeNew().multiply(sphereShape2.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,
+			final ContactPointInfo contactInfo = new ContactPointInfo(shape1Info.proxyShape(), shape2Info.proxyShape(), shape1Info.collisionShape(), shape2Info.collisionShape(),
-					vectorBetweenCenters.safeNormalizeNew(), penetrationDepth, intersectionOnBody1, intersectionOnBody2);
+					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
+	 * Create and return an AABB for a triangle
-	 * @param[in] _trianglePoints List of 3 point od a triangle
+	 * @param trianglePoints List of 3 point od a triangle
 	 * @return An AABB box
 	 */
-	public static AABB createAABBForTriangle(final Vector3f[] _trianglePoints) {
+	public static AABB createAABBForTriangle(final Vector3f[] trianglePoints) {
-		final Vector3f minCoords = new Vector3f(_trianglePoints[0].x, _trianglePoints[0].y, _trianglePoints[0].z);
+		final Vector3f minCoords = trianglePoints[0].min(trianglePoints[1]).min(trianglePoints[2]);
-		final Vector3f maxCoords = new Vector3f(_trianglePoints[0].x, _trianglePoints[0].y, _trianglePoints[0].z);
+		final Vector3f maxCoords = trianglePoints[0].max(trianglePoints[1]).max(trianglePoints[2]);
-		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);
-		}
 		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.maxCoordinates = Vector3f.ZERO;
-		this.minCoordinates = new Vector3f();
+		this.minCoordinates = Vector3f.ZERO;
 	}
 	
 	/**
-	 * @brief contructor Whit sizes
+	 * contructor Whit sizes
-	 * @param[in] _minCoordinates Minimum coordinates
+	 * @param minCoordinates Minimum coordinates
-	 * @param[in] _maxCoordinates Maximum 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
+	 * Return true if the current AABB contains the AABB given in parameter
-	 * @param[in] _aabb AABB box that is contains in the current.
+	 * @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.x() <= aabb.minCoordinates.x();
-		isInside = isInside && this.minCoordinates.y <= _aabb.minCoordinates.y;
+		isInside = isInside && this.minCoordinates.y() <= aabb.minCoordinates.y();
-		isInside = isInside && this.minCoordinates.z <= _aabb.minCoordinates.z;
+		isInside = isInside && this.minCoordinates.z() <= aabb.minCoordinates.z();
-		isInside = isInside && this.maxCoordinates.x >= _aabb.maxCoordinates.x;
+		isInside = isInside && this.maxCoordinates.x() >= aabb.maxCoordinates.x();
-		isInside = isInside && this.maxCoordinates.y >= _aabb.maxCoordinates.y;
+		isInside = isInside && this.maxCoordinates.y() >= aabb.maxCoordinates.y();
-		isInside = isInside && this.maxCoordinates.z >= _aabb.maxCoordinates.z;
+		isInside = isInside && this.maxCoordinates.z() >= aabb.maxCoordinates.z();
 		return isInside;
 	}
 	
 	/**
-	 * @brief Return true if a point is inside the AABB
+	 * Return true if a point is inside the AABB
-	 * @param[in] _point Point to check.
+	 * @param point Point to check.
 	 * @return true The point in contained inside
 	 */
-	public boolean contains(final Vector3f _point) {
+	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
+		return point.x() >= this.minCoordinates.x() - Constant.FLOAT_EPSILON && point.x() <= this.maxCoordinates.x() + Constant.FLOAT_EPSILON
-				&& _point.y <= this.maxCoordinates.y + Constant.FLOAT_EPSILON && _point.z >= this.minCoordinates.z - Constant.FLOAT_EPSILON
+				&& point.y() >= this.minCoordinates.y() - Constant.FLOAT_EPSILON && point.y() <= this.maxCoordinates.y() + Constant.FLOAT_EPSILON
-				&& _point.z <= this.maxCoordinates.z + 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,74 +108,65 @@ 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);
+		final Vector3f diff = this.maxCoordinates.less(this.minCoordinates);
-		return (diff.x * diff.y * diff.z);
+		return (diff.x() * diff.y() * diff.z());
 	}
 	
 	/**
-	 * @brief Inflate each side of the AABB by a given size
+	 * Inflate each side of the AABB by a given size
-	 * @param[in] _dx Inflate X size
+	 * @param dx Inflate X size
-	 * @param[in] _dy Inflate Y size
+	 * @param dy Inflate Y size
-	 * @param[in] _dz Inflate Z size
+	 * @param dz Inflate Z size
 	 */
-	public void inflate(final float _dx, final float _dy, final float _dz) {
+	public void inflate(final float dx, final float dy, final float dz) {
-		this.maxCoordinates.add(new Vector3f(_dx, _dy, _dz));
+		this.maxCoordinates = this.maxCoordinates.add(new Vector3f(dx, dy, dz));
-		this.minCoordinates.less(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
+	 * 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 aabb1 first AABB box to merge with aabb2.
-	 * @param[in] _aabb2 second AABB box to merge with _aabb1.
+	 * @param aabb2 second AABB box to merge with aabb1.
 	 */
-	public void mergeTwoAABBs(final AABB _aabb1, final AABB _aabb2) {
+	public void mergeTwoAABBs(final AABB aabb1, final AABB aabb2) {
-		this.minCoordinates.setX(FMath.min(_aabb1.minCoordinates.x, _aabb2.minCoordinates.x));
+		this.minCoordinates = FMath.min(aabb1.minCoordinates, aabb2.minCoordinates);
-		this.minCoordinates.setY(FMath.min(_aabb1.minCoordinates.y, _aabb2.minCoordinates.y));
+		this.maxCoordinates = FMath.max(aabb1.maxCoordinates, aabb2.maxCoordinates);
-		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));
 	}
 	
 	/**
-	 * @brief Merge the AABB in parameter with the current one
+	 * Merge the AABB in parameter with the current one
-	 * @param[in] _aabb Other AABB box to merge.
+	 * @param aabb Other AABB box to merge.
 	 */
-	public void mergeWithAABB(final AABB _aabb) {
+	public void mergeWithAABB(final AABB aabb) {
-		this.minCoordinates.setX(FMath.min(this.minCoordinates.x, _aabb.minCoordinates.x));
+		this.minCoordinates = FMath.min(this.minCoordinates, aabb.minCoordinates);
-		this.minCoordinates.setY(FMath.min(this.minCoordinates.y, _aabb.minCoordinates.y));
+		this.maxCoordinates = FMath.max(this.maxCoordinates, aabb.maxCoordinates);
-		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));
 	}
 	
 	/**
-	 * @brief Set the maximum coordinates of the AABB
+	 * Set the maximum coordinates of the AABB
-	 * @param[in] _max The 3d maximum coordonates
+	 * @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
+	 * Set the minimum coordinates of the AABB
-	 * @param[in] _min The 3d minimum coordonates
+	 * @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.
+	 * @param aabb Other AABB box to check.
-	 * @return true Collision detected
+	 * @return true Collision detected / false Not collide
-	 * @return false Not collide
 	 */
 	public boolean testCollision(final AABB aabb) {
 		if (this == aabb) {
@@ -228,13 +174,13 @@ public class AABB {
 			return true;
 		}
 		//Log.info("test : " + this + " && " + aabb);
-		if (this.maxCoordinates.getX() < aabb.minCoordinates.getX() || aabb.maxCoordinates.getX() < this.minCoordinates.getX()) {
+		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 ");
@@ -242,55 +188,55 @@ public class AABB {
 	}
 	
 	/**
-	 * @brief check if the AABB of a triangle intersects the AABB
+	 * check if the AABB of a triangle intersects the AABB
-	 * @param[in] _trianglePoints List of 3 point od a triangle
+	 * @param trianglePoints List of 3 point od a triangle
 	 * @return true The triangle is contained in the Box
 	 */
-	public boolean testCollisionTriangleAABB(final Vector3f[] _trianglePoints) {
+	public boolean testCollisionTriangleAABB(final Vector3f[] trianglePoints) {
-		if (FMath.min(_trianglePoints[0].x, _trianglePoints[1].x, _trianglePoints[2].x) > this.maxCoordinates.x) {
+		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 point2 = ray.point2.less(ray.point1).multiply(ray.maxFraction).add(ray.point1);
-		final Vector3f e = this.maxCoordinates.lessNew(this.minCoordinates);
+		final Vector3f e = this.maxCoordinates.less(this.minCoordinates);
-		final Vector3f d = point2.lessNew(ray.point1);
+		final Vector3f d = point2.less(ray.point1);
-		final Vector3f m = ray.point1.addNew(point2).less(this.minCoordinates).less(this.maxCoordinates);
+		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);
+		float adx = FMath.abs(d.x());
-		if (FMath.abs(m.x) > e.x + adx) {
+		if (FMath.abs(m.x()) > e.x() + adx) {
 			return false;
 		}
-		float ady = FMath.abs(d.y);
+		float ady = FMath.abs(d.y());
-		if (FMath.abs(m.y) > e.y + ady) {
+		if (FMath.abs(m.y()) > e.y() + ady) {
 			return false;
 		}
-		float adz = FMath.abs(d.z);
+		float adz = FMath.abs(d.z());
-		if (FMath.abs(m.z) > e.z + adz) {
+		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.x() > 0.0f && extent.x() > margin);
-		assert (extent.getY() > 0.0f && extent.getY() > margin);
+		assert (extent.y() > 0.0f && extent.y() > margin);
-		assert (extent.getZ() > 0.0f && extent.getZ() > margin);
+		assert (extent.z() > 0.0f && extent.z() > margin);
-		this.extent = extent.lessNew(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) {
+	public Matrix3f computeLocalInertiaTensor(final float mass) {
-		final float factor = (1.0f / 3.0f) * _mass;
+		final float factor = (1.0f / 3.0f) * mass;
-		final Vector3f realExtent = this.extent.addNew(new Vector3f(this.margin, this.margin, this.margin));
+		final Vector3f realExtent = this.extent.add(new Vector3f(this.margin, this.margin, this.margin));
-		final float xSquare = realExtent.x * realExtent.x;
+		final float xSquare = realExtent.x() * realExtent.x();
-		final float ySquare = realExtent.y * realExtent.y;
+		final float ySquare = realExtent.y() * realExtent.y();
-		final float zSquare = realExtent.z * realExtent.z;
+		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));
+		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) {
+	public Bounds getLocalBounds() {
-		// Maximum bounds
+		Vector3f tmp = new Vector3f(this.extent.x() + this.margin, this.extent.y() + this.margin, this.extent.z() + this.margin);
-		_max.set(this.extent.x + this.margin, this.extent.y + this.margin, this.extent.z + this.margin);
+		return new Bounds(tmp.multiply(-1), tmp);
-		// Minimum bounds
-		_min.set(-_max.x, -_max.y, -_max.z);
 	}
 	
 	/*
-	protected size_t getSizeInBytes() {
+	protected sizet getSizeInBytes() {
 		return sizeof(BoxShape);
 	}
 	 */
 	@Override
-	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) {
-		//Log.error("getLocalSupportPointWithoutMargin(" + _direction);
+		//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);
+		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) {
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
-		final Vector3f rayDirection = _ray.point2.less(_ray.point1);
+		final Vector3f rayDirection = ray.point2.less(ray.point1);
 		float tMin = Float.MIN_VALUE;
 		float tMax = Float.MAX_VALUE;
-		Vector3f normalDirection = new Vector3f(0, 0, 0);
+		Vector3f normalDirection = Vector3f.ZERO;
-		Vector3f currentNormal = new Vector3f(0, 0, 0);
+		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 t1 = (-this.extent.get(iii) - ray.point1.get(iii)) * oneOverD;
-				float t2 = (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;
+				float tmpX = (iii == 0) ? -this.extent.get(iii) : 0.0f;
-				currentNormal.y = (iii == 1) ? -this.extent.get(iii) : 0.0f;
+				float tmpY = (iii == 1) ? -this.extent.get(iii) : 0.0f;
-				currentNormal.z = (iii == 2) ? -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);
+		final Vector3f localHitPoint = rayDirection.multiply(tMin).add(ray.point1);
-		_raycastInfo.body = _proxyShape.getBody();
+		raycastInfo.body = proxyShape.getBody();
-		_raycastInfo.proxyShape = _proxyShape;
+		raycastInfo.proxyShape = proxyShape;
-		_raycastInfo.hitFraction = tMin;
+		raycastInfo.hitFraction = tMin;
-		_raycastInfo.worldPoint = localHitPoint;
+		raycastInfo.worldPoint = localHitPoint;
-		_raycastInfo.worldNormal = normalDirection;
+		raycastInfo.worldNormal = normalDirection;
 		return true;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
+	public void setLocalScaling(final Vector3f scaling) {
-		this.extent.divide(this.scaling).multiply(_scaling);
+		this.extent = this.extent.divide(this.scaling).multiply(scaling);
-		super.setLocalScaling(_scaling);
+		super.setLocalScaling(scaling);
 	}
 	
 	@Override
-	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+	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
+		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);
+				&& 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 ixxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3;
-		final float Iyy = factor1 * _mass * sum1 + factor2 * _mass * 0.25f * radiusSquareDouble;
+		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);
+		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) {
+	public Bounds getLocalBounds() {
-		// Maximum bounds
+		Vector3f tmp = new Vector3f(this.margin, this.halfHeight + this.margin, this.margin);
-		max.setX(this.margin);
+		return new Bounds(tmp.multiply(-1), tmp);
-		max.setY(this.halfHeight + this.margin);
-		max.setZ(this.margin);
-		// Minimum bounds
-		min.setX(-max.x);
-		min.setY(-max.y);
-		min.setZ(-max.z);
 	}
 	
 	@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) {
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
-		final Vector3f n = _ray.point2.lessNew(_ray.point1);
+		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 d = q.less(p);
-		final Vector3f m = _ray.point1.lessNew(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)) {
+				if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
-					_raycastInfo.body = _proxyShape.getBody();
+					raycastInfo.body = proxyShape.getBody();
-					_raycastInfo.proxyShape = _proxyShape;
+					raycastInfo.proxyShape = proxyShape;
-					_raycastInfo.hitFraction = hitFraction;
+					raycastInfo.hitFraction = hitFraction;
-					_raycastInfo.worldPoint = hitLocalPoint;
+					raycastInfo.worldPoint = hitLocalPoint;
-					final Vector3f normalDirection = hitLocalPoint.lessNew(p);
+					final Vector3f normalDirection = hitLocalPoint.less(p);
-					_raycastInfo.worldNormal = normalDirection;
+					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;
+			if (mDotD <= dDotD) {
-			} else {
 				// 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)) {
+			if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
-				_raycastInfo.body = _proxyShape.getBody();
+				raycastInfo.body = proxyShape.getBody();
-				_raycastInfo.proxyShape = _proxyShape;
+				raycastInfo.proxyShape = proxyShape;
-				_raycastInfo.hitFraction = hitFraction;
+				raycastInfo.hitFraction = hitFraction;
-				_raycastInfo.worldPoint = hitLocalPoint;
+				raycastInfo.worldPoint = hitLocalPoint;
-				final Vector3f normalDirection = hitLocalPoint.lessNew(p);
+				final Vector3f normalDirection = hitLocalPoint.less(p);
-				_raycastInfo.worldNormal = normalDirection;
+				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)) {
+			if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
-				_raycastInfo.body = _proxyShape.getBody();
+				raycastInfo.body = proxyShape.getBody();
-				_raycastInfo.proxyShape = _proxyShape;
+				raycastInfo.proxyShape = proxyShape;
-				_raycastInfo.hitFraction = hitFraction;
+				raycastInfo.hitFraction = hitFraction;
-				_raycastInfo.worldPoint = hitLocalPoint;
+				raycastInfo.worldPoint = hitLocalPoint;
-				final Vector3f normalDirection = hitLocalPoint.lessNew(q);
+				final Vector3f normalDirection = hitLocalPoint.less(q);
-				_raycastInfo.worldNormal = normalDirection;
+				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);
+		final Vector3f localHitPoint = n.multiply(t).add(ray.point1);
-		_raycastInfo.body = _proxyShape.getBody();
+		raycastInfo.body = proxyShape.getBody();
-		_raycastInfo.proxyShape = _proxyShape;
+		raycastInfo.proxyShape = proxyShape;
-		_raycastInfo.hitFraction = t;
+		raycastInfo.hitFraction = t;
-		_raycastInfo.worldPoint = localHitPoint;
+		raycastInfo.worldPoint = localHitPoint;
-		final Vector3f v = localHitPoint.lessNew(p);
+		final Vector3f v = localHitPoint.less(p);
-		final Vector3f w = d.multiplyNew(v.dot(d) / d.length2());
+		final Vector3f w = d.multiply(v.dot(d) / d.length2());
-		final Vector3f normalDirection = localHitPoint.lessNew(p.addNew(w)).safeNormalize();
+		final Vector3f normalDirection = localHitPoint.less(p.add(w)).safeNormalize();
-		_raycastInfo.worldNormal = normalDirection;
+		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) {
+	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);
+		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;
+			hitFraction = t;
-			_hitLocalPoint = rayDirection.multiplyNew(t).add(_point1);
+			hitLocalPoint = rayDirection.multiply(t).add(point1);
 			return true;
 		}
 		return false;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
+	public void setLocalScaling(final Vector3f scaling) {
-		this.halfHeight = (this.halfHeight / this.scaling.y) * _scaling.y;
+		this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
-		this.margin = (this.margin / this.scaling.x) * _scaling.x;
+		this.margin = (this.margin / this.scaling.x()) * scaling.x();
-		super.setLocalScaling(_scaling);
+		super.setLocalScaling(scaling);
 	}
 	
 	@Override
-	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+	public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) {
-		final float diffYCenterSphere1 = _localPoint.y - this.halfHeight;
+		final float diffYCenterSphere1 = localPoint.y() - this.halfHeight;
-		final float diffYCenterSphere2 = _localPoint.y + this.halfHeight;
+		final float diffYCenterSphere2 = localPoint.y() + this.halfHeight;
-		final float xSquare = _localPoint.x * _localPoint.x;
+		final float xSquare = localPoint.x() * localPoint.x();
-		final float zSquare = _localPoint.z * _localPoint.z;
+		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;
+package org.atriasoft.ephysics.collision.shapes;
-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.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)) {
+		if (CollisionShape.isConvex(shapeType1) && CollisionShape.isConvex(shapeType2)) {
-			return Defaults.NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
+			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
+	 * Check if the shape is convex
-	 * @param[in] _shapeType shape type
+	 * @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) {
+	public static boolean isConvex(final CollisionShapeType shapeType) {
-		return _shapeType != CollisionShapeType.CONCAVE_MESH && _shapeType != CollisionShapeType.HEIGHTFIELD;
+		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
+	 * 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 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
+	 * @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);
+		Bounds bounds = getLocalBounds();
-		final Vector3f maxBounds = new Vector3f(0, 0, 0);
-		getLocalBounds(minBounds, maxBounds);
 		// Rotate the local bounds according to the orientation of the body
-		final Matrix3f worldAxis = _transform.getOrientation().getMatrix().getAbsolute();
+		final Matrix3f worldAxis = transform.getOrientation().getMatrix().abs();
-		final Vector3f worldMinBounds = new Vector3f(worldAxis.getColumn(0).dot(minBounds), worldAxis.getColumn(1).dot(minBounds), worldAxis.getColumn(2).dot(minBounds));
+		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(maxBounds), worldAxis.getColumn(1).dot(maxBounds), worldAxis.getColumn(2).dot(maxBounds));
+		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 minCoordinates = transform.getPosition().add(worldMinBounds);
-		final Vector3f maxCoordinates = _transform.getPosition().addNew(worldMaxBounds);
+		final Vector3f maxCoordinates = transform.getPosition().add(worldMaxBounds);
 		// Update the AABB with the new minimum and maximum coordinates
-		_aabb.setMin(minCoordinates);
+		aabb.setMin(minCoordinates);
-		_aabb.setMax(maxCoordinates);
+		aabb.setMax(maxCoordinates);
 	}
 	
 	/**
-	 * @brief Compute the local inertia tensor of the sphere
+	 * Compute the local inertia tensor of the sphere
-	 * @param[out] _tensor The 3x3 inertia tensor matrix of the shape in local-space coordinates
+	 * @return 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
+	 * @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 min The minimum bounds of the shape in local-space coordinates
-	 * @param _max The maximum 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) {
+	public void setLocalScaling(final Vector3f scaling) {
-		this.scaling = _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), CAPSULE(5), CONCAVE_MESH(7), CONE(3), CONVEX_MESH(6), CYLINDER(4), HEIGHTFIELD(8), SPHERE(2), TRIANGLE(0);
-	BOX(1),
-	SPHERE(2),
-	CONE(3),
-	CYLINDER(4),
-	CAPSULE(5),
-	CONVEX_MESH(6),
-	CONCAVE_MESH(7),
-	HEIGHTFIELD(8);
 	
 	public static final int NB_COLLISION_SHAPE_TYPES = 9;
 	
 	public static CollisionShapeType getType(final int value) {
-		switch (value) {
+		return switch (value) {
-			case 0:
+			case 0 -> TRIANGLE;
-				return TRIANGLE;
+			case 1 -> BOX;
-			case 1:
+			case 2 -> SPHERE;
-				return BOX;
+			case 3 -> CONE;
-			case 2:
+			case 4 -> CYLINDER;
-				return SPHERE;
+			case 5 -> CAPSULE;
-			case 3:
+			case 6 -> CONVEX_MESH;
-				return CONE;
+			case 7 -> CONCAVE_MESH;
-			case 4:
+			case 8 -> HEIGHTFIELD;
-				return CYLINDER;
+			default -> null;
-			case 5:
+		};
-				return CAPSULE;
-			case 6:
-				return CONVEX_MESH;
-			case 7:
-				return CONCAVE_MESH;
-			case 8:
-				return HEIGHTFIELD;
-		}
-		return null;
 	}
 	
 	public final int value;

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

@@ -25,21 +25,21 @@ import org.atriasoft.etk.math.Vector3f;
  */
 public class ConcaveMeshShape extends ConcaveShape {
 	class ConcaveMeshRaycastCallback {
-		private final List<DTree> hitAABBNodes = new ArrayList<>();
-		private final DynamicAABBTree dynamicAABBTree;
 		private final ConcaveMeshShape concaveMeshShape;
-		private final ProxyShape proxyShape;
+		private final DynamicAABBTree dynamicAABBTree;
-		private final RaycastInfo raycastInfo;
+		private final List<DTree> hitAABBNodes = new ArrayList<>();
-		private final Ray ray;
 		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) {
+		ConcaveMeshRaycastCallback(final DynamicAABBTree dynamicAABBTree, final ConcaveMeshShape concaveMeshShape, final ProxyShape proxyShape, final RaycastInfo raycastInfo, final Ray ray) {
-			this.dynamicAABBTree = _dynamicAABBTree;
+			this.dynamicAABBTree = dynamicAABBTree;
-			this.concaveMeshShape = _concaveMeshShape;
+			this.concaveMeshShape = concaveMeshShape;
-			this.proxyShape = _proxyShape;
+			this.proxyShape = proxyShape;
-			this.raycastInfo = _raycastInfo;
+			this.raycastInfo = raycastInfo;
-			this.ray = _ray;
+			this.ray = ray;
 			this.isHit = false;
 			
 		}
@@ -50,10 +50,10 @@ public class ConcaveMeshShape extends ConcaveShape {
 		}
 		
 		/// 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);
+			this.hitAABBNodes.add(node);
-			return _ray.maxFraction;
+			return ray.maxFraction;
 		}
 		
 		/// Raycast all collision shapes that have been collected
@@ -61,11 +61,11 @@ public class ConcaveMeshShape extends ConcaveShape {
 			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 data0 = this.dynamicAABBTree.getNodeDataInt0(it);
-				final int data_1 = this.dynamicAABBTree.getNodeDataInt_1(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();
 				
@@ -82,57 +82,56 @@ 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.meshSubpart = data0;
-					this.raycastInfo.triangleIndex = data_1;
+					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());
+		return new Bounds(treeAABB.getMin(), treeAABB.getMax());
-		_max.set(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) {
+	protected void getTriangleVerticesWithIndexPointer(final int subPart, final int triangleIndex, final Vector3f[] outTriangleVertices) {
-		assert (_outTriangleVertices != null);
+		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 ...");
 		}
-		final Triangle trianglePoints = triangleVertexArray.getTriangle(_triangleIndex);
+		final Triangle trianglePoints = triangleVertexArray.getTriangle(triangleIndex);
-		_outTriangleVertices[0] = trianglePoints.get(0).multiplyNew(this.scaling);
+		outTriangleVertices[0] = trianglePoints.get(0).multiply(this.scaling);
-		_outTriangleVertices[1] = trianglePoints.get(1).multiplyNew(this.scaling);
+		outTriangleVertices[1] = trianglePoints.get(1).multiply(this.scaling);
-		_outTriangleVertices[2] = trianglePoints.get(2).multiplyNew(this.scaling);
+		outTriangleVertices[2] = trianglePoints.get(2).multiply(this.scaling);
 	}
 	
 	/// Insert all the triangles longo the dynamic AABB tree
@@ -164,17 +163,17 @@ public class ConcaveMeshShape 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) {
 		// 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) {
+			public float callback(final DTree node, final Ray ray) {
-				return raycastCallback.operator__parenthese(_node, _ray);
+				return raycastCallback.operatorparenthese(node, ray);
 			}
 		});
 		raycastCallback.raycastTriangles();
@@ -182,28 +181,28 @@ public class ConcaveMeshShape extends ConcaveShape {
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
+	public void setLocalScaling(final Vector3f scaling) {
-		super.setLocalScaling(_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 data0 = self.dynamicAABBTree.getNodeDataInt0(node);
-				final int data_1 = self.dynamicAABBTree.getNodeDataInt_1(_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) {
+	boolean isSmoothMeshCollisionEnabled; //!< True if the smooth mesh collision algorithm is enabled
-		super(_type);
+	
+	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) {
+	public void setIsSmoothMeshCollisionEnabled(final boolean isEnabled) {
-		this.isSmoothMeshCollisionEnabled = _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) {
+	public void setRaycastTestType(final TriangleRaycastSide testType) {
-		this.raycastTestType = _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 radius Radius of the cone (in meters)
-	 * @param _height Height of the cone (in meters)
+	 * @param height Height of the cone (in meters)
-	 * @param _margin Collision margin (in meters) around the collision shape
+	 * @param margin Collision margin (in meters) around the collision shape
 	 */
-	public ConeShape(final float _radius, final float _height) {
+	public ConeShape(final float radius, final float height) {
-		this(_radius, _height, Defaults.OBJECT_MARGIN);
+		this(radius, height, Defaults.OBJECT_MARGIN);
 	}
 	
-	public ConeShape(final float _radius, final float _height, final float _margin) {
+	public ConeShape(final float radius, final float height, final float margin) {
-		super(CollisionShapeType.CONE, _margin);
+		super(CollisionShapeType.CONE, margin);
-		this.radius = _radius;
+		this.radius = radius;
-		this.halfHeight = _height * 0.5f;
+		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);
+		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);
+		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) {
+	public Bounds getLocalBounds() {
-		// Maximum bounds
+		Vector3f max = new Vector3f(this.radius + this.margin, this.halfHeight + this.margin, this.radius + this.margin);
-		max.setX(this.radius + this.margin);
+		return new Bounds(max.multiply(-1), max);
-		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);
 	}
 	
 	@Override
-	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) {
-		final Vector3f v = _direction;
+		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) {
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
-		final Vector3f r = _ray.point2.lessNew(_ray.point1);
+		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 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 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 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 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 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[0] = r.multiply(tHit[0]).add(ray.point1);
-		localHitPoint[1] = r.multiplyNew(tHit[1]).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);
+			localHitPoint[2] = r.multiply(tHit[2]).add(ray.point1);
-			if ((localHitPoint[2].lessNew(centerBase)).length2() > this.radius * this.radius) {
+			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 x = localHitPoint[hitIndex].x() * factor22;
-			final float z = localHitPoint[hitIndex].z * factor22;
+			final float z = localHitPoint[hitIndex].z() * factor22;
-			localNormal[hitIndex].setX(x);
+			localNormal[hitIndex] = new Vector3f(x, FMath.sqrt(x * x + z * z) * rOverH, z);
-			localNormal[hitIndex].setY(FMath.sqrt(x * x + z * z) * rOverH);
-			localNormal[hitIndex].setZ(z);
 		}
-		_raycastInfo.body = _proxyShape.getBody();
+		raycastInfo.body = proxyShape.getBody();
-		_raycastInfo.proxyShape = _proxyShape;
+		raycastInfo.proxyShape = proxyShape;
-		_raycastInfo.hitFraction = t;
+		raycastInfo.hitFraction = t;
-		_raycastInfo.worldPoint = localHitPoint[hitIndex];
+		raycastInfo.worldPoint = localHitPoint[hitIndex];
-		_raycastInfo.worldNormal = localNormal[hitIndex];
+		raycastInfo.worldNormal = localNormal[hitIndex];
 		return true;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
+	public void setLocalScaling(final Vector3f scaling) {
-		this.halfHeight = (this.halfHeight / this.scaling.y) * _scaling.y;
+		this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
-		this.radius = (this.radius / this.scaling.x) * _scaling.x;
+		this.radius = (this.radius / this.scaling.x()) * scaling.x();
-		super.setLocalScaling(_scaling);
+		super.setLocalScaling(scaling);
 	}
 	
 	@Override
-	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+	public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) {
-		final float radiusHeight = this.radius * (-_localPoint.y + this.halfHeight) / (this.halfHeight * 2.0f);
+		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);
+		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) {
+	public ConvexMeshShape(final float margin) {
-		super(CollisionShapeType.CONVEX_MESH, _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 arrayVertices Array with the vertices of the convex mesh
-	 * @param _nbVertices Number of vertices in 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 stride Stride between the beginning of two elements in the vertices array
-	 * @param _margin Collision margin (in meters) around the collision shape
+	 * @param margin Collision margin (in meters) around the collision shape
 	 */
-	public ConvexMeshShape(final float[] _arrayVertices, final int _nbVertices, final int _stride) {
+	public ConvexMeshShape(final float[] arrayVertices, final int nbVertices, final int stride) {
-		this(_arrayVertices, _nbVertices, _stride, Defaults.OBJECT_MARGIN);
+		this(arrayVertices, nbVertices, stride, Defaults.OBJECT_MARGIN);
 	}
 	
-	public ConvexMeshShape(final float[] _arrayVertices, final int _nbVertices, final int _stride, final float _margin) {
+	public ConvexMeshShape(final float[] arrayVertices, final int nbVertices, final int stride, final float margin) {
-		super(CollisionShapeType.CONVEX_MESH, _margin);
+		super(CollisionShapeType.CONVEX_MESH, margin);
-		this.numberVertices = _nbVertices;
+		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]));
+			this.vertices.add(new Vector3f(arrayVertices[offset], arrayVertices[offset + 1], arrayVertices[offset + 2]));
-			offset += _stride;
+			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 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 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 margin Collision margin (in meters) around the collision shape
 	 */
-	public ConvexMeshShape(final TriangleVertexArray _triangleVertexArray) {
+	public ConvexMeshShape(final TriangleVertexArray triangleVertexArray) {
-		this(_triangleVertexArray, true);
+		this(triangleVertexArray, true);
 	}
 	
-	public ConvexMeshShape(final TriangleVertexArray _triangleVertexArray, final boolean _isEdgesInformationUsed) {
+	public ConvexMeshShape(final TriangleVertexArray triangleVertexArray, final boolean isEdgesInformationUsed) {
-		this(_triangleVertexArray, true, Defaults.OBJECT_MARGIN);
+		this(triangleVertexArray, true, Defaults.OBJECT_MARGIN);
 	}
 	
-	public ConvexMeshShape(final TriangleVertexArray _triangleVertexArray, final boolean _isEdgesInformationUsed, final float _margin) {
+	public ConvexMeshShape(final TriangleVertexArray triangleVertexArray, final boolean isEdgesInformationUsed, final float margin) {
-		super(CollisionShapeType.CONVEX_MESH, _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()) {
+		for (final Vector3f it : triangleVertexArray.getVertices()) {
-			this.vertices.add(it.multiplyNew(this.scaling));
+			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++) {
+			for (int iii = 0; iii < triangleVertexArray.getNbTriangles(); iii++) {
-				final int vertexIndex[] = { 0, 0, 0 };
+				final int[] vertexIndex = { 0, 0, 0 };
-				vertexIndex[0] = _triangleVertexArray.getIndices()[iii * 3];
+				vertexIndex[0] = triangleVertexArray.getIndices()[iii * 3];
-				vertexIndex[1] = _triangleVertexArray.getIndices()[iii * 3 + 1];
+				vertexIndex[1] = triangleVertexArray.getIndices()[iii * 3 + 1];
-				vertexIndex[2] = _triangleVertexArray.getIndices()[iii * 3 + 2];
+				vertexIndex[2] = triangleVertexArray.getIndices()[iii * 3 + 2];
 				// Add information about the edges
 				addEdge(vertexIndex[0], vertexIndex[1]);
 				addEdge(vertexIndex[0], vertexIndex[2]);
@@ -154,83 +153,82 @@ 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 v1 Index of the first vertex of the edge to add
-	 * @param _v2 Index of the second 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)) {
+		if (!this.edgesAdjacencyList.containsKey(v1)) {
-			this.edgesAdjacencyList.put(_v1, new SetInteger());
+			this.edgesAdjacencyList.put(v1, new SetInteger());
 		}
 		// If the entry for vertex v2 does not exist in the adjacency list
-		if (!this.edgesAdjacencyList.containsKey(_v2)) {
+		if (!this.edgesAdjacencyList.containsKey(v2)) {
-			this.edgesAdjacencyList.put(_v2, new SetInteger());
+			this.edgesAdjacencyList.put(v2, new SetInteger());
 		}
 		// Add the edge in the adjacency list
-		this.edgesAdjacencyList.get(_v1).add(_v2);
+		this.edgesAdjacencyList.get(v1).add(v2);
-		this.edgesAdjacencyList.get(_v2).add(_v1);
+		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
+		this.maxBounds = this.maxBounds.max(vertex.multiply(this.scaling));
-		if (_vertex.x * this.scaling.x > this.maxBounds.x) {
+		this.minBounds = this.minBounds.max(vertex.multiply(this.scaling));
-			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);
-		}
 	}
 	
 	@Override
-	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
+	public Matrix3f computeLocalInertiaTensor(final float mass) {
-		final float factor = (1.0f / 3.0f) * _mass;
+		final float factor = (1.0f / 3.0f) * mass;
-		final Vector3f realExtent = this.maxBounds.lessNew(this.minBounds).multiply(0.5f);
+		final Vector3f realExtent = this.maxBounds.less(this.minBounds).multiply(0.5f);
-		assert (realExtent.x > 0 && realExtent.y > 0 && realExtent.z > 0);
+		assert (realExtent.x() > 0 && realExtent.y() > 0 && realExtent.z() > 0);
-		final float xSquare = realExtent.x * realExtent.x;
+		final float xSquare = realExtent.x() * realExtent.x();
-		final float ySquare = realExtent.y * realExtent.y;
+		final float ySquare = realExtent.y() * realExtent.y();
-		final float zSquare = realExtent.z * realExtent.z;
+		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));
+		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) {
+	public Bounds getLocalBounds() {
-		_min.set(this.minBounds);
+		return new Bounds(this.minBounds, this.maxBounds);
-		_max.set(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) {
+		if (!this.isEdgesInformationUsed) {
+			// If the edges information is not used
+			double maxDotProduct = Float.MIN_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));
+				// If the current dot product is larger than the maximum one
+				if (dotProduct > maxDotProduct) {
+					indexMaxDotProduct = i;
+					maxDotProduct = dotProduct;
+				}
+			}
+			assert (maxDotProduct >= 0.0f);
+			// Return the vertex with the largest dot product in the support direction
+			return this.vertices.get(indexMaxDotProduct).multiply(this.scaling);
+		}
 		assert (this.edgesAdjacencyList.size() == this.numberVertices);
-			int maxVertex = (Integer) (_cachedCollisionData.data);
+		int maxVertex = (Integer) (cachedCollisionData.data);
-			float maxDotProduct = _direction.dot(this.vertices.get(maxVertex));
+		float maxDotProduct = direction.dot(this.vertices.get(maxVertex));
 		boolean isOptimal;
 		// Perform hill-climbing (local search)
 		do {
@@ -239,7 +237,7 @@ public class ConvexMeshShape extends ConvexShape {
 			// 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));
+				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;
@@ -249,27 +247,9 @@ public class ConvexMeshShape extends ConvexShape {
 			}
 		} while (!isOptimal);
 		// Cache the support vertex
-			_cachedCollisionData.data = maxVertex;
+		cachedCollisionData.data = maxVertex;
 		// Return the support vertex
-			return this.vertices.get(maxVertex).multiplyNew(this.scaling);
+		return this.vertices.get(maxVertex).multiply(this.scaling);
-		} else {
-			// If the edges information is not used
-			double maxDotProduct = Float.MIN_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));
-				// If the current dot product is larger than the maximum one
-				if (dotProduct > maxDotProduct) {
-					indexMaxDotProduct = i;
-					maxDotProduct = dotProduct;
-				}
-			}
-			assert (maxDotProduct >= 0.0f);
-			// Return the vertex with the largest dot product in the support direction
-			return this.vertices.get(indexMaxDotProduct).multiplyNew(this.scaling);
-		}
 	}
 	
 	/**
@@ -281,43 +261,27 @@ public class ConvexMeshShape extends ConvexShape {
 	}
 	
 	@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) {
-		return _proxyShape.getBody().getWorld().getCollisionDetection().getNarrowPhaseGJKAlgorithm().raycast(_ray, _proxyShape, _raycastInfo);
+		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.minBounds = Vector3f.ZERO;
-		this.maxBounds.setZero();
+		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.minBounds = this.minBounds.min(this.vertices.get(i));
-				this.maxBounds.setX(this.vertices.get(i).x);
+			this.maxBounds = this.maxBounds.max(this.vertices.get(i));
-			}
-			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);
-			}
 		}
 		// Apply the local scaling factor
-		this.maxBounds.multiply(this.scaling);
+		this.maxBounds = this.maxBounds.multiply(this.scaling);
-		this.minBounds.multiply(this.scaling);
+		this.minBounds = this.minBounds.multiply(this.scaling);
 		// Add the object margin to the bounds
-		this.maxBounds.add(this.margin);
+		this.maxBounds = this.maxBounds.add(this.margin);
-		this.minBounds.less(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) {
+	public void setIsEdgesInformationUsed(final boolean isEdgesUsed) {
-		this.isEdgesInformationUsed = _isEdgesUsed;
+		this.isEdgesInformationUsed = isEdgesUsed;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
+	public void setLocalScaling(final Vector3f scaling) {
-		super.setLocalScaling(_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) {
+	public ConvexShape(final CollisionShapeType type, final float margin) {
-		super(_type);
+		super(type);
-		this.margin = _margin;
+		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) {
+	public Vector3f getLocalSupportPointWithMargin(final Vector3f direction, final CacheData cachedCollisionData) {
-		////Log.error(" ->  getLocalSupportPointWithMargin(" + _direction);
+		////Log.error(" ->  getLocalSupportPointWithMargin(" + direction);
 		// Get the support point without margin
-		final Vector3f supportPoint = getLocalSupportPointWithoutMargin(_direction, _cachedCollisionData);
+		Vector3f supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
 		////Log.error(" ->  supportPoint = " + supportPoint);
 		////Log.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(" ->      direction.length2()=" + FMath.floatToString(direction.length2()));
-			////Log.error(" ->      Constant.FLOAT_EPSILON=" + FMath.floatToString(Constant.FLOAT_EPSILON));
+			////Log.error(" ->      Constant.FLOATEPSILON=" + FMath.floatToString(Constant.FLOATEPSILON));
-			if (_direction.length2() > Constant.FLOAT_EPSILON * Constant.FLOAT_EPSILON) {
+			if (direction.length2() > Constant.FLOAT_EPSILON * Constant.FLOAT_EPSILON) {
-				unitVec = _direction.safeNormalizeNew();
+				unitVec = direction.safeNormalize();
 				////Log.error(" ->          unitVec= " + unitVec);
 			}
-			supportPoint.add(unitVec.multiplyNew(this.margin));
+			supportPoint = supportPoint.add(unitVec.multiply(this.margin));
 		}
 		////Log.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) {
+	public CylinderShape(final float radius, final float height) {
-		this(_radius, _height, Defaults.OBJECT_MARGIN);
+		this(radius, height, Defaults.OBJECT_MARGIN);
 	}
 	
-	public CylinderShape(final float _radius, final float _height, final float _margin) {
+	public CylinderShape(final float radius, final float height, final float margin) {
-		super(CollisionShapeType.CYLINDER, _margin);
+		super(CollisionShapeType.CYLINDER, margin);
-		this.radius = _radius;
+		this.radius = radius;
-		this.halfHeight = _height / 2.0f;
+		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);
+		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);
+		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) {
+	public Bounds getLocalBounds() {
-		// Maximum bounds
+		Vector3f max = new Vector3f(this.radius + this.margin, this.halfHeight + this.margin, this.radius + this.margin);
-		max.setX(this.radius + this.margin);
+		return new Bounds(max.multiply(-1), max);
-		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);
 	}
 	
 	@Override
-	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f direction, final CacheData cachedCollisionData) {
-		final Vector3f supportPoint = new Vector3f(0.0f, 0.0f, 0.0f);
+		Vector3f supportPoint = Vector3f.ZERO;
-		final float uDotv = _direction.y;
+		final float uDotv = direction.y();
-		final Vector3f w = new Vector3f(_direction.x, 0.0f, _direction.z);
+		final Vector3f w = new Vector3f(direction.x(), 0.0f, direction.z());
-		final float lengthW = FMath.sqrt(_direction.x * _direction.x + _direction.z * _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) {
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
-		final Vector3f n = _ray.point2.lessNew(_ray.point1);
+		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 d = q.less(p);
-		final Vector3f m = _ray.point1.lessNew(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);
+				final Vector3f localHitPoint = n.multiply(t).add(ray.point1);
-				_raycastInfo.body = _proxyShape.getBody();
+				raycastInfo.body = proxyShape.getBody();
-				_raycastInfo.proxyShape = _proxyShape;
+				raycastInfo.proxyShape = proxyShape;
-				_raycastInfo.hitFraction = t;
+				raycastInfo.hitFraction = t;
-				_raycastInfo.worldPoint = localHitPoint;
+				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);
+				final Vector3f localHitPoint = n.multiply(t).add(ray.point1);
-				_raycastInfo.body = _proxyShape.getBody();
+				raycastInfo.body = proxyShape.getBody();
-				_raycastInfo.proxyShape = _proxyShape;
+				raycastInfo.proxyShape = proxyShape;
-				_raycastInfo.hitFraction = t;
+				raycastInfo.hitFraction = t;
-				_raycastInfo.worldPoint = localHitPoint;
+				raycastInfo.worldPoint = localHitPoint;
-				_raycastInfo.worldNormal = new Vector3f(0, 1.0f, 0);
+				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);
+			final Vector3f localHitPoint = n.multiply(t).add(ray.point1);
-			_raycastInfo.body = _proxyShape.getBody();
+			raycastInfo.body = proxyShape.getBody();
-			_raycastInfo.proxyShape = _proxyShape;
+			raycastInfo.proxyShape = proxyShape;
-			_raycastInfo.hitFraction = t;
+			raycastInfo.hitFraction = t;
-			_raycastInfo.worldPoint = localHitPoint;
+			raycastInfo.worldPoint = localHitPoint;
-			_raycastInfo.worldNormal = new Vector3f(0, -1.0f, 0);
+			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);
+			final Vector3f localHitPoint = n.multiply(t).add(ray.point1);
-			_raycastInfo.body = _proxyShape.getBody();
+			raycastInfo.body = proxyShape.getBody();
-			_raycastInfo.proxyShape = _proxyShape;
+			raycastInfo.proxyShape = proxyShape;
-			_raycastInfo.hitFraction = t;
+			raycastInfo.hitFraction = t;
-			_raycastInfo.worldPoint = localHitPoint;
+			raycastInfo.worldPoint = localHitPoint;
-			_raycastInfo.worldNormal = new Vector3f(0, 1.0f, 0);
+			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);
+		final Vector3f localHitPoint = n.multiply(t).add(ray.point1);
-		_raycastInfo.body = _proxyShape.getBody();
+		raycastInfo.body = proxyShape.getBody();
-		_raycastInfo.proxyShape = _proxyShape;
+		raycastInfo.proxyShape = proxyShape;
-		_raycastInfo.hitFraction = t;
+		raycastInfo.hitFraction = t;
-		_raycastInfo.worldPoint = localHitPoint;
+		raycastInfo.worldPoint = localHitPoint;
-		final Vector3f v = localHitPoint.lessNew(p);
+		final Vector3f v = localHitPoint.less(p);
-		final Vector3f w = d.multiplyNew(v.dot(d) / d.length2());
+		final Vector3f w = d.multiply(v.dot(d) / d.length2());
-		final Vector3f normalDirection = localHitPoint.lessNew(p.addNew(w));
+		final Vector3f normalDirection = localHitPoint.less(p.add(w));
-		_raycastInfo.worldNormal = normalDirection;
+		raycastInfo.worldNormal = normalDirection;
 		return true;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
+	public void setLocalScaling(final Vector3f scaling) {
-		this.halfHeight = (this.halfHeight / this.scaling.y) * _scaling.y;
+		this.halfHeight = (this.halfHeight / this.scaling.y()) * scaling.y();
-		this.radius = (this.radius / this.scaling.x) * _scaling.x;
+		this.radius = (this.radius / this.scaling.x()) * scaling.x();
-		super.setLocalScaling(_scaling);
+		super.setLocalScaling(scaling);
 	}
 	
 	@Override
-	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+	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);
+		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) {
+		public TriangleOverlapCallback(final Ray ray, final ProxyShape proxyShape, final RaycastInfo raycastInfo, final HeightFieldShape heightFieldShape) {
-			this.ray = _ray;
+			this.ray = ray;
-			this.proxyShape = _proxyShape;
+			this.proxyShape = proxyShape;
-			this.raycastInfo = _raycastInfo;
+			this.raycastInfo = raycastInfo;
-			this.heightFieldShape = _heightFieldShape;
+			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 float width; //!< Height field width
-	protected AABB AABB; //!< Local AABB of the height field (without scaling)
 	
 	/**
 		 *  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) {
+	public HeightFieldShape(final int nbGridColumns, final int nbGridRows, final float minHeight, final float maxHeight, final float[] heightFieldData) {
-		this(_nbGridColumns, _nbGridRows, _minHeight, _maxHeight, _heightFieldData, 1);
+		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.numberColumns = nbGridColumns;
-		this.numberRows = _nbGridRows;
+		this.numberRows = nbGridRows;
-		this.width = _nbGridColumns - 1;
+		this.width = nbGridColumns - 1;
-		this.length = _nbGridRows - 1;
+		this.length = nbGridRows - 1;
-		this.minHeight = _minHeight;
+		this.minHeight = minHeight;
-		this.maxHeight = _maxHeight;
+		this.maxHeight = maxHeight;
-		this.upAxis = _upAxis;
+		this.upAxis = upAxis;
-		assert (_nbGridColumns >= 2);
+		assert (nbGridColumns >= 2);
-		assert (_nbGridRows >= 2);
+		assert (nbGridRows >= 2);
 		assert (this.width >= 1);
 		assert (this.length >= 1);
-		assert (_minHeight <= _maxHeight);
+		assert (minHeight <= maxHeight);
-		assert (_upAxis == 0 || _upAxis == 1 || _upAxis == 2);
+		assert (upAxis == 0 || upAxis == 1 || upAxis == 2);
-		this.heightFieldData = _heightFieldData;
+		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.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.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.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.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.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.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) {
+	protected int computeIntegerGridValue(final float value) {
-		return (int) ((_value < 0.0f) ? _value - 0.5f : _value + 0.5f);
+		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());
+		Vector3f minPoint = FMath.max(aabbToCollide.getMin(), this.aabb.getMin());
-		minPoint = FMath.min(minPoint, this.AABB.getMax());
+		minPoint = FMath.min(minPoint, this.aabb.getMax());
-		Vector3f maxPoint = FMath.min(_aabbToCollide.getMax(), this.AABB.getMax());
+		Vector3f maxPoint = FMath.min(aabbToCollide.getMax(), this.aabb.getMax());
-		maxPoint = FMath.max(maxPoint, this.AABB.getMin());
+		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);
+		final Vector3f translateVec = this.aabb.getExtent().multiply(0.5f);
-		minPoint.add(translateVec);
+		minPoint = minPoint.add(translateVec);
-		maxPoint.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);
+		Vector3f minCoords = new Vector3f(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 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) {
+	protected float getHeightAt(final int xxx, final int yyy) {
-		return this.heightFieldData[_yyy * this.numberColumns + _xxx];
+		return this.heightFieldData[yyy * this.numberColumns + xxx];
 	}
 	
 	@Override
-	public void getLocalBounds(final Vector3f _min, final Vector3f _max) {
+	public Bounds getLocalBounds() {
-		_min.set(this.AABB.getMin().multiplyNew(this.scaling));
+		return new Bounds(this.aabb.getMin().multiply(this.scaling), this.aabb.getMax().multiply(this.scaling));
-		_max.set(this.AABB.getMax().multiplyNew(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,
+	protected		void getTriangleVerticesWithIndexPointer(final long subPart,
-		                                         final long _triangleIndex,
+		                                         final long triangleIndex,
-		                                         Vector3f* _outTriangleVertices) ;
+		                                         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) {
+	public void setLocalScaling(final Vector3f scaling) {
-		super.setLocalScaling(_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();
+		Bounds bounds = computeMinMaxGridCoordinates(aabb);
-		final Vector3f maxGridCoords = new Vector3f();
-		computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, 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);
+				iMin = FMath.clamp((int) bounds.min().y(), 0, this.numberColumns - 1);
-				iMax = FMath.clamp((int) maxGridCoords.y, 0, this.numberColumns - 1);
+				iMax = FMath.clamp((int) bounds.max().y(), 0, this.numberColumns - 1);
-				jMin = FMath.clamp((int) minGridCoords.z, 0, this.numberRows - 1);
+				jMin = FMath.clamp((int) bounds.min().z(), 0, this.numberRows - 1);
-				jMax = FMath.clamp((int) maxGridCoords.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);
+				iMin = FMath.clamp((int) bounds.min().x(), 0, this.numberColumns - 1);
-				iMax = FMath.clamp((int) maxGridCoords.x, 0, this.numberColumns - 1);
+				iMax = FMath.clamp((int) bounds.max().x(), 0, this.numberColumns - 1);
-				jMin = FMath.clamp((int) minGridCoords.z, 0, this.numberRows - 1);
+				jMin = FMath.clamp((int) bounds.min().z(), 0, this.numberRows - 1);
-				jMax = FMath.clamp((int) maxGridCoords.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);
+				iMin = FMath.clamp((int) bounds.min().x(), 0, this.numberColumns - 1);
-				iMax = FMath.clamp((int) maxGridCoords.x, 0, this.numberColumns - 1);
+				iMax = FMath.clamp((int) bounds.max().x(), 0, this.numberColumns - 1);
-				jMin = FMath.clamp((int) minGridCoords.y, 0, this.numberRows - 1);
+				jMin = FMath.clamp((int) bounds.min().y(), 0, this.numberRows - 1);
-				jMax = FMath.clamp((int) maxGridCoords.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) {
+	public SphereShape(final float radius) {
-		super(CollisionShapeType.SPHERE, _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.setMin(transform.getPosition().less(extents));
-		_aabb.setMax(_transform.getPosition().addNew(extents));
+		aabb.setMax(transform.getPosition().add(extents));
 	}
 	
 	@Override
-	public void computeLocalInertiaTensor(final Matrix3f _tensor, final float _mass) {
+	public Matrix3f computeLocalInertiaTensor(final float mass) {
-		final float diag = 0.4f * _mass * this.margin * this.margin;
+		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);
+		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) {
+	public Bounds getLocalBounds() {
-		// Maximum bounds
+		return new Bounds(new Vector3f(-this.margin, -this.margin, -this.margin), new Vector3f(this.margin, this.margin, this.margin));
-		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);
 	}
 	
 	@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) {
+	public boolean raycast(final Ray ray, final RaycastInfo raycastInfo, final ProxyShape proxyShape) {
-		final Vector3f m = _ray.point1;
+		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.body = proxyShape.getBody();
-			_raycastInfo.proxyShape = _proxyShape;
+			raycastInfo.proxyShape = proxyShape;
-			_raycastInfo.hitFraction = t;
+			raycastInfo.hitFraction = t;
-			_raycastInfo.worldPoint = rayDirection.multiplyNew(t).add(_ray.point1);
+			raycastInfo.worldPoint = rayDirection.multiply(t).add(ray.point1);
-			_raycastInfo.worldNormal = _raycastInfo.worldPoint;
+			raycastInfo.worldNormal = raycastInfo.worldPoint;
 			return true;
 		}
 		return false;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
+	public void setLocalScaling(final Vector3f scaling) {
-		this.margin = (this.margin / this.scaling.x) * _scaling.x;
+		this.margin = (this.margin / this.scaling.x()) * scaling.x();
-		super.setLocalScaling(_scaling);
+		super.setLocalScaling(scaling);
 	}
 	
 	@Override
-	public boolean testPointInside(final Vector3f _localPoint, final ProxyShape _proxyShape) {
+	public boolean testPointInside(final Vector3f localPoint, final ProxyShape proxyShape) {
-		return (_localPoint.length2() < this.margin * this.margin);
+		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 front triangle
-		BACK, //!< Raycast against back triangle
+		FRONT, //!< Raycast against back triangle
-		FRONT_AND_BACK //!< Raycast against front and 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[0] = point1;
-		this.points[1] = _point2;
+		this.points[1] = point2;
-		this.points[2] = _point3;
+		this.points[2] = point3;
 		this.raycastTestType = TriangleRaycastSide.FRONT;
 	}
 	
 	/**
 		 *  Constructor
-		 * @param _point1 First point of the triangle
+		 * @param point1 First point of the triangle
-		 * @param _point2 Second point of the triangle
+		 * @param point2 Second point of the triangle
-		 * @param _point3 Third point of the triangle
+		 * @param point3 Third point of the triangle
-		 * @param _margin The collision margin (in meters) around the collision shape
+		 * @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) {
+	public TriangleShape(final Vector3f point1, final Vector3f point2, final Vector3f point3, final float margin) {
-		super(CollisionShapeType.TRIANGLE, _margin);
+		super(CollisionShapeType.TRIANGLE, margin);
-		this.points[0] = _point1;
+		this.points[0] = point1;
-		this.points[1] = _point2;
+		this.points[1] = point2;
-		this.points[2] = _point3;
+		this.points[2] = point3;
 		this.raycastTestType = TriangleRaycastSide.FRONT;
 	}
 	
 	@Override
-	public void computeAABB(final AABB aabb, final Transform3D _transform) {
+	public void computeAABB(final AABB aabb, final Transform3D transform) {
-		final Vector3f worldPoint1 = _transform.multiplyNew(this.points[0]);
+		final Vector3f worldPoint1 = transform.multiply(this.points[0]);
-		final Vector3f worldPoint2 = _transform.multiplyNew(this.points[1]);
+		final Vector3f worldPoint2 = transform.multiply(this.points[1]);
-		final Vector3f worldPoint3 = _transform.multiplyNew(this.points[2]);
+		final Vector3f worldPoint3 = transform.multiply(this.points[2]);
-		final Vector3f xAxis = new Vector3f(worldPoint1.x, worldPoint2.x, worldPoint3.x);
+		final Vector3f xAxis = new Vector3f(worldPoint1.x(), worldPoint2.x(), worldPoint3.x());
-		final Vector3f yAxis = new Vector3f(worldPoint1.y, worldPoint2.y, worldPoint3.y);
+		final Vector3f yAxis = new Vector3f(worldPoint1.y(), worldPoint2.y(), worldPoint3.y());
-		final Vector3f zAxis = new Vector3f(worldPoint1.z, worldPoint2.z, worldPoint3.z);
+		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) {
+	public Matrix3f computeLocalInertiaTensor(final float mass) {
-		_tensor.setZero();
+		return Matrix3f.ZERO;
 	}
 	
 	@Override
-	public void getLocalBounds(final Vector3f min, final Vector3f max) {
+	public Bounds getLocalBounds() {
-		final Vector3f xAxis = new Vector3f(this.points[0].x, this.points[1].x, this.points[2].x);
+		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 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);
+		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);
+		return new Bounds(new Vector3f(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);
+				new Vector3f(xAxis.getMax() + this.margin, yAxis.getMax() + this.margin, zAxis.getMax() + this.margin));
 	}
 	
 	@Override
-	public Vector3f getLocalSupportPointWithoutMargin(final Vector3f _direction, final CacheData _cachedCollisionData) {
+	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]));
+		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) {
+	public Vector3f getVertex(final int index) {
-		assert (_index >= 0 && _index < 3);
+		assert (index >= 0 && index < 3);
-		return this.points[_index];
+		return this.points[index];
 	}
 	
 	@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) {
-		final Vector3f pq = _ray.point2.lessNew(_ray.point1);
+		final Vector3f pq = ray.point2.less(ray.point1);
-		final Vector3f pa = this.points[0].lessNew(_ray.point1);
+		final Vector3f pa = this.points[0].less(ray.point1);
-		final Vector3f pb = this.points[1].lessNew(_ray.point1);
+		final Vector3f pb = this.points[1].less(ray.point1);
-		final Vector3f pc = this.points[2].lessNew(_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 Vector3f localHitPoint = this.points[0].multiply(u).add(this.points[1].multiply(v)).add(this.points[2].multiply(w));
-		final float hitFraction = localHitPoint.lessNew(_ray.point1).length() / pq.length();
+		final float hitFraction = localHitPoint.less(ray.point1).length() / pq.length();
-		if (hitFraction < 0.0f || hitFraction > _ray.maxFraction) {
+		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.body = proxyShape.getBody();
-		_raycastInfo.proxyShape = _proxyShape;
+		raycastInfo.proxyShape = proxyShape;
-		_raycastInfo.worldPoint = localHitPoint;
+		raycastInfo.worldPoint = localHitPoint;
-		_raycastInfo.hitFraction = hitFraction;
+		raycastInfo.hitFraction = hitFraction;
-		_raycastInfo.worldNormal = localHitNormal;
+		raycastInfo.worldNormal = localHitNormal;
 		return true;
 	}
 	
 	@Override
-	public void setLocalScaling(final Vector3f _scaling) {
+	public void setLocalScaling(final Vector3f scaling) {
-		this.points[0].divide(this.scaling).multiply(_scaling);
+		this.points[0] = this.points[0].divide(this.scaling).multiply(scaling);
-		this.points[1].divide(this.scaling).multiply(_scaling);
+		this.points[1] = this.points[1].divide(this.scaling).multiply(scaling);
-		this.points[2].divide(this.scaling).multiply(_scaling);
+		this.points[2] = this.points[2].divide(this.scaling).multiply(scaling);
-		super.setLocalScaling(_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) {
+	public void setRaycastTestType(final TriangleRaycastSide testType) {
-		this.raycastTestType = _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;
-	
+	// Smallest decimal value (negative)
-	// Default internal constant timestep in seconds
+	public static final float DECIMAL_SMALLEST = Float.MIN_VALUE;
-	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;
 	
 	// Default bounciness factor for a rigid body
 	public static final float DEFAULT_BOUNCINESS = 0.5f;
 	
-	// True if the spleeping technique is enabled
+	// Default friction coefficient for a rigid body
-	public static final boolean SPLEEPING_ENABLED = true;
+	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
+	// True if the spleeping technique is enabled
-	public static final int DEFAULT_VELOCITY_SOLVER_NUM_ITERATIONS = 10;
+	public static final boolean SPLEEPING_ENABLED = true;
 	
-	// Number of iterations when solving the position constraints of the Sequential Impulse technique
+	private Defaults() {}
-	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;
 }

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 static final 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 biasVector = Vector3f.ZERO; //!< Bias vector for the raint
-	private Vector3f localAnchorPointBody2 = new Vector3f(0, 0, 0); //!< Anchor point of body 2 (in local-space coordinates of body 2)
+	private Matrix3f i1 = Matrix3f.IDENTITY; //!< Inertia tensor of body 1 (in world-space coordinates)
-	private Vector3f r1World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space
+	private Matrix3f i2 = Matrix3f.IDENTITY; //!< Inertia tensor of body 2 (in world-space coordinates)
-	private Vector3f r2World = new Vector3f(0, 0, 0); //!< Vector from center of body 2 to anchor point in world-space
+	private Vector3f impulse = Vector3f.ZERO; //!< Accumulated impulse
-	private Matrix3f i1 = new Matrix3f(); //!< Inertia tensor of body 1 (in world-space coordinates)
+	private Matrix3f inverseMassMatrix = Matrix3f.IDENTITY; //!< Inverse mass matrix K=JM^-1J^-t of the raint
-	private Matrix3f i2 = new Matrix3f(); //!< Inertia tensor of body 2 (in world-space coordinates)
+	private Vector3f localAnchorPointBody1 = Vector3f.ZERO; //!< Anchor point of body 1 (in local-space coordinates of body 1)
-	private Vector3f biasVector = new Vector3f(0, 0, 0); //!< Bias vector for the raint
+	private Vector3f localAnchorPointBody2 = Vector3f.ZERO; //!< Anchor point of body 2 (in local-space coordinates of body 2)
-	private Matrix3f inverseMassMatrix = new Matrix3f(); //!< Inverse mass matrix K=JM^-1J^-t of the raint
+	private Vector3f r1World = Vector3f.ZERO; //!< Vector from center of body 2 to anchor point in world-space
-	private final Vector3f impulse = new Vector3f(0, 0, 0); //!< Accumulated impulse
+	private Vector3f r2World = Vector3f.ZERO; //!< Vector from center of body 2 to anchor point in world-space
-	/// Constructor
 	
 	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);
+		Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
-		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU1.multiply(this.i1).multiply(skewSymmetricMatrixU1.transpose()));
-		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew()));
+		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);
+			final float biasFactor = (BallAndSocketJoint.BETA / raintSolverData.timeStep);
-			this.biasVector = (x2.addNew(this.r2World).less(x1).less(this.r1World)).multiply(biasFactor);
+			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];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		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));
+		final Vector3f deltaLambda = this.inverseMassMatrix.multiply(jv.multiply(-1).less(this.biasVector));
-		this.impulse.add(deltaLambda);
+		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));
+		v1 = v1.add(linearImpulseBody1.multiply(this.body1.massInverse));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		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));
+		v2 = v2.add(deltaLambda.multiply(this.body2.massInverse));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		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];
+		Vector3f x1 = raintSolverData.positions[this.indexBody1];
-		final Vector3f x2 = raintSolverData.positions[this.indexBody2];
+		Vector3f x2 = raintSolverData.positions[this.indexBody2];
-		final Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+		Quaternion q1 = raintSolverData.orientations[this.indexBody1];
-		final Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+		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);
+		Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
-		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiplyNew(skewSymmetricMatrixU1.transposeNew()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU1.multiply(this.i1).multiply(skewSymmetricMatrixU1.transpose()));
-		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiplyNew(skewSymmetricMatrixU2.transposeNew()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU2.multiply(this.i2).multiply(skewSymmetricMatrixU2.transpose()));
-		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 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 v1 = linearImpulseBody1.multiply(inverseMassBody1);
-		final Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1);
+		final Vector3f w1 = this.i1.multiply(angularImpulseBody1);
 		
 		// Update the body center of mass and orientation of body 1
-		x1.add(v1);
+		x1 = x1.add(v1);
-		q1.add((new Quaternion(0, w1)).multiplyNew(q1).multiplyNew(0.5f));
+		q1 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
-		q1.normalize();
+		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 v2 = lambda.multiply(inverseMassBody2);
-		final Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2);
+		final Vector3f w2 = this.i2.multiply(angularImpulseBody2);
 		
 		// Update the body position/orientation of body 2
-		x2.add(v2);
+		x2 = x2.add(v2);
-		q2.add((new Quaternion(0, w2)).multiplyNew(q2).multiply(0.5f));
+		q2 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
-		q2.normalize();
+		q2 = q2.normalize();
 	}
 	
 	@Override
 	public void warmstart(final ConstraintSolverData raintSolverData) {
 		
 		// Get the velocities
-		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		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));
+		v1 = v1.add(linearImpulseBody1.multiply(this.body1.massInverse));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		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));
+		v2 = v2.add(this.impulse.multiply(this.body2.massInverse));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		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 rigidBody1 Pointer to the first body of the joint
-	 * @param _rigidBody2 Pointer to the second body of the joint
+	 * @param rigidBody2 Pointer to the second body of the joint
-	 * @param _initAnchorPointWorldSpace The anchor point in world-space coordinates
+	 * @param initAnchorPointWorldSpace The anchor point in world-space coordinates
 	 */
-	public BallAndSocketJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace) {
+	public BallAndSocketJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace) {
-		super(_rigidBody1, _rigidBody2, JointType.BALLSOCKETJOINT);
+		super(rigidBody1, rigidBody2, JointType.BALLSOCKETJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		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 final Vector3f normal; //!< Normalized normal vector of the contact (from body1 toward body2) in world space
-	private Vector3f worldPointOnBody2; //!< Contact point on body 2 in world space
+	private float penetrationDepth; //!< Penetration depth
-	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 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.localPointOnBody1 = contactInfo.localPoint1;
-		this.localPointOnBody2 = contactInfo.localPoint2.clone();
+		this.localPointOnBody2 = contactInfo.localPoint2;
-		this.worldPointOnBody1 = contactInfo.shape1.getBody().getTransform().multiplyNew(contactInfo.shape1.getLocalToBodyTransform().multiply(contactInfo.localPoint1));
+		this.worldPointOnBody1 = contactInfo.shape1.getBody().getTransform().multiply(contactInfo.shape1.getLocalToBodyTransform().multiply(contactInfo.localPoint1));
-		this.worldPointOnBody2 = contactInfo.shape2.getBody().getTransform().multiplyNew(contactInfo.shape2.getLocalToBodyTransform().multiply(contactInfo.localPoint2));
+		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.frictionVectors1 = new Vector3f(0, 0, 0);
-		this.frictionVectors_2 = 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,
+	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) {
+			final float penetrationDepth, final Vector3f localPoint1, final Vector3f localPoint2) {
-		this.shape1 = _proxyShape1;
+		this.shape1 = proxyShape1;
-		this.shape2 = _proxyShape2;
+		this.shape2 = proxyShape2;
-		this.collisionShape1 = _collShape1;
+		this.collisionShape1 = collShape1;
-		this.collisionShape2 = _collShape2;
+		this.collisionShape2 = collShape2;
-		this.normal = _normal;
+		this.normal = normal;
-		this.penetrationDepth = _penetrationDepth;
+		this.penetrationDepth = penetrationDepth;
-		this.localPoint1 = _localPoint1;
+		this.localPoint1 = localPoint1;
-		this.localPoint2 = _localPoint2;
+		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 static final 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 biasRotation = Vector3f.ZERO; //!< 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 biasTranslation = Vector3f.ZERO; //!< 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 Matrix3f i1 = Matrix3f.IDENTITY; //!< 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 i2 = Matrix3f.IDENTITY; //!< 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 Vector3f impulseRotation = Vector3f.ZERO; //!< 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 = Vector3f.ZERO; //!< 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 Quaternion initOrientationDifferenceInv = Quaternion.IDENTITY; //!< Accumulated impulse for the 3 translation raints
-	protected Vector3f impulseRotation = new Vector3f(0, 0, 0); //!< Accumulate impulse for the 3 rotation raints
+	protected Matrix3f inverseMassMatrixRotation = Matrix3f.IDENTITY; //!< 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 inverseMassMatrixTranslation = Matrix3f.IDENTITY; //!< 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 localAnchorPointBody1 = Vector3f.ZERO; //!< 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 localAnchorPointBody2 = Vector3f.ZERO; //!< Bias vector for the 3 translation raints
-	protected Vector3f biasRotation = new Vector3f(0, 0, 0); //!< Bias vector for the 3 rotation raints
+	protected Vector3f r1World = Vector3f.ZERO; //!< Bias vector for the 3 rotation raints
-	protected Quaternion initOrientationDifferenceInv = new Quaternion(); //!< Inverse of the initial orientation difference between the two bodies
+	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 = transform2.getOrientation().multiply(transform1.getOrientation().inverse());
-		this.initOrientationDifferenceInv.normalize();
+		this.initOrientationDifferenceInv = this.initOrientationDifferenceInv.normalize();
-		this.initOrientationDifferenceInv.inverse();
+		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);
+		Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
-		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU1.multiply(this.i1).multiply(skewSymmetricMatrixU1.transpose()));
-		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew()));
+		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);
+		final float biasFactor = (FixedJoint.BETA / raintSolverData.timeStep);
-		this.biasTranslation.setZero();
+		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());
+			final Quaternion currentOrientationDifference = orientationBody2.multiply(orientationBody1.inverse()).normalize();
-			currentOrientationDifference.normalize();
+			final Quaternion qError = currentOrientationDifference.multiply(this.initOrientationDifferenceInv);
-			final Quaternion qError = currentOrientationDifference.multiplyNew(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.impulseTranslation = Vector3f.ZERO;
-			this.impulseRotation.setZero();
+			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];
+		Vector3f x1 = raintSolverData.positions[this.indexBody1];
-		final Vector3f x2 = raintSolverData.positions[this.indexBody2];
+		Vector3f x2 = raintSolverData.positions[this.indexBody2];
-		final Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+		Quaternion q1 = raintSolverData.orientations[this.indexBody1];
-		final Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+		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);
+		Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
-		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).add(skewSymmetricMatrixU1.transposeNew()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU1.multiply(this.i1).add(skewSymmetricMatrixU1.transpose()));
-		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).add(skewSymmetricMatrixU2.transposeNew()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU2.multiply(this.i2).add(skewSymmetricMatrixU2.transpose()));
-		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 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);
+		final Vector3f v1 = linearImpulseBody1.multiply(inverseMassBody1);
-		Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1);
+		Vector3f w1 = this.i1.multiply(angularImpulseBody1);
 		
 		// Update the body position/orientation of body 1
-		x1.add(v1);
+		x1 = x1.add(v1);
-		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
-		q1.normalize();
+		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);
+		final Vector3f v2 = lambdaTranslation.multiply(inverseMassBody2);
-		Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2);
+		Vector3f w2 = this.i2.multiply(angularImpulseBody2);
 		
 		// Update the body position/orientation of body 2
-		x2.add(v2);
+		x2 = x2.add(v2);
-		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
+		q2 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
-		q2.normalize();
+		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());
+		final Quaternion currentOrientationDifference = q2.multiply(q1.inverse()).normalize();
-		currentOrientationDifference.normalize();
+		final Quaternion qError = currentOrientationDifference.multiply(this.initOrientationDifferenceInv);
-		final Quaternion qError = currentOrientationDifference.multiplyNew(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 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
-		q1.normalize();
+		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 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
-		q2.normalize();
+		q2 = q2.normalize();
 	}
 	
 	@Override
 	public void solveVelocityConstraint(final ConstraintSolverData raintSolverData) {
 		
 		// Get the velocities
-		final Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		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));
+		final Vector3f deltaLambda = this.inverseMassMatrixTranslation.multiply(jvTranslation.multiply(-1).less(this.biasTranslation));
-		this.impulseTranslation.add(deltaLambda);
+		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));
+		v1 = v1.add(linearImpulseBody1.multiply(inverseMassBody1));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		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));
+		v2 = v2.add(deltaLambda.multiply(inverseMassBody2));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		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));
+		final Vector3f deltaLambda2 = this.inverseMassMatrixRotation.multiply(jvRotation.multiply(-1).less(this.biasRotation));
-		this.impulseRotation.add(deltaLambda2);
+		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];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		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);
+		Vector3f linearImpulseBody1 = this.impulseTranslation.multiply(-1);
-		final Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
+		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));
+		v1 = v1.add(linearImpulseBody1.multiply(inverseMassBody1));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		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));
+		v2 = v2.add(this.impulseTranslation.multiply(inverseMassBody2));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		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 rigidBody1 The first body of the joint
-	 * @param _rigidBody2 The second 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 initAnchorPointWorldSpace The initial anchor point of the joint in world-space coordinates
 	 */
-	FixedJointInfo(final RigidBody _rigidBody1, final RigidBody _rigidBody2, final Vector3f _initAnchorPointWorldSpace) {
+	FixedJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace) {
-		super(_rigidBody1, _rigidBody2, JointType.FIXEDJOINT);
+		super(rigidBody1, rigidBody2, JointType.FIXEDJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		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 static final float BETA = 0.2f; //!< 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 Vector3f b2CrossA1; //!< Cross product of vector b2 and a1
-	private final Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
+	private float bLowerLimit; //!< Bias of the lower limit raint
-	private final Vector3f hingeLocalAxisBody1; //!< Hinge rotation axis (in local-space coordinates of body 1)
+	private Vector2f bRotation; //!< Bias vector for the error correction for the rotation raints
-	private final Vector3f hingeLocalAxisBody2; //!< Hinge rotation axis (in local-space coordiantes of body 2)
+	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 Vector2f impulseRotation; //!< Impulse for the 2 rotation raints
-	private Matrix2f inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^t 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 Matrix2f inverseMassMatrixRotation; //!< Inverse mass matrix K=JM^-1J^t for the 2 rotation raints
-	private Vector2f bRotation; //!< Bias vector for the error correction for the rotation raints
+	private Matrix3f inverseMassMatrixTranslation; //!< Inverse mass matrix K=JM^-1J^t for the 3 translation 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 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.hingeLocalAxisBody1 = transform1.getOrientation().inverse().multiply(jointInfo.rotationAxisWorld);
-		this.hingeLocalAxisBody2 = transform2.getOrientation().inverseNew().multiply(jointInfo.rotationAxisWorld);
+		this.hingeLocalAxisBody2 = transform2.getOrientation().inverse().multiply(jointInfo.rotationAxisWorld);
-		this.hingeLocalAxisBody1.normalize();
+		this.hingeLocalAxisBody1 = this.hingeLocalAxisBody1.normalize();
-		this.hingeLocalAxisBody2.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 = transform2.getOrientation().multiply(transform1.getOrientation().inverse()).normalize().inverse();
-		this.initOrientationDifferenceInv.normalize();
-		this.initOrientationDifferenceInv.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());
+		Quaternion currentOrientationDiff = orientationBody2.multiply(orientationBody1.inverse());
-		currentOrientationDiff.normalize();
+		currentOrientationDiff = currentOrientationDiff.normalize();
 		
 		// Compute the relative rotation idering the initial orientation difference
-		final Quaternion relativeRotation = currentOrientationDiff.multiplyNew(this.initOrientationDifferenceInv);
+		Quaternion relativeRotation = currentOrientationDiff.multiply(this.initOrientationDifferenceInv);
-		relativeRotation.normalize();
+		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);
+		Vector3f a2 = orientationBody2.multiply(this.hingeLocalAxisBody2);
-		this.mA1.normalize();
+		this.mA1 = this.mA1.normalize();
-		a2.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);
+		Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
-		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU1.multiply(this.i1).multiply(skewSymmetricMatrixU1.transpose()));
-		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU2.multiply(this.i2).multiply(skewSymmetricMatrixU2.transpose()));
-		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 translation raints
-		this.bTranslation.setZero();
+		this.bTranslation = Vector3f.ZERO;
-		final float biasFactor = (BETA / raintSolverData.timeStep);
+		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 i1B2CrossA1 = this.i1.multiply(this.b2CrossA1);
-		final Vector3f I1C2CrossA1 = this.i1.multiplyNew(this.c2CrossA1);
+		final Vector3f i1C2CrossA1 = this.i1.multiply(this.c2CrossA1);
-		final Vector3f I2B2CrossA1 = this.i2.multiplyNew(this.b2CrossA1);
+		final Vector3f i2B2CrossA1 = this.i2.multiply(this.b2CrossA1);
-		final Vector3f I2C2CrossA1 = this.i2.multiplyNew(this.c2CrossA1);
+		final Vector3f i2C2CrossA1 = this.i2.multiply(this.c2CrossA1);
-		final float el11 = this.b2CrossA1.dot(I1B2CrossA1) + this.b2CrossA1.dot(I2B2CrossA1);
+		final float el11 = this.b2CrossA1.dot(i1B2CrossA1) + this.b2CrossA1.dot(i2B2CrossA1);
-		final float el12 = this.b2CrossA1.dot(I1C2CrossA1) + this.b2CrossA1.dot(I2C2CrossA1);
+		final float el12 = this.b2CrossA1.dot(i1C2CrossA1) + this.b2CrossA1.dot(i2C2CrossA1);
-		final float el21 = this.c2CrossA1.dot(I1B2CrossA1) + this.c2CrossA1.dot(I2B2CrossA1);
+		final float el21 = this.c2CrossA1.dot(i1B2CrossA1) + this.c2CrossA1.dot(i2B2CrossA1);
-		final float el22 = this.c2CrossA1.dot(I1C2CrossA1) + this.c2CrossA1.dot(I2C2CrossA1);
+		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.impulseTranslation = Vector3f.ZERO;
-			this.impulseRotation.setZero();
+			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];
+		Vector3f x1 = raintSolverData.positions[this.indexBody1];
-		final Vector3f x2 = raintSolverData.positions[this.indexBody2];
+		Vector3f x2 = raintSolverData.positions[this.indexBody2];
-		final Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+		Quaternion q1 = raintSolverData.orientations[this.indexBody1];
-		final Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+		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);
+		Vector3f a2 = q2.multiply(this.hingeLocalAxisBody2);
-		this.mA1.normalize();
+		this.mA1 = this.mA1.normalize();
-		a2.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);
+		Matrix3f massMatrix = new Matrix3f(inverseMassBodies, 0, 0, 0, inverseMassBodies, 0, 0, 0, inverseMassBodies);
-		massMatrix.add(skewSymmetricMatrixU1.multiplyNew(this.i1).multiply(skewSymmetricMatrixU1.transposeNew()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU1.multiply(this.i1).multiply(skewSymmetricMatrixU1.transpose()));
-		massMatrix.add(skewSymmetricMatrixU2.multiplyNew(this.i2).multiply(skewSymmetricMatrixU2.transposeNew()));
+		massMatrix = massMatrix.add(skewSymmetricMatrixU2.multiply(this.i2).multiply(skewSymmetricMatrixU2.transpose()));
-		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 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);
+		final Vector3f v1 = linearImpulseBody1.multiply(inverseMassBody1);
-		Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1);
+		Vector3f w1 = this.i1.multiply(angularImpulseBody1);
 		
 		// Update the body position/orientation of body 1
-		x1.add(v1);
+		x1 = x1.add(v1);
-		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
-		q1.normalize();
+		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);
+		final Vector3f v2 = lambdaTranslation.multiply(inverseMassBody2);
-		Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2);
+		Vector3f w2 = this.i2.multiply(angularImpulseBody2);
 		
 		// Update the body position/orientation of body 2
-		x2.add(v2);
+		x2 = x2.add(v2);
-		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
+		q2 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
-		q2.normalize();
+		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 i1B2CrossA1 = this.i1.multiply(this.b2CrossA1);
-		final Vector3f I1C2CrossA1 = this.i1.multiplyNew(this.c2CrossA1);
+		final Vector3f i1C2CrossA1 = this.i1.multiply(this.c2CrossA1);
-		final Vector3f I2B2CrossA1 = this.i2.multiplyNew(this.b2CrossA1);
+		final Vector3f i2B2CrossA1 = this.i2.multiply(this.b2CrossA1);
-		final Vector3f I2C2CrossA1 = this.i2.multiplyNew(this.c2CrossA1);
+		final Vector3f i2C2CrossA1 = this.i2.multiply(this.c2CrossA1);
-		final float el11 = this.b2CrossA1.dot(I1B2CrossA1) + this.b2CrossA1.dot(I2B2CrossA1);
+		final float el11 = this.b2CrossA1.dot(i1B2CrossA1) + this.b2CrossA1.dot(i2B2CrossA1);
-		final float el12 = this.b2CrossA1.dot(I1C2CrossA1) + this.b2CrossA1.dot(I2C2CrossA1);
+		final float el12 = this.b2CrossA1.dot(i1C2CrossA1) + this.b2CrossA1.dot(i2C2CrossA1);
-		final float el21 = this.c2CrossA1.dot(I1B2CrossA1) + this.c2CrossA1.dot(I2B2CrossA1);
+		final float el21 = this.c2CrossA1.dot(i1B2CrossA1) + this.c2CrossA1.dot(i2B2CrossA1);
-		final float el22 = this.c2CrossA1.dot(I1C2CrossA1) + this.c2CrossA1.dot(I2C2CrossA1);
+		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 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
-		q1.normalize();
+		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 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
-		q2.normalize();
+		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 = q1.add((new Quaternion(0, w1Tmp)).multiply(q1).multiply(0.5f));
-				q1.normalize();
+				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 = q2.add((new Quaternion(0, w2Tmp)).multiply(q2).multiply(0.5f));
-				q2.normalize();
+				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 = q1.add((new Quaternion(0, w1Tmp)).multiply(q1).multiply(0.5f));
-				q1.normalize();
+				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 = q2.add((new Quaternion(0, w2Tmp)).multiply(q2).multiply(0.5f));
-				q2.normalize();
+				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];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		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));
+		final Vector3f deltaLambdaTranslation = this.inverseMassMatrixTranslation.multiply(jvTranslation.multiply(-1).less(this.bTranslation));
-		this.impulseTranslation.add(deltaLambdaTranslation);
+		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));
+		v1 = v1.add(linearImpulseBody1.multiply(inverseMassBody1));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		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));
+		v2 = v2.add(deltaLambdaTranslation.multiply(inverseMassBody2));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		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));
+		final Vector2f deltaLambdaRotation = this.inverseMassMatrixRotation.multiplyNew(jvRotation.multiply(-1).less(this.bRotation));
-		this.impulseRotation.add(deltaLambdaRotation);
+		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];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		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 linearImpulseBody1 = this.impulseTranslation.multiply(-1);
-		final Vector3f angularImpulseBody1 = this.impulseTranslation.cross(this.r1World);
+		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));
+		v1 = v1.add(linearImpulseBody1.multiply(inverseMassBody1));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		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));
+		v2 = v2.add(this.impulseTranslation.multiply(inverseMassBody2));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		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 rigidBody1 The first body of the joint
-	 * @param _rigidBody2 The second body of the joint
+	 * @param rigidBody2 The second body of the joint
-	 * @param _initAnchorPointWorldSpace The initial anchor point in world-space coordinates
+	 * @param initAnchorPointWorldSpace The initial anchor point in world-space coordinates
-	 * @param _initRotationAxisWorld The initial rotation axis 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) {
+	public HingeJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace, final Vector3f initRotationAxisWorld) {
-		super(_rigidBody1, _rigidBody2, JointType.HINGEJOINT);
+		super(rigidBody1, rigidBody2, JointType.HINGEJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
-		this.rotationAxisWorld = _initRotationAxisWorld;
+		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 rigidBody1 The first body of the joint
-	 * @param _rigidBody2 The second body of the joint
+	 * @param rigidBody2 The second body of the joint
-	 * @param _initAnchorPointWorldSpace The initial anchor point in world-space coordinates
+	 * @param initAnchorPointWorldSpace The initial anchor point in world-space coordinates
-	 * @param _initRotationAxisWorld The intial rotation axis in world-space coordinates
+	 * @param initRotationAxisWorld The intial rotation axis in world-space coordinates
-	 * @param _initMinAngleLimit The initial minimum limit angle (in radian)
+	 * @param initMinAngleLimit The initial minimum limit angle (in radian)
-	 * @param _initMaxAngleLimit The initial maximum 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,
+	public HingeJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace, final Vector3f initRotationAxisWorld, final float initMinAngleLimit,
-			final float _initMaxAngleLimit) {
+			final float initMaxAngleLimit) {
-		super(_rigidBody1, _rigidBody2, JointType.HINGEJOINT);
+		super(rigidBody1, rigidBody2, JointType.HINGEJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
-		this.rotationAxisWorld = _initRotationAxisWorld;
+		this.rotationAxisWorld = initRotationAxisWorld;
 		this.isLimitEnabled = true;
 		this.isMotorEnabled = false;
-		this.minAngleLimit = _initMinAngleLimit;
+		this.minAngleLimit = initMinAngleLimit;
-		this.maxAngleLimit = _initMaxAngleLimit;
+		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 rigidBody1 The first body of the joint
-	 * @param _rigidBody2 The second body of the joint
+	 * @param rigidBody2 The second body of the joint
-	 * @param _initAnchorPointWorldSpace The initial anchor point in world-space
+	 * @param initAnchorPointWorldSpace The initial anchor point in world-space
-	 * @param _initRotationAxisWorld The initial rotation axis in world-space
+	 * @param initRotationAxisWorld The initial rotation axis in world-space
-	 * @param _initMinAngleLimit The initial minimum limit angle (in radian)
+	 * @param initMinAngleLimit The initial minimum limit angle (in radian)
-	 * @param _initMaxAngleLimit The initial maximum 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 initMotorSpeed The initial motor speed of the joint (in radian per second)
-	 * @param _initMaxMotorTorque The initial maximum motor torque (in Newtons)
+	 * @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,
+	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) {
+			final float initMaxAngleLimit, final float initMotorSpeed, final float initMaxMotorTorque) {
-		super(_rigidBody1, _rigidBody2, JointType.HINGEJOINT);
+		super(rigidBody1, rigidBody2, JointType.HINGEJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
-		this.rotationAxisWorld = _initRotationAxisWorld;
+		this.rotationAxisWorld = initRotationAxisWorld;
 		this.isLimitEnabled = true;
 		this.isMotorEnabled = false;
-		this.minAngleLimit = _initMinAngleLimit;
+		this.minAngleLimit = initMinAngleLimit;
-		this.maxAngleLimit = _initMaxAngleLimit;
+		this.maxAngleLimit = initMaxAngleLimit;
-		this.motorSpeed = _initMotorSpeed;
+		this.motorSpeed = initMotorSpeed;
-		this.maxMotorTorque = _initMaxMotorTorque;
+		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) {
+	JointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final JointType raintType) {
-		this.body1 = _rigidBody1;
+		this.body1 = rigidBody1;
-		this.body2 = _rigidBody2;
+		this.body2 = rigidBody2;
-		this.type = _raintType;
+		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) {
+	public JointListElement(final Joint initJoint, final JointListElement initNext) {
-		this.joint = _initJoint;
+		this.joint = initJoint;
-		this.next = _initNext;
+		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 float bLowerLimit; //!< Bias of the lower limit raint
-	private final Vector3f localAnchorPointBody2; //!< Anchor point of body 2 (in local-space coordinates of body 2)
+	private Vector3f bRotation; //!< Bias of the 3 rotation raints
-	private final Vector3f sliderAxisBody1; //!< Slider axis (in local-space coordinates of body 1)
+	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 float impulseLowerLimit; //!< Accumulated impulse for the lower limit raint
-	private Vector3f N1; //!< First vector orthogonal to the slider axis local-space of body 1
+	private float impulseMotor; //!< Accumulated impulse for the motor
-	private Vector3f N2; //!< Second vector orthogonal to the slider axis and N1 in local-space of body 1
+	private Vector3f impulseRotation; //!< Accumulated impulse for the 3 rotation raints
-	private Vector3f R1; //!< Vector r1 in world-space coordinates
+	private Vector2f impulseTranslation; //!< Accumulated impulse for the 2 translation raints
-	private Vector3f R2; //!< Vector r2 in world-space coordinates
+	private float impulseUpperLimit; //!< Accumulated impulse for the upper limit raint
-	private Vector3f R2CrossN1; //!< Cross product of r2 and n1
+	private Quaternion initOrientationDifferenceInv; //!< Inverse of the initial orientation difference between the two bodies
-	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 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 Matrix3f inverseMassMatrixRotationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the rotation raint (3x3 matrix)
-	private final Vector3f impulseRotation; //!< Accumulated impulse for the 3 rotation raints
+	private Matrix2f inverseMassMatrixTranslationConstraint; //!< Inverse of mass matrix K=JM^-1J^t for the translation raint (2x2 matrix)
-	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 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 = transform2.getOrientation().multiply(transform1.getOrientation().inverse());
-		this.initOrientationDifferenceInv.normalize();
+		this.initOrientationDifferenceInv = this.initOrientationDifferenceInv.normalize();
-		this.initOrientationDifferenceInv.inverse();
+		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 = this.body1.getTransform().getOrientation().inverse().multiply(jointInfo.sliderAxisWorldSpace);
-		this.sliderAxisBody1.normalize();
+		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 anchorBody1 = x1.add(q1.multiply(this.localAnchorPointBody1));
-		final Vector3f anchorBody2 = x2.addNew(q2.multiply(this.localAnchorPointBody2));
+		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);
+		Vector3f sliderAxisWorld = q1.multiply(this.sliderAxisBody1);
-		sliderAxisWorld.normalize();
+		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.r1 = orientationBody1.multiply(this.localAnchorPointBody1);
-			this.R2 = orientationBody2.multiply(this.localAnchorPointBody2);
+			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.sliderAxisWorld = this.sliderAxisWorld.normalize();
-			this.N1 = this.sliderAxisWorld.getOrthoVector();
+			this.n1 = this.sliderAxisWorld.getOrthoVector();
-			this.N2 = this.sliderAxisWorld.cross(this.N1);
+			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.r2CrossN1 = this.r2.cross(this.n1);
-			this.R2CrossN2 = this.R2.cross(this.N2);
+			this.r2CrossN2 = this.r2.cross(this.n2);
-			this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld);
+			this.r2CrossSliderAxis = this.r2.cross(this.sliderAxisWorld);
-			final Vector3f r1PlusU = this.R1.addNew(u);
+			final Vector3f r1PlusU = this.r1.add(u);
-			this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1);
+			this.r1PlusUCrossN1 = (r1PlusU).cross(this.n1);
-			this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2);
+			this.r1PlusUCrossN2 = (r1PlusU).cross(this.n2);
-			this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld);
+			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 i1r1PlusUCrossN1 = this.i1.multiply(this.r1PlusUCrossN1);
-			final Vector3f I1R1PlusUCrossN2 = this.i1.multiplyNew(this.R1PlusUCrossN2);
+			final Vector3f i1r1PlusUCrossN2 = this.i1.multiply(this.r1PlusUCrossN2);
-			final Vector3f I2R2CrossN1 = this.i2.multiplyNew(this.R2CrossN1);
+			final Vector3f i2r2CrossN1 = this.i2.multiply(this.r2CrossN1);
-			final Vector3f I2R2CrossN2 = this.i2.multiplyNew(this.R2CrossN2);
+			final Vector3f i2r2CrossN2 = this.i2.multiply(this.r2CrossN2);
-			final float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + this.R2CrossN1.dot(I2R2CrossN1);
+			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 el12 = this.r1PlusUCrossN1.dot(i1r1PlusUCrossN2) + this.r2CrossN1.dot(i2r2CrossN2);
-			final float el21 = this.R1PlusUCrossN2.dot(I1R1PlusUCrossN1) + this.R2CrossN2.dot(I2R2CrossN1);
+			final float el21 = this.r1PlusUCrossN2.dot(i1r1PlusUCrossN1) + this.r2CrossN2.dot(i2r2CrossN1);
-			final float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + this.R2CrossN2.dot(I2R2CrossN2);
+			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();
+			this.bTranslation = Vector2f.ZERO;
-			final float biasFactor = (BETA / raintSolverData.timeStep);
+			final float biasFactor = (SliderJoint.BETA / raintSolverData.timeStep);
 			if (this.positionCorrectionTechnique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
-				this.bTranslation.setX(u.dot(this.N1));
+				this.bTranslation = new Vector2f(u.dot(this.n1), u.dot(this.n2));
-				this.bTranslation.setY(u.dot(this.N2));
+				this.bTranslation = this.bTranslation.multiply(biasFactor);
-				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());
+				Quaternion currentOrientationDifference = orientationBody2.multiply(orientationBody1.inverse());
-				currentOrientationDifference.normalize();
+				currentOrientationDifference = currentOrientationDifference.normalize();
-				final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv);
+				final Quaternion qError = currentOrientationDifference.multiply(this.initOrientationDifferenceInv);
-				this.bRotation = qError.getVectorV().multiplyNew(biasFactor * 2.0f);
+				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.inverseMassMatrixLimit = this.body1.massInverse + this.body2.massInverse + this.r1PlusUCrossSliderAxis.dot(this.i1.multiply(this.r1PlusUCrossSliderAxis))
-						+ this.R2CrossSliderAxis.dot(this.i2.multiplyNew(this.R2CrossSliderAxis));
+						+ 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.impulseTranslation = Vector2f.ZERO;
-				this.impulseRotation.setZero();
+				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];
+		Vector3f x1 = raintSolverData.positions[this.indexBody1];
-		final Vector3f x2 = raintSolverData.positions[this.indexBody2];
+		Vector3f x2 = raintSolverData.positions[this.indexBody2];
-		final Quaternion q1 = raintSolverData.orientations[this.indexBody1];
+		Quaternion q1 = raintSolverData.orientations[this.indexBody1];
-		final Quaternion q2 = raintSolverData.orientations[this.indexBody2];
+		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.r1 = q1.multiply(this.localAnchorPointBody1);
-		this.R2 = q2.multiply(this.localAnchorPointBody2);
+		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.sliderAxisWorld = this.sliderAxisWorld.normalize();
-		this.N1 = this.sliderAxisWorld.getOrthoVector();
+		this.n1 = this.sliderAxisWorld.getOrthoVector();
-		this.N2 = this.sliderAxisWorld.cross(this.N1);
+		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.r2CrossN1 = this.r2.cross(this.n1);
-		this.R2CrossN2 = this.R2.cross(this.N2);
+		this.r2CrossN2 = this.r2.cross(this.n2);
-		this.R2CrossSliderAxis = this.R2.cross(this.sliderAxisWorld);
+		this.r2CrossSliderAxis = this.r2.cross(this.sliderAxisWorld);
-		final Vector3f r1PlusU = this.R1.addNew(u);
+		final Vector3f r1PlusU = this.r1.add(u);
-		this.R1PlusUCrossN1 = (r1PlusU).cross(this.N1);
+		this.r1PlusUCrossN1 = (r1PlusU).cross(this.n1);
-		this.R1PlusUCrossN2 = (r1PlusU).cross(this.N2);
+		this.r1PlusUCrossN2 = (r1PlusU).cross(this.n2);
-		this.R1PlusUCrossSliderAxis = (r1PlusU).cross(this.sliderAxisWorld);
+		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 i1r1PlusUCrossN1 = this.i1.multiply(this.r1PlusUCrossN1);
-		final Vector3f I1R1PlusUCrossN2 = this.i1.multiplyNew(this.R1PlusUCrossN2);
+		final Vector3f i1r1PlusUCrossN2 = this.i1.multiply(this.r1PlusUCrossN2);
-		final Vector3f I2R2CrossN1 = this.i2.multiplyNew(this.R2CrossN1);
+		final Vector3f i2r2CrossN1 = this.i2.multiply(this.r2CrossN1);
-		final Vector3f I2R2CrossN2 = this.i2.multiplyNew(this.R2CrossN2);
+		final Vector3f i2r2CrossN2 = this.i2.multiply(this.r2CrossN2);
-		final float el11 = sumInverseMass + this.R1PlusUCrossN1.dot(I1R1PlusUCrossN1) + this.R2CrossN1.dot(I2R2CrossN1);
+		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 el12 = this.r1PlusUCrossN1.dot(i1r1PlusUCrossN2) + this.r2CrossN1.dot(i2r2CrossN2);
-		final float el21 = this.R1PlusUCrossN2.dot(I1R1PlusUCrossN1) + this.R2CrossN2.dot(I2R2CrossN1);
+		final float el21 = this.r1PlusUCrossN2.dot(i1r1PlusUCrossN1) + this.r2CrossN2.dot(i2r2CrossN1);
-		final float el22 = sumInverseMass + this.R1PlusUCrossN2.dot(I1R1PlusUCrossN2) + this.R2CrossN2.dot(I2R2CrossN2);
+		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));
+		final Vector3f linearImpulseBody1 = this.n1.multiply(-1).multiply(lambdaTranslation.x()).less(this.n2.multiply(lambdaTranslation.y()));
-		Vector3f angularImpulseBody1 = this.R1PlusUCrossN1.multiplyNew(-1).multiplyNew(lambdaTranslation.x).lessNew(this.R1PlusUCrossN2.multiplyNew(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);
+		final Vector3f v1 = linearImpulseBody1.multiply(inverseMassBody1);
-		Vector3f w1 = this.i1.multiplyNew(angularImpulseBody1);
+		Vector3f w1 = this.i1.multiply(angularImpulseBody1);
 		
 		// Update the body position/orientation of body 1
-		x1.add(v1);
+		x1 = x1.add(v1);
-		q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
+		q1 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
-		q1.normalize();
+		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));
+		final Vector3f linearImpulseBody2 = this.n1.multiply(lambdaTranslation.x()).add(this.n2.multiply(lambdaTranslation.y()));
-		Vector3f angularImpulseBody2 = this.R2CrossN1.multiplyNew(lambdaTranslation.x).add(this.R2CrossN2.multiplyNew(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);
+		final Vector3f v2 = linearImpulseBody2.multiply(inverseMassBody2);
-		Vector3f w2 = this.i2.multiplyNew(angularImpulseBody2);
+		Vector3f w2 = this.i2.multiply(angularImpulseBody2);
 		
 		// Update the body position/orientation of body 2
-		x2.add(v2);
+		x2 = x2.add(v2);
-		q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
+		q2 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
-		q2.normalize();
+		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());
+		Quaternion currentOrientationDifference = q2.multiply(q1.inverse());
-		currentOrientationDifference.normalize();
+		currentOrientationDifference = currentOrientationDifference.normalize();
-		final Quaternion qError = currentOrientationDifference.multiplyNew(this.initOrientationDifferenceInv);
+		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 = q1.add((new Quaternion(0, w1)).multiply(q1).multiply(0.5f));
-		q1.normalize();
+		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 = q2.add((new Quaternion(0, w2)).multiply(q2).multiply(0.5f));
-		q2.normalize();
+		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.inverseMassMatrixLimit = this.body1.massInverse + this.body2.massInverse + this.r1PlusUCrossSliderAxis.dot(this.i1.multiply(this.r1PlusUCrossSliderAxis))
-						+ this.R2CrossSliderAxis.dot(this.i2.multiplyNew(this.R2CrossSliderAxis));
+						+ 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 linearImpulseBody1Tmp = this.sliderAxisWorld.multiply(-lambdaLowerLimit);
-				final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(-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 v1Tmp = linearImpulseBody1Tmp.multiply(inverseMassBody1);
-				final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp);
+				final Vector3f w1Tmp = this.i1.multiply(angularImpulseBody1Tmp);
 				
 				// Update the body position/orientation of body 1
-				x1.add(v1_tmp);
+				x1 = x1.add(v1Tmp);
-				q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f));
+				q1 = q1.add((new Quaternion(0, w1Tmp)).multiply(q1).multiply(0.5f));
-				q1.normalize();
+				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 linearImpulseBody2Tmp = this.sliderAxisWorld.multiply(lambdaLowerLimit);
-				final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(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 v2Tmp = linearImpulseBody2Tmp.multiply(inverseMassBody2);
-				final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp);
+				final Vector3f w2Tmp = this.i2.multiply(angularImpulseBody2Tmp);
 				
 				// Update the body position/orientation of body 2
-				x2.add(v2_tmp);
+				x2 = x2.add(v2Tmp);
-				q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f));
+				q2 = q2.add((new Quaternion(0, w2Tmp)).multiply(q2).multiply(0.5f));
-				q2.normalize();
+				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 linearImpulseBody1Tmp = this.sliderAxisWorld.multiply(lambdaUpperLimit);
-				final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(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 v1Tmp = linearImpulseBody1Tmp.multiply(inverseMassBody1);
-				final Vector3f w1_tmp = this.i1.multiplyNew(angularImpulseBody1_tmp);
+				final Vector3f w1Tmp = this.i1.multiply(angularImpulseBody1Tmp);
 				
 				// Update the body position/orientation of body 1
-				x1.add(v1_tmp);
+				x1 = x1.add(v1Tmp);
-				q1.add((new Quaternion(0, w1_tmp)).multiply(q1).multiply(0.5f));
+				q1 = q1.add((new Quaternion(0, w1Tmp)).multiply(q1).multiply(0.5f));
-				q1.normalize();
+				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 linearImpulseBody2Tmp = this.sliderAxisWorld.multiply(-lambdaUpperLimit);
-				final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(-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 v2Tmp = linearImpulseBody2Tmp.multiply(inverseMassBody2);
-				final Vector3f w2_tmp = this.i2.multiplyNew(angularImpulseBody2_tmp);
+				final Vector3f w2Tmp = this.i2.multiply(angularImpulseBody2Tmp);
 				
 				// Update the body position/orientation of body 2
-				x2.add(v2_tmp);
+				x2 = x2.add(v2Tmp);
-				q2.add((new Quaternion(0, w2_tmp)).multiply(q2).multiply(0.5f));
+				q2 = q2.add((new Quaternion(0, w2Tmp)).multiply(q2).multiply(0.5f));
-				q2.normalize();
+				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];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		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 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 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));
+		final Vector2f deltaLambda = this.inverseMassMatrixTranslationConstraint.multiplyNew(jvTranslation.multiply(-1).less(this.bTranslation));
-		this.impulseTranslation.add(deltaLambda);
+		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));
+		final Vector3f linearImpulseBody1 = this.n1.multiply(-1).multiply(deltaLambda.x()).less(this.n2.multiply(deltaLambda.y()));
-		Vector3f angularImpulseBody1 = this.R1PlusUCrossN1.multiplyNew(-1).multiply(deltaLambda.x).less(this.R1PlusUCrossN2.multiplyNew(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));
+		v1 = v1.add(linearImpulseBody1.multiply(inverseMassBody1));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		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));
+		final Vector3f linearImpulseBody2 = this.n1.multiply(deltaLambda.x()).add(this.n2.multiply(deltaLambda.y()));
-		Vector3f angularImpulseBody2 = this.R2CrossN1.multiplyNew(deltaLambda.x).add(this.R2CrossN2.multiplyNew(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));
+		v2 = v2.add(linearImpulseBody2.multiply(inverseMassBody2));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		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));
+		final Vector3f deltaLambda2 = this.inverseMassMatrixRotationConstraint.multiply(jvRotation.multiply(-1).less(this.bRotation));
-		this.impulseRotation.add(deltaLambda2);
+		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 linearImpulseBody1Tmp = this.sliderAxisWorld.multiply(-deltaLambdaLower);
-				final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(-deltaLambdaLower);
+				final Vector3f angularImpulseBody1Tmp = this.r1PlusUCrossSliderAxis.multiply(-deltaLambdaLower);
 				
 				// Apply the impulse to the body 1
-				v1.add(linearImpulseBody1_tmp.multiplyNew(inverseMassBody1));
+				v1 = v1.add(linearImpulseBody1Tmp.multiply(inverseMassBody1));
-				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 linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(deltaLambdaLower);
+				final Vector3f linearImpulseBody2Tmp = this.sliderAxisWorld.multiply(deltaLambdaLower);
-				final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(deltaLambdaLower);
+				final Vector3f angularImpulseBody2Tmp = this.r2CrossSliderAxis.multiply(deltaLambdaLower);
 				
 				// Apply the impulse to the body 2
-				v2.add(linearImpulseBody2_tmp.multiplyNew(inverseMassBody2));
+				v2 = v2.add(linearImpulseBody2Tmp.multiply(inverseMassBody2));
-				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 = 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 linearImpulseBody1Tmp = this.sliderAxisWorld.multiply(deltaLambdaUpper);
-				final Vector3f angularImpulseBody1_tmp = this.R1PlusUCrossSliderAxis.multiplyNew(deltaLambdaUpper);
+				final Vector3f angularImpulseBody1Tmp = this.r1PlusUCrossSliderAxis.multiply(deltaLambdaUpper);
 				
 				// Apply the impulse to the body 1
-				v1.add(linearImpulseBody1_tmp.multiplyNew(inverseMassBody1));
+				v1 = v1.add(linearImpulseBody1Tmp.multiply(inverseMassBody1));
-				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 linearImpulseBody2_tmp = this.sliderAxisWorld.multiplyNew(-deltaLambdaUpper);
+				final Vector3f linearImpulseBody2Tmp = this.sliderAxisWorld.multiply(-deltaLambdaUpper);
-				final Vector3f angularImpulseBody2_tmp = this.R2CrossSliderAxis.multiplyNew(-deltaLambdaUpper);
+				final Vector3f angularImpulseBody2Tmp = this.r2CrossSliderAxis.multiply(-deltaLambdaUpper);
 				
 				// Apply the impulse to the body 2
-				v2.add(linearImpulseBody2_tmp.multiplyNew(inverseMassBody2));
+				v2 = v2.add(linearImpulseBody2Tmp.multiply(inverseMassBody2));
-				w2.add(this.i2.multiplyNew(angularImpulseBody2_tmp));
+				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];
+		Vector3f v1 = raintSolverData.linearVelocities[this.indexBody1];
-		final Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
+		Vector3f v2 = raintSolverData.linearVelocities[this.indexBody2];
-		final Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
+		Vector3f w1 = raintSolverData.angularVelocities[this.indexBody1];
-		final Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
+		Vector3f w2 = raintSolverData.angularVelocities[this.indexBody2];
 		
 		// Get the inverse mass and inverse inertia tensors of the bodies
-		final float inverseMassBody1 = this.body1.massInverse;
+		float inverseMassBody1 = this.body1.massInverse;
-		final float inverseMassBody2 = this.body2.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));
+		Vector3f linearImpulseBody1 = this.n1.multiply(-1).multiply(this.impulseTranslation.x()).less(this.n2.multiply(this.impulseTranslation.y()));
-		final Vector3f angularImpulseBody1 = this.R1PlusUCrossN1.multiplyNew(-1).multiply(this.impulseTranslation.x).less(this.R1PlusUCrossN2.multiplyNew(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);
+		linearImpulseBody1 = linearImpulseBody1.add(linearImpulseLimits);
-		angularImpulseBody1.add(this.R1PlusUCrossSliderAxis.multiplyNew(impulseLimits));
+		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));
+		v1 = v1.add(linearImpulseBody1.multiply(inverseMassBody1));
-		w1.add(this.i1.multiplyNew(angularImpulseBody1));
+		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));
+		Vector3f linearImpulseBody2 = this.n1.multiply(this.impulseTranslation.x()).add(this.n2.multiply(this.impulseTranslation.y()));
-		final Vector3f angularImpulseBody2 = this.R2CrossN1.multiplyNew(this.impulseTranslation.x).add(this.R2CrossN2.multiplyNew(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);
+		linearImpulseBody2 = linearImpulseBody2.less(linearImpulseLimits);
-		angularImpulseBody2.add(this.R2CrossSliderAxis.multiplyNew(-impulseLimits));
+		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));
+		v2 = v2.add(linearImpulseBody2.multiply(inverseMassBody2));
-		w2.add(this.i2.multiplyNew(angularImpulseBody2));
+		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 rigidBody1 The first body of the joint
-		 * @param _rigidBody2 The second body of the joint
+		 * @param rigidBody2 The second body of the joint
-		 * @param _initAnchorPointWorldSpace The initial anchor point in world-space
+		 * @param initAnchorPointWorldSpace The initial anchor point in world-space
-		 * @param _initSliderAxisWorldSpace The initial slider axis 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) {
+	public SliderJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace, final Vector3f initSliderAxisWorldSpace) {
-		super(_rigidBody1, _rigidBody2, JointType.SLIDERJOINT);
+		super(rigidBody1, rigidBody2, JointType.SLIDERJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
-		this.sliderAxisWorldSpace = _initSliderAxisWorldSpace;
+		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 rigidBody1 The first body of the joint
-		 * @param _rigidBody2 The second body of the joint
+		 * @param rigidBody2 The second body of the joint
-		 * @param _initAnchorPointWorldSpace The initial anchor point in world-space
+		 * @param initAnchorPointWorldSpace The initial anchor point in world-space
-		 * @param _initSliderAxisWorldSpace The initial slider axis in world-space
+		 * @param initSliderAxisWorldSpace The initial slider axis in world-space
-		 * @param _initMinTranslationLimit The initial minimum translation limit (in meters)
+		 * @param initMinTranslationLimit The initial minimum translation limit (in meters)
-		 * @param _initMaxTranslationLimit The initial maximum 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,
+	public SliderJointInfo(final RigidBody rigidBody1, final RigidBody rigidBody2, final Vector3f initAnchorPointWorldSpace, final Vector3f initSliderAxisWorldSpace,
-			final float _initMinTranslationLimit, final float _initMaxTranslationLimit) {
+			final float initMinTranslationLimit, final float initMaxTranslationLimit) {
-		super(_rigidBody1, _rigidBody2, JointType.SLIDERJOINT);
+		super(rigidBody1, rigidBody2, JointType.SLIDERJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
-		this.sliderAxisWorldSpace = _initSliderAxisWorldSpace;
+		this.sliderAxisWorldSpace = initSliderAxisWorldSpace;
 		this.isLimitEnabled = true;
 		this.isMotorEnabled = false;
-		this.minTranslationLimit = _initMinTranslationLimit;
+		this.minTranslationLimit = initMinTranslationLimit;
-		this.maxTranslationLimit = _initMaxTranslationLimit;
+		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 rigidBody1 The first body of the joint
-		 * @param _rigidBody2 The second body of the joint
+		 * @param rigidBody2 The second body of the joint
-		 * @param _initAnchorPointWorldSpace The initial anchor point in world-space
+		 * @param initAnchorPointWorldSpace The initial anchor point in world-space
-		 * @param _initSliderAxisWorldSpace The initial slider axis in world-space
+		 * @param initSliderAxisWorldSpace The initial slider axis in world-space
-		 * @param _initMinTranslationLimit The initial minimum translation limit (in meters)
+		 * @param initMinTranslationLimit The initial minimum translation limit (in meters)
-		 * @param _initMaxTranslationLimit The initial maximum 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 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 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,
+	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) {
+			final float initMinTranslationLimit, final float initMaxTranslationLimit, final float initMotorSpeed, final float initMaxMotorForce) {
-		super(_rigidBody1, _rigidBody2, JointType.SLIDERJOINT);
+		super(rigidBody1, rigidBody2, JointType.SLIDERJOINT);
-		this.anchorPointWorldSpace = _initAnchorPointWorldSpace;
+		this.anchorPointWorldSpace = initAnchorPointWorldSpace;
-		this.sliderAxisWorldSpace = _initSliderAxisWorldSpace;
+		this.sliderAxisWorldSpace = initSliderAxisWorldSpace;
 		this.isLimitEnabled = true;
 		this.isMotorEnabled = true;
-		this.minTranslationLimit = _initMinTranslationLimit;
+		this.minTranslationLimit = initMinTranslationLimit;
-		this.maxTranslationLimit = _initMaxTranslationLimit;
+		this.maxTranslationLimit = initMaxTranslationLimit;
-		this.motorSpeed = _initMotorSpeed;
+		this.motorSpeed = initMotorSpeed;
-		this.maxMotorForce = _initMaxMotorForce;
+		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 ray Ray to use for raycasting
-	 * @param _raycastCallback Pointer to the class with the callback method
+	 * @param raycastCallback Pointer to the class with the callback method
-	 * @param _raycastWithCategoryMaskBits Bits mask corresponding to the category of bodies to be raycasted
+	 * @param raycastWithCategoryMaskBits Bits mask corresponding to the category of bodies to be raycasted
 	 */
-	public void raycast(final Ray _ray, final RaycastCallback _raycastCallback) {
+	public void raycast(final Ray ray, final RaycastCallback raycastCallback) {
-		raycast(_ray, _raycastCallback, 0xFFFF);
+		raycast(ray, raycastCallback, 0xFFFF);
 	}
 	
-	public void raycast(final Ray _ray, final RaycastCallback _raycastCallback, final int _raycastWithCategoryMaskBits) {
+	public void raycast(final Ray ray, final RaycastCallback raycastCallback, final int raycastWithCategoryMaskBits) {
-		this.collisionDetection.raycast(_raycastCallback, _ray, _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) {
+	public void setCollisionDispatch(final CollisionDispatch collisionDispatch) {
-		this.collisionDetection.setCollisionDispatch(_collisionDispatch);
+		this.collisionDetection.setCollisionDispatch(collisionDispatch);
 	}
 	
 	/**
 	 *  Test if the AABBs of two bodies overlap
-	 * @param _body1 Pointer to the first body to test
+	 * @param body1 Pointer to the first body to test
-	 * @param _body2 Pointer to the second 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 body1AABB = body1.getAABB();
-		final AABB body2AABB = _body2.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 shape1 Pointer to the first proxy shape to test
-	 * @param _shape2 Pointer to the second proxy shape to test
+	 * @param shape2 Pointer to the second proxy shape to test
 	 */
-	public boolean testAABBOverlap(final ProxyShape _shape1, final ProxyShape _shape2) {
+	public boolean testAABBOverlap(final ProxyShape shape1, final ProxyShape shape2) {
-		return this.collisionDetection.testAABBOverlap(_shape1, _shape2);
+		return this.collisionDetection.testAABBOverlap(shape1, shape2);
 	}
 	
 	/**
 	 *  Test and report collisions between two bodies
-	 * @param _body1 Pointer to the first body to test
+	 * @param body1 Pointer to the first body to test
-	 * @param _body2 Pointer to the second body to test
+	 * @param body2 Pointer to the second body to test
-	 * @param _callback Pointer to the object with the callback method
+	 * @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 body Pointer to the first body to test
-	 * @param _callback Pointer to the object with the callback method
+	 * @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 shape Pointer to the proxy shape to test
-	 * @param _callback Pointer to the object with the callback method
+	 * @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 shape1 Pointer to the first proxy shape to test
-	 * @param _shape2 Pointer to the second 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 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 inverseInertiaTensorBody1 = Matrix3f.IDENTITY; //!< Inverse inertia tensor of body 1
-	Matrix3f inverseInertiaTensorBody2 = new Matrix3f(); //!< Inverse inertia tensor of body 2
+	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 normal = Vector3f.ZERO; //!< 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 frictionPointBody1 = Vector3f.ZERO; //!< 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 frictionPointBody2 = Vector3f.ZERO; //!< Point on body 2 where to apply the friction raints
-	Vector3f r1Friction = new Vector3f(0, 0, 0); //!< R1 vector for the friction raints
+	Vector3f r1Friction = Vector3f.ZERO; //!< R1 vector for the friction raints
-	Vector3f r2Friction = new Vector3f(0, 0, 0); //!< R2 vector for the friction raints
+	Vector3f r2Friction = Vector3f.ZERO; //!< R2 vector for the friction raints
-	Vector3f r1CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 1st friction vector
+	Vector3f r1CrossT1 = Vector3f.ZERO; //!< Cross product of r1 with 1st friction vector
-	Vector3f r1CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 2nd friction vector
+	Vector3f r1CrossT2 = Vector3f.ZERO; //!< Cross product of r1 with 2nd friction vector
-	Vector3f r2CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 1st friction vector
+	Vector3f r2CrossT1 = Vector3f.ZERO; //!< Cross product of r2 with 1st friction vector
-	Vector3f r2CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 2nd 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
+	Matrix3f inverseRollingResistance = Matrix3f.IDENTITY; //!< Matrix K for the rolling resistance raint
-	Vector3f frictionVector1 = new Vector3f(0, 0, 0); //!< First friction direction at contact manifold center
+	Vector3f frictionVector1 = Vector3f.ZERO; //!< First friction direction at contact manifold center
-	Vector3f frictionvec2 = new Vector3f(0, 0, 0); //!< Second friction direction at contact manifold center
+	Vector3f frictionvec2 = Vector3f.ZERO; //!< Second friction direction at contact manifold center
-	Vector3f oldFrictionVector1 = new Vector3f(0, 0, 0); //!< Old 1st friction direction at contact manifold center
+	Vector3f oldFrictionVector1 = Vector3f.ZERO; //!< Old 1st friction direction at contact manifold center
-	Vector3f oldFrictionvec2 = new Vector3f(0, 0, 0); //!< Old 2nd 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 rollingResistanceImpulse = Vector3f.ZERO; //!< Accumulated rolling resistance impulse
-	Vector3f normal = new Vector3f(0, 0, 0); //!< Normal vector of the contact
+	Vector3f normal = Vector3f.ZERO; //!< Normal vector of the contact
-	Vector3f frictionVector1 = new Vector3f(0, 0, 0); //!< First friction vector in the tangent plane
+	Vector3f frictionVector1 = Vector3f.ZERO; //!< First friction vector in the tangent plane
-	Vector3f frictionvec2 = new Vector3f(0, 0, 0); //!< Second friction vector in the tangent plane
+	Vector3f frictionvec2 = Vector3f.ZERO; //!< Second friction vector in the tangent plane
-	Vector3f oldFrictionVector1 = new Vector3f(0, 0, 0); //!< Old first friction vector in the tangent plane
+	Vector3f oldFrictionVector1 = Vector3f.ZERO; //!< Old first friction vector in the tangent plane
-	Vector3f oldFrictionvec2 = new Vector3f(0, 0, 0); //!< Old second friction vector in the tangent plane
+	Vector3f oldFrictionvec2 = Vector3f.ZERO; //!< 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 r1 = Vector3f.ZERO; //!< 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 r2 = Vector3f.ZERO; //!< 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 r1CrossT1 = Vector3f.ZERO; //!< Cross product of r1 with 1st friction vector
-	Vector3f r1CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r1 with 2nd friction vector
+	Vector3f r1CrossT2 = Vector3f.ZERO; //!< Cross product of r1 with 2nd friction vector
-	Vector3f r2CrossT1 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 1st friction vector
+	Vector3f r2CrossT1 = Vector3f.ZERO; //!< Cross product of r2 with 1st friction vector
-	Vector3f r2CrossT2 = new Vector3f(0, 0, 0); //!< Cross product of r2 with 2nd friction vector
+	Vector3f r2CrossT2 = Vector3f.ZERO; //!< Cross product of r2 with 2nd friction vector
-	Vector3f r1CrossN = new Vector3f(0, 0, 0); //!< Cross product of r1 with the contact normal
+	Vector3f r1CrossN = Vector3f.ZERO; //!< Cross product of r1 with the contact normal
-	Vector3f r2CrossN = new Vector3f(0, 0, 0); //!< Cross product of r2 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/ContactSolver.java

@@ -23,10 +23,10 @@ import org.atriasoft.etk.math.Vector3f;
  * when the raint is satisfied. The condition C(x)=0 describes a valid position and the
  * condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is
  * the Jacobian matrix of the raint, v is a vector that contains the velocity of both
- * bodies and b is the raint bias. We are looking for a force F_c that will act on the
+ * bodies and b is the raint bias. We are looking for a force Fc that will act on the
  * bodies to keep the raint satisfied. Note that from the  work principle, we have
- * F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
+ * Fc = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a
- * Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange
+ * Lagrange multiplier. Therefore, finding the force Fc is equivalent to finding the Lagrange
  * multiplier lambda.
  *
  * An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a
@@ -35,10 +35,10 @@ import org.atriasoft.etk.math.Vector3f;
  *
  * --- Step 1 ---
  *
- * First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and
+ * First, we integrate the applied force Fa acting of each rigid body (like gravity, ...) and
  * we obtain some new velocities v2' that tends to violate the raints.
  *
- * v2' = v1 + dt * M^-1 * F_a
+ * v2' = v1 + dt * M^-1 * Fa
  *
  * where M is a matrix that contains mass and inertia tensor information.
  *
@@ -47,11 +47,11 @@ import org.atriasoft.etk.math.Vector3f;
  * During the second step, we iterate over all the raints for a certain number of
  * iterations and for each raint we compute the impulse to apply to the bodies needed
  * so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that
- * M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and
+ * M * deltaV = Pc where M is the mass of the body, deltaV is the difference of velocity and
- * P_c is the raint impulse to apply to the body. Therefore, we have
+ * Pc is the raint impulse to apply to the body. Therefore, we have
- * v2 = v2' + M^-1 * P_c. For each raint, we can compute the Lagrange multiplier lambda
+ * v2 = v2' + M^-1 * Pc. For each raint, we can compute the Lagrange multiplier lambda
  * using : lambda = -this.c (Jv2' + b) where this.c = 1 / (J * M^-1 * J^t). Now that we have the
- * Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to
+ * Lagrange multiplier lambda, we can compute the impulse Pc = J^t * lambda * dt to apply to
  * the bodies to satisfy the raint.
  *
  * --- Step 3 ---
@@ -84,30 +84,30 @@ import org.atriasoft.etk.math.Vector3f;
  */
 public class ContactSolver {
 	
-	private static float BETA = 0.2f; //!< Beta value for the penetration depth position correction without split impulses
+	private static final float BETA = 0.2f; //!< Beta value for the penetration depth position correction without split impulses
-	private static float BETA_SPLIT_IMPULSE = 0.2f; //!< Beta value for the penetration depth position correction with split impulses
+	private static final float BETA_SPLIT_IMPULSE = 0.2f; //!< Beta value for the penetration depth position correction with split impulses
-	private static float SLOP = 0.01f; //!< Slop distance (allowed penetration distance between bodies)
+	private static final float SLOP = 0.01f; //!< Slop distance (allowed penetration distance between bodies)
-	private Vector3f[] splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse)
-	private Vector3f[] splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse)
-	private float timeStep; //!< Current time step
-	private List<ContactManifoldSolver> contactConstraints = new ArrayList<>(); //!< Contact raints
-	private Vector3f[] linearVelocities; //!< Array of linear velocities
 	private Vector3f[] angularVelocities; //!< Array of angular velocities
-	private final Map<RigidBody, Integer> mapBodyToConstrainedVelocityIndex; //!< Reference to the map of rigid body to their index in the rained velocities array
+	private List<ContactManifoldSolver> contactConstraints = new ArrayList<>(); //!< Contact raints
-	private final boolean isWarmStartingActive; //!< True if the warm starting of the solver is active
-	private boolean isSplitImpulseActive; //!< True if the split impulse position correction is active
 	private boolean isSolveFrictionAtContactManifoldCenterActive; //!< True if we solve 3 friction raints at the contact manifold center only instead of 2 friction raints at each contact point
+	private boolean isSplitImpulseActive; //!< True if the split impulse position correction is active
+	private final boolean isWarmStartingActive; //!< True if the warm starting of the solver is active
+	private Vector3f[] linearVelocities; //!< Array of linear velocities
+	private final Map<RigidBody, Integer> mapBodyToConstrainedVelocityIndex; //!< Reference to the map of rigid body to their index in the rained velocities array
+	private Vector3f[] splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse)
+	private Vector3f[] splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse)
+	private float timeStep; //!< Current time step
 	
 	/**
 	 *  Constructor
-	 * @param _mapBodyToVelocityIndex
+	 * @param mapBodyToVelocityIndex
 	 */
-	public ContactSolver(final Map<RigidBody, Integer> _mapBodyToVelocityIndex) {
+	public ContactSolver(final Map<RigidBody, Integer> mapBodyToVelocityIndex) {
 		this.splitLinearVelocities = null;
 		this.splitAngularVelocities = null;
 		this.linearVelocities = null;
 		this.angularVelocities = null;
-		this.mapBodyToConstrainedVelocityIndex = _mapBodyToVelocityIndex;
+		this.mapBodyToConstrainedVelocityIndex = mapBodyToVelocityIndex;
 		this.isWarmStartingActive = false; // TODO default true
 		this.isSplitImpulseActive = true; // TODO default true
 		this.isSolveFrictionAtContactManifoldCenterActive = true; // TODO default true
@@ -115,39 +115,39 @@ public class ContactSolver {
 	
 	/**
 	 *  Apply an impulse to the two bodies of a raint
-	 * @param _impulse Impulse to apply
+	 * @param impulse Impulse to apply
-	 * @param _manifold Constraint to apply the impulse
+	 * @param manifold Constraint to apply the impulse
 	 */
-	private void applyImpulse(final Impulse _impulse, final ContactManifoldSolver _manifold) {
+	private void applyImpulse(final Impulse impulse, final ContactManifoldSolver manifold) {
-		//Log.info("    --        _manifold.massInverseBody1=" + _manifold.massInverseBody1);
+		//Log.info("    --        manifold.massInverseBody1=" + manifold.massInverseBody1);
-		//Log.info("    --        _manifold.inverseInertiaTensorBody1=" + _manifold.inverseInertiaTensorBody1);
+		//Log.info("    --        manifold.inverseInertiaTensorBody1=" + manifold.inverseInertiaTensorBody1);
-		//Log.info("    --        _manifold.massInverseBody2=" + _manifold.massInverseBody2);
+		//Log.info("    --        manifold.massInverseBody2=" + manifold.massInverseBody2);
-		//Log.info("    --        _manifold.inverseInertiaTensorBody2=" + _manifold.inverseInertiaTensorBody2);
+		//Log.info("    --        manifold.inverseInertiaTensorBody2=" + manifold.inverseInertiaTensorBody2);
-		//Log.info("    --        _impulse.angularImpulseBody2=" + _impulse.angularImpulseBody2);
+		//Log.info("    --        impulse.angularImpulseBody2=" + impulse.angularImpulseBody2);
 		// Update the velocities of the body 1 by applying the impulse P
-		this.linearVelocities[_manifold.indexBody1].add(_impulse.linearImpulseBody1.multiplyNew(_manifold.massInverseBody1));
+		this.linearVelocities[manifold.indexBody1] = this.linearVelocities[manifold.indexBody1].add(impulse.linearImpulseBody1().multiply(manifold.massInverseBody1));
-		//Log.info("    --        this.angularVelocities[_manifold.indexBody1]=" + this.angularVelocities[_manifold.indexBody1]);
+		//Log.info("    --        this.angularVelocities[manifold.indexBody1]=" + this.angularVelocities[manifold.indexBody1]);
-		this.angularVelocities[_manifold.indexBody1].add(_manifold.inverseInertiaTensorBody1.multiplyNew(_impulse.angularImpulseBody1));
+		this.angularVelocities[manifold.indexBody1] = this.angularVelocities[manifold.indexBody1].add(manifold.inverseInertiaTensorBody1.multiply(impulse.angularImpulseBody1()));
-		//Log.info("    --        this.angularVelocities[_manifold.indexBody1]=" + this.angularVelocities[_manifold.indexBody1]);
+		//Log.info("    --        this.angularVelocities[manifold.indexBody1]=" + this.angularVelocities[manifold.indexBody1]);
 		// Update the velocities of the body 1 by applying the impulse P
-		this.linearVelocities[_manifold.indexBody2].add(_impulse.linearImpulseBody2.multiplyNew(_manifold.massInverseBody2));
+		this.linearVelocities[manifold.indexBody2] = this.linearVelocities[manifold.indexBody2].add(impulse.linearImpulseBody2().multiply(manifold.massInverseBody2));
-		//Log.info("    --        this.angularVelocities[_manifold.indexBody2]=" + this.angularVelocities[_manifold.indexBody2]);
+		//Log.info("    --        this.angularVelocities[manifold.indexBody2]=" + this.angularVelocities[manifold.indexBody2]);
-		this.angularVelocities[_manifold.indexBody2].add(_manifold.inverseInertiaTensorBody2.multiplyNew(_impulse.angularImpulseBody2));
+		this.angularVelocities[manifold.indexBody2] = this.angularVelocities[manifold.indexBody2].add(manifold.inverseInertiaTensorBody2.multiply(impulse.angularImpulseBody2()));
-		//Log.info("    --        this.angularVelocities[_manifold.indexBody2]=" + this.angularVelocities[_manifold.indexBody2]);
+		//Log.info("    --        this.angularVelocities[manifold.indexBody2]=" + this.angularVelocities[manifold.indexBody2]);
 	}
 	
 	/**
 	 *  Apply an impulse to the two bodies of a raint
-	 * @param _impulse Impulse to apply
+	 * @param impulse Impulse to apply
-	 * @param _manifold Constraint to apply the impulse
+	 * @param manifold Constraint to apply the impulse
 	 */
-	private void applySplitImpulse(final Impulse _impulse, final ContactManifoldSolver _manifold) {
+	private void applySplitImpulse(final Impulse impulse, final ContactManifoldSolver manifold) {
 		// Update the velocities of the body 1 by applying the impulse P
-		this.splitLinearVelocities[_manifold.indexBody1].add(_impulse.linearImpulseBody1.multiplyNew(_manifold.massInverseBody1));
+		this.splitLinearVelocities[manifold.indexBody1] = this.splitLinearVelocities[manifold.indexBody1].add(impulse.linearImpulseBody1().multiply(manifold.massInverseBody1));
-		this.splitAngularVelocities[_manifold.indexBody1].add(_manifold.inverseInertiaTensorBody1.multiplyNew(_impulse.angularImpulseBody1));
+		this.splitAngularVelocities[manifold.indexBody1] = this.splitAngularVelocities[manifold.indexBody1].add(manifold.inverseInertiaTensorBody1.multiply(impulse.angularImpulseBody1()));
 		// Update the velocities of the body 1 by applying the impulse P
-		this.splitLinearVelocities[_manifold.indexBody2].add(_impulse.linearImpulseBody2.multiplyNew(_manifold.massInverseBody2));
+		this.splitLinearVelocities[manifold.indexBody2] = this.splitLinearVelocities[manifold.indexBody2].add(impulse.linearImpulseBody2().multiply(manifold.massInverseBody2));
-		this.splitAngularVelocities[_manifold.indexBody2].add(_manifold.inverseInertiaTensorBody2.multiplyNew(_impulse.angularImpulseBody2));
+		this.splitAngularVelocities[manifold.indexBody2] = this.splitAngularVelocities[manifold.indexBody2].add(manifold.inverseInertiaTensorBody2.multiply(impulse.angularImpulseBody2()));
 	}
 	
 	/**
@@ -159,73 +159,73 @@ public class ContactSolver {
 	
 	/**
 	 *  Compute the first friction raint impulse
-	 * @param _deltaLambda Ratio to apply at the calculation.
+	 * @param deltaLambda Ratio to apply at the calculation.
-	 * @param _contactPoint Contact point property
+	 * @param contactPoint Contact point property
 	 * @return Impulse of the friction result
 	 */
-	private Impulse computeFriction1Impulse(final float _deltaLambda, final ContactPointSolver _contactPoint) {
+	private Impulse computeFriction1Impulse(final float deltaLambda, final ContactPointSolver contactPoint) {
-		//Log.error("=== r1CrossT1=" + _contactPoint.r1CrossT1);
+		//Log.error("=== r1CrossT1=" + contactPoint.r1CrossT1);
-		//Log.error("=== r2CrossT1=" + _contactPoint.r2CrossT1);
+		//Log.error("=== r2CrossT1=" + contactPoint.r2CrossT1);
-		return new Impulse(_contactPoint.frictionVector1.multiplyNew(-1).multiply(_deltaLambda), _contactPoint.r1CrossT1.multiplyNew(-1).multiply(_deltaLambda),
+		return new Impulse(contactPoint.frictionVector1.multiply(-1).multiply(deltaLambda), contactPoint.r1CrossT1.multiply(-1).multiply(deltaLambda),
-				_contactPoint.frictionVector1.multiplyNew(_deltaLambda), _contactPoint.r2CrossT1.multiplyNew(_deltaLambda));
+				contactPoint.frictionVector1.multiply(deltaLambda), contactPoint.r2CrossT1.multiply(deltaLambda));
 	}
 	
 	/**
 	 *  Compute the second friction raint impulse
-	 * @param _deltaLambda Ratio to apply at the calculation.
+	 * @param deltaLambda Ratio to apply at the calculation.
-	 * @param _contactPoint Contact point property
+	 * @param contactPoint Contact point property
 	 * @return Impulse of the friction result
 	 */
-	private Impulse computeFriction2Impulse(final float _deltaLambda, final ContactPointSolver _contactPoint) {
+	private Impulse computeFriction2Impulse(final float deltaLambda, final ContactPointSolver contactPoint) {
-		//Log.error("=== r1CrossT2=" + _contactPoint.r1CrossT2);
+		//Log.error("=== r1CrossT2=" + contactPoint.r1CrossT2);
-		//Log.error("=== r2CrossT2=" + _contactPoint.r2CrossT2);
+		//Log.error("=== r2CrossT2=" + contactPoint.r2CrossT2);
-		return new Impulse(_contactPoint.frictionvec2.multiplyNew(-1).multiply(_deltaLambda), _contactPoint.r1CrossT2.multiplyNew(-1).multiplyNew(_deltaLambda),
+		return new Impulse(contactPoint.frictionvec2.multiply(-1).multiply(deltaLambda), contactPoint.r1CrossT2.multiply(-1).multiply(deltaLambda), contactPoint.frictionvec2.multiply(deltaLambda),
-				_contactPoint.frictionvec2.multiplyNew(_deltaLambda), _contactPoint.r2CrossT2.multiplyNew(_deltaLambda));
+				contactPoint.r2CrossT2.multiply(deltaLambda));
 	}
 	
 	/**
 	 *  Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
 	 *        plane for a contact manifold. The two vectors have to be such that : t1 x t2 = contactNormal.
-	 * @param _deltaVelocity Velocity ratio (with the delta time step)
+	 * @param deltaVelocity Velocity ratio (with the delta time step)
-	 * @param[in,out] _contactPoint Contact point property
+	 * @param contactPoint Contact point property
 	 */
-	private void computeFrictionVectors(final Vector3f _deltaVelocity, final ContactManifoldSolver _contactManifold) {
+	private void computeFrictionVectors(final Vector3f deltaVelocity, final ContactManifoldSolver contactManifold) {
-		assert (_contactManifold.normal.length() > 0.0f);
+		assert (contactManifold.normal.length() > 0.0f);
 		// Compute the velocity difference vector in the tangential plane
-		final Vector3f normalVelocity = _contactManifold.normal.multiplyNew(_deltaVelocity.dot(_contactManifold.normal));
+		final Vector3f normalVelocity = contactManifold.normal.multiply(deltaVelocity.dot(contactManifold.normal));
-		final Vector3f tangentVelocity = _deltaVelocity.lessNew(normalVelocity);
+		final Vector3f tangentVelocity = deltaVelocity.less(normalVelocity);
 		// If the velocty difference in the tangential plane is not zero
 		final float lengthTangenVelocity = tangentVelocity.length();
 		if (lengthTangenVelocity > Constant.FLOAT_EPSILON) {
 			// Compute the first friction vector in the direction of the tangent
 			// velocity difference
-			_contactManifold.frictionVector1 = tangentVelocity.divideNew(lengthTangenVelocity);
+			contactManifold.frictionVector1 = tangentVelocity.divide(lengthTangenVelocity);
-			//Log.error("===1==>>>>>> _contactManifold.frictionVector1=" + _contactManifold.frictionVector1);
+			//Log.error("===1==>>>>>> contactManifold.frictionVector1=" + contactManifold.frictionVector1);
 		} else {
 			// Get any orthogonal vector to the normal as the first friction vector
-			_contactManifold.frictionVector1 = _contactManifold.normal.getOrthoVector();
+			contactManifold.frictionVector1 = contactManifold.normal.getOrthoVector();
-			//Log.error("===2==>>>>>> _contactManifold.frictionVector1=" + _contactManifold.frictionVector1);
+			//Log.error("===2==>>>>>> contactManifold.frictionVector1=" + contactManifold.frictionVector1);
 		}
 		
 		// The second friction vector is computed by the cross product of the firs
 		// friction vector and the contact normal
-		_contactManifold.frictionvec2 = _contactManifold.normal.cross(_contactManifold.frictionVector1).safeNormalizeNew();
+		contactManifold.frictionvec2 = contactManifold.normal.cross(contactManifold.frictionVector1).safeNormalize();
 	}
 	
 	/**
 	 *  Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction
 	 *        plane for a contact point. The two vectors have to be such that : t1 x t2 = contactNormal.
-	 * @param _deltaVelocity Velocity ratio (with the delta time step)
+	 * @param deltaVelocity Velocity ratio (with the delta time step)
-	 * @param[in,out] _contactPoint Contact point property
+	 * @param contactPoint Contact point property
 	 */
-	private void computeFrictionVectors(final Vector3f _deltaVelocity, final ContactPointSolver _contactPoint) {
+	private void computeFrictionVectors(final Vector3f deltaVelocity, final ContactPointSolver contactPoint) {
-		assert (_contactPoint.normal.length() > 0.0f);
+		assert (contactPoint.normal.length() > 0.0f);
-		//Log.error("===3==>>>>>> _contactPoint.normal=" + _contactPoint.normal);
+		//Log.error("===3==>>>>>> contactPoint.normal=" + contactPoint.normal);
-		//Log.error("===3==>>>>>> _deltaVelocity=" + _deltaVelocity);
+		//Log.error("===3==>>>>>> deltaVelocity=" + deltaVelocity);
 		// Compute the velocity difference vector in the tangential plane
-		final Vector3f normalVelocity = _contactPoint.normal.multiplyNew(_deltaVelocity.dot(_contactPoint.normal));
+		final Vector3f normalVelocity = contactPoint.normal.multiply(deltaVelocity.dot(contactPoint.normal));
 		//Log.error("===3==>>>>>> normalVelocity=" + normalVelocity);
-		final Vector3f tangentVelocity = _deltaVelocity.lessNew(normalVelocity);
+		final Vector3f tangentVelocity = deltaVelocity.less(normalVelocity);
 		//Log.error("===3==>>>>>> tangentVelocity=" + tangentVelocity);
 		// If the velocty difference in the tangential plane is not zero
 		final float lengthTangenVelocity = tangentVelocity.length();
@@ -233,38 +233,38 @@ public class ContactSolver {
 		if (lengthTangenVelocity > Constant.FLOAT_EPSILON) {
 			// Compute the first friction vector in the direction of the tangent
 			// velocity difference
-			_contactPoint.frictionVector1 = tangentVelocity.divideNew(lengthTangenVelocity);
+			contactPoint.frictionVector1 = tangentVelocity.divide(lengthTangenVelocity);
-			//Log.error("===3==>>>>>> _contactPoint.frictionVector1=" + _contactPoint.frictionVector1);
+			//Log.error("===3==>>>>>> contactPoint.frictionVector1=" + contactPoint.frictionVector1);
 		} else {
 			// Get any orthogonal vector to the normal as the first friction vector
-			_contactPoint.frictionVector1 = _contactPoint.normal.getOrthoVector();
+			contactPoint.frictionVector1 = contactPoint.normal.getOrthoVector();
-			//Log.error("===4==>>>>>> _contactPoint.frictionVector1=" + _contactPoint.frictionVector1);
+			//Log.error("===4==>>>>>> contactPoint.frictionVector1=" + contactPoint.frictionVector1);
 		}
 		// The second friction vector is computed by the cross product of the firs
 		// friction vector and the contact normal
-		_contactPoint.frictionvec2 = _contactPoint.normal.cross(_contactPoint.frictionVector1).safeNormalizeNew();
+		contactPoint.frictionvec2 = contactPoint.normal.cross(contactPoint.frictionVector1).safeNormalize();
 	}
 	
 	/**
 	 *  Compute the mixed friction coefficient from the friction coefficient of each body
-	 * @param _body1 First body to compute
+	 * @param body1 First body to compute
-	 * @param _body2 Second body to compute
+	 * @param body2 Second body to compute
 	 * @return Mixed friction coefficient
 	 */
-	private float computeMixedFrictionCoefficient(final RigidBody _body1, final RigidBody _body2) {
+	private float computeMixedFrictionCoefficient(final RigidBody body1, final RigidBody body2) {
 		// Use the geometric mean to compute the mixed friction coefficient
-		return FMath.sqrt(_body1.getMaterial().getFrictionCoefficient() * _body2.getMaterial().getFrictionCoefficient());
+		return FMath.sqrt(body1.getMaterial().getFrictionCoefficient() * body2.getMaterial().getFrictionCoefficient());
 	}
 	
 	/**
 	 *  Compute the collision restitution factor from the restitution factor of each body
-	 * @param _body1 First body to compute
+	 * @param body1 First body to compute
-	 * @param _body2 Second body to compute
+	 * @param body2 Second body to compute
 	 * @return Collision restitution factor
 	 */
-	private float computeMixedRestitutionFactor(final RigidBody _body1, final RigidBody _body2) {
+	private float computeMixedRestitutionFactor(final RigidBody body1, final RigidBody body2) {
-		final float restitution1 = _body1.getMaterial().getBounciness();
+		final float restitution1 = body1.getMaterial().getBounciness();
-		final float restitution2 = _body2.getMaterial().getBounciness();
+		final float restitution2 = body2.getMaterial().getBounciness();
 		//Log.error("######################### restitution1=" + FMath.floatToString(restitution1));
 		//Log.error("######################### restitution2=" + FMath.floatToString(restitution2));
 		// Return the largest restitution factor
@@ -273,25 +273,25 @@ public class ContactSolver {
 	
 	/**
 	 *  Compute the mixed rolling resistance factor between two bodies
-	 * @param _body1 First body to compute
+	 * @param body1 First body to compute
-	 * @param _body2 Second body to compute
+	 * @param body2 Second body to compute
 	 * @return Mixed rolling resistance
 	 */
-	private float computeMixedRollingResistance(final RigidBody _body1, final RigidBody _body2) {
+	private float computeMixedRollingResistance(final RigidBody body1, final RigidBody body2) {
-		return 0.5f * (_body1.getMaterial().getRollingResistance() + _body2.getMaterial().getRollingResistance());
+		return 0.5f * (body1.getMaterial().getRollingResistance() + body2.getMaterial().getRollingResistance());
 	}
 	
 	/**
 	 *  Compute a penetration raint impulse
-	 * @param _deltaLambda Ratio to apply at the calculation.
+	 * @param deltaLambda Ratio to apply at the calculation.
-	 * @param[in,out] _contactPoint Contact point property
+	 * @param contactPoint Contact point property
 	 * @return Impulse of the penetration result
 	 */
-	private Impulse computePenetrationImpulse(final float _deltaLambda, final ContactPointSolver _contactPoint) {
+	private Impulse computePenetrationImpulse(final float deltaLambda, final ContactPointSolver contactPoint) {
-		//Log.error("=== r1CrossN=" + _contactPoint.r1CrossN);
+		//Log.error("=== r1CrossN=" + contactPoint.r1CrossN);
-		//Log.error("=== r2CrossN=" + _contactPoint.r2CrossN);
+		//Log.error("=== r2CrossN=" + contactPoint.r2CrossN);
-		return new Impulse(_contactPoint.normal.multiplyNew(-1).multiply(_deltaLambda), _contactPoint.r1CrossN.multiplyNew(-1).multiply(_deltaLambda), _contactPoint.normal.multiplyNew(_deltaLambda),
+		return new Impulse(contactPoint.normal.multiply(-1).multiply(deltaLambda), contactPoint.r1CrossN.multiply(-1).multiply(deltaLambda), contactPoint.normal.multiply(deltaLambda),
-				_contactPoint.r2CrossN.multiplyNew(_deltaLambda));
+				contactPoint.r2CrossN.multiply(deltaLambda));
 	}
 	
 	/**
@@ -303,13 +303,13 @@ public class ContactSolver {
 			//Log.warning("}}}}   ccc=" + ccc);
 			final ContactManifoldSolver manifold = this.contactConstraints.get(ccc);
 			// Get the inertia tensors of both bodies
-			final Matrix3f I1 = manifold.inverseInertiaTensorBody1;
+			final Matrix3f i1 = manifold.inverseInertiaTensorBody1;
 			//Log.warning("}}}}       I1=" + I1);
-			final Matrix3f I2 = manifold.inverseInertiaTensorBody2;
+			final Matrix3f i2 = manifold.inverseInertiaTensorBody2;
 			//Log.warning("}}}}       I2=" + I2);
 			// If we solve the friction raints at the center of the contact manifold
 			if (this.isSolveFrictionAtContactManifoldCenterActive) {
-				manifold.normal = new Vector3f(0.0f, 0.0f, 0.0f);
+				manifold.normal = Vector3f.ZERO;
 				//Log.error("]]]    manifold.normal=" + manifold.normal);
 			}
 			//Log.warning("}}}}       manifold.normal=" + manifold.normal);
@@ -331,15 +331,15 @@ public class ContactSolver {
 				//Log.warning("}}}}           contactPoint.normal=" + contactPoint.normal);
 				final ContactPoint externalContact = contactPoint.externalContact;
 				// Compute the velocity difference
-				final Vector3f deltaV = v2.addNew(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1));
+				final Vector3f deltaV = v2.add(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1));
 				//Log.warning("}}}}           deltaV=" + deltaV);
 				contactPoint.r1CrossN = contactPoint.r1.cross(contactPoint.normal);
 				contactPoint.r2CrossN = contactPoint.r2.cross(contactPoint.normal);
 				//Log.warning("}}}}           contactPoint.r1CrossN=" + contactPoint.r1CrossN);
 				//Log.warning("}}}}           contactPoint.r2CrossN=" + contactPoint.r2CrossN);
 				// Compute the inverse mass matrix K for the penetration raint
-				final float massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1.multiplyNew(contactPoint.r1CrossN)).cross(contactPoint.r1)).dot(contactPoint.normal)
+				final float massPenetration = manifold.massInverseBody1 + manifold.massInverseBody2 + ((i1.multiply(contactPoint.r1CrossN)).cross(contactPoint.r1)).dot(contactPoint.normal)
-						+ ((I2.multiplyNew(contactPoint.r2CrossN)).cross(contactPoint.r2)).dot(contactPoint.normal);
+						+ ((i2.multiply(contactPoint.r2CrossN)).cross(contactPoint.r2)).dot(contactPoint.normal);
 				//Log.warning("}}}}           massPenetration=" + FMath.floatToString(massPenetration));
 				if (massPenetration > 0.0f) {
 					contactPoint.inversePenetrationMass = 1.0f / massPenetration;
@@ -361,11 +361,10 @@ public class ContactSolver {
 					//Log.warning("}}}}           contactPoint.r2CrossT2=" + contactPoint.r2CrossT2);
 					// Compute the inverse mass matrix K for the friction
 					// raints at each contact point
-					final float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2
+					final float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((i1.multiply(contactPoint.r1CrossT1)).cross(contactPoint.r1)).dot(contactPoint.frictionVector1)
-							+ ((I1.multiplyNew(contactPoint.r1CrossT1)).cross(contactPoint.r1)).dot(contactPoint.frictionVector1)
+							+ ((i2.multiply(contactPoint.r2CrossT1)).cross(contactPoint.r2)).dot(contactPoint.frictionVector1);
-							+ ((I2.multiplyNew(contactPoint.r2CrossT1)).cross(contactPoint.r2)).dot(contactPoint.frictionVector1);
+					final float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((i1.multiply(contactPoint.r1CrossT2)).cross(contactPoint.r1)).dot(contactPoint.frictionvec2)
-					final float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1.multiplyNew(contactPoint.r1CrossT2)).cross(contactPoint.r1)).dot(contactPoint.frictionvec2)
+							+ ((i2.multiply(contactPoint.r2CrossT2)).cross(contactPoint.r2)).dot(contactPoint.frictionvec2);
-							+ ((I2.multiplyNew(contactPoint.r2CrossT2)).cross(contactPoint.r2)).dot(contactPoint.frictionvec2);
 					if (friction1Mass > 0.0f) {
 						contactPoint.inverseFriction1Mass = 1.0f / friction1Mass;
 					}
@@ -383,7 +382,7 @@ public class ContactSolver {
 				//Log.warning("}}}}+++++++++++++++++++++           deltaV=" + deltaV);
 				//Log.warning("}}}}+++++++++++++++++++++           contactPoint.normal=" + contactPoint.normal);
 				//Log.warning("}}}}+++++++++++++++++++++           deltaVDotN=" + FMath.floatToString(deltaVDotN));
-				//Log.warning("}}}}+++++++++++++++++++++           Configuration.RESTITUTION_VELOCITY_THRESHOLD=" + FMath.floatToString(Configuration.RESTITUTION_VELOCITY_THRESHOLD));
+				//Log.warning("}}}}+++++++++++++++++++++           Configuration.RESTITUTIONVELOCITYTHRESHOLD=" + FMath.floatToString(Configuration.RESTITUTIONVELOCITYTHRESHOLD));
 				//Log.warning("}}}}+++++++++++++++++++++           manifold.restitutionFactor=" + FMath.floatToString(manifold.restitutionFactor));
 				if (deltaVDotN < -Configuration.RESTITUTION_VELOCITY_THRESHOLD) {
 					contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN;
@@ -402,22 +401,22 @@ public class ContactSolver {
 				// If we solve the friction raints at the center of the contact manifold
 				if (this.isSolveFrictionAtContactManifoldCenterActive) {
 					//Log.error("]]]    contactPoint.normal=" + contactPoint.normal);
-					manifold.normal.add(contactPoint.normal);
+					manifold.normal = manifold.normal.add(contactPoint.normal);
 					//Log.error("]]]    manifold.normal=" + manifold.normal);
 				}
 			}
 			// Compute the inverse K matrix for the rolling resistance raint
-			manifold.inverseRollingResistance.setZero();
+			manifold.inverseRollingResistance = Matrix3f.ZERO;
 			if (manifold.rollingResistanceFactor > 0 && (manifold.isBody1DynamicType || manifold.isBody2DynamicType)) {
-				manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1.addNew(manifold.inverseInertiaTensorBody2);
+				manifold.inverseRollingResistance = manifold.inverseInertiaTensorBody1.add(manifold.inverseInertiaTensorBody2);
-				manifold.inverseRollingResistance = manifold.inverseRollingResistance.inverseNew();
+				manifold.inverseRollingResistance = manifold.inverseRollingResistance.inverse();
 			}
 			// If we solve the friction raints at the center of the contact manifold
 			if (this.isSolveFrictionAtContactManifoldCenterActive) {
 				//Log.error("]]]    manifold.normal=" + manifold.normal);
-				manifold.normal.normalize();
+				manifold.normal = manifold.normal.normalize();
 				//Log.error("]]]    manifold.normal=" + manifold.normal);
-				final Vector3f deltaVFrictionPoint = v2.addNew(w2.cross(manifold.r2Friction)).less(v1).less(w1.cross(manifold.r1Friction));
+				final Vector3f deltaVFrictionPoint = v2.add(w2.cross(manifold.r2Friction)).less(v1).less(w1.cross(manifold.r1Friction));
 				//Log.error("]]]    deltaVFrictionPoint=" + deltaVFrictionPoint);
 				// Compute the friction vectors
 				computeFrictionVectors(deltaVFrictionPoint, manifold);
@@ -431,14 +430,14 @@ public class ContactSolver {
 				//Log.warning("}}}}           manifold.r1CrossT2=" + manifold.r1CrossT2);
 				//Log.warning("}}}}           manifold.r2CrossT1=" + manifold.r2CrossT1);
 				//Log.warning("}}}}           manifold.r2CrossT2=" + manifold.r2CrossT2);
-				final float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1.multiplyNew(manifold.r1CrossT1)).cross(manifold.r1Friction)).dot(manifold.frictionVector1)
+				final float friction1Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((i1.multiply(manifold.r1CrossT1)).cross(manifold.r1Friction)).dot(manifold.frictionVector1)
-						+ ((I2.multiplyNew(manifold.r2CrossT1)).cross(manifold.r2Friction)).dot(manifold.frictionVector1);
+						+ ((i2.multiply(manifold.r2CrossT1)).cross(manifold.r2Friction)).dot(manifold.frictionVector1);
 				//Log.error("]]]    friction1Mass=" + FMath.floatToString(friction1Mass));
-				final float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((I1.multiplyNew(manifold.r1CrossT2)).cross(manifold.r1Friction)).dot(manifold.frictionvec2)
+				final float friction2Mass = manifold.massInverseBody1 + manifold.massInverseBody2 + ((i1.multiply(manifold.r1CrossT2)).cross(manifold.r1Friction)).dot(manifold.frictionvec2)
-						+ ((I2.multiplyNew(manifold.r2CrossT2)).cross(manifold.r2Friction)).dot(manifold.frictionvec2);
+						+ ((i2.multiply(manifold.r2CrossT2)).cross(manifold.r2Friction)).dot(manifold.frictionvec2);
 				//Log.error("]]]    friction2Mass=" + FMath.floatToString(friction2Mass));
-				final float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1.multiplyNew(manifold.normal))
+				final float frictionTwistMass = manifold.normal.dot(manifold.inverseInertiaTensorBody1.multiply(manifold.normal))
-						+ manifold.normal.dot(manifold.inverseInertiaTensorBody2.multiplyNew(manifold.normal));
+						+ manifold.normal.dot(manifold.inverseInertiaTensorBody2.multiply(manifold.normal));
 				//Log.error("]]]    frictionTwistMass=" + FMath.floatToString(frictionTwistMass));
 				if (friction1Mass > 0.0f) {
 					manifold.inverseFriction1Mass = 1.0f / friction1Mass;
@@ -458,25 +457,25 @@ public class ContactSolver {
 	
 	/**
 	 *  Initialize the raint solver for a given island
-	 * @param _dt Delta step time
+	 * @param dt Delta step time
-	 * @param _island Island list property
+	 * @param island Island list property
 	 */
-	public void initializeForIsland(final float _dt, final Island _island) {
+	public void initializeForIsland(final float dt, final Island island) {
-		assert (_island != null);
+		assert (island != null);
-		assert (_island.getNbBodies() > 0);
+		assert (island.getNbBodies() > 0);
-		assert (_island.getNbContactManifolds() > 0);
+		assert (island.getNbContactManifolds() > 0);
 		assert (this.splitLinearVelocities != null);
 		assert (this.splitAngularVelocities != null);
 		// Set the current time step
-		// TODO: optimize this... this is so ugly ...
+		// TODO optimize this... this is so ugly ...
-		this.timeStep = _dt;
+		this.timeStep = dt;
 		{
-			this.contactConstraints = new ArrayList<>(_island.getNbContactManifolds());
+			this.contactConstraints = new ArrayList<>(island.getNbContactManifolds());
 		}
-		// this.contactConstraints.resize(_island.getNbContactManifolds());
+		// this.contactConstraints.resize(island.getNbContactManifolds());
 		// For each contact manifold of the island
-		final List<ContactManifold> contactManifolds = _island.getContactManifold();
+		final List<ContactManifold> contactManifolds = island.getContactManifold();
-		for (int iii = 0; iii < _island.getNbContactManifolds(); ++iii) {
+		for (int iii = 0; iii < island.getNbContactManifolds(); ++iii) {
 			final ContactManifold externalManifold = contactManifolds.get(iii);
 			final ContactManifoldSolver internalManifold = new ContactManifoldSolver(); // TODO Note no real need to reallocate a new one ==> maybe reuse (done in C++ not in java ...)
 			this.contactConstraints.add(internalManifold);
@@ -522,37 +521,37 @@ public class ContactSolver {
 				final Vector3f p2 = externalContact.getWorldPointOnBody2();
 				contactPoint.externalContact = externalContact;
 				contactPoint.normal = externalContact.getNormal();
-				contactPoint.r1 = p1.lessNew(x1);
+				contactPoint.r1 = p1.less(x1);
-				contactPoint.r2 = p2.lessNew(x2);
+				contactPoint.r2 = p2.less(x2);
 				contactPoint.penetrationDepth = externalContact.getPenetrationDepth();
 				contactPoint.isRestingContact = externalContact.getIsRestingContact();
 				externalContact.setIsRestingContact(true);
-				contactPoint.oldFrictionVector1 = externalContact.getFrictionVector1().clone();
+				contactPoint.oldFrictionVector1 = externalContact.getFrictionVector1();
-				contactPoint.oldFrictionvec2 = externalContact.getFrictionvec2().clone();
+				contactPoint.oldFrictionvec2 = externalContact.getFrictionvec2();
 				contactPoint.penetrationImpulse = 0.0f;
 				contactPoint.friction1Impulse = 0.0f;
 				contactPoint.friction2Impulse = 0.0f;
-				contactPoint.rollingResistanceImpulse = new Vector3f(0.0f, 0.0f, 0.0f);
+				contactPoint.rollingResistanceImpulse = Vector3f.ZERO;
 				// If we solve the friction raints at the center of the contact manifold
 				if (this.isSolveFrictionAtContactManifoldCenterActive) {
-					internalManifold.frictionPointBody1.add(p1);
+					internalManifold.frictionPointBody1 = internalManifold.frictionPointBody1.add(p1);
-					internalManifold.frictionPointBody2.add(p2);
+					internalManifold.frictionPointBody2 = internalManifold.frictionPointBody2.add(p2);
 					//Log.error("]]]    internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1);
 					//Log.error("]]]    internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2);
 				}
 			}
 			// If we solve the friction raints at the center of the contact manifold
 			if (this.isSolveFrictionAtContactManifoldCenterActive) {
-				internalManifold.frictionPointBody1.divide(internalManifold.nbContacts);
+				internalManifold.frictionPointBody1 = internalManifold.frictionPointBody1.divide(internalManifold.nbContacts);
-				internalManifold.frictionPointBody2.divide(internalManifold.nbContacts);
+				internalManifold.frictionPointBody2 = internalManifold.frictionPointBody2.divide(internalManifold.nbContacts);
 				//Log.error("]]]    internalManifold.frictionPointBody1=" + internalManifold.frictionPointBody1);
 				//Log.error("]]]    internalManifold.frictionPointBody2=" + internalManifold.frictionPointBody2);
-				internalManifold.r1Friction = internalManifold.frictionPointBody1.lessNew(x1);
+				internalManifold.r1Friction = internalManifold.frictionPointBody1.less(x1);
-				internalManifold.r2Friction = internalManifold.frictionPointBody2.lessNew(x2);
+				internalManifold.r2Friction = internalManifold.frictionPointBody2.less(x2);
 				//Log.error("]]]    internalManifold.r1Friction=" + internalManifold.r1Friction);
 				//Log.error("]]]    internalManifold.r2Friction=" + internalManifold.r2Friction);
-				internalManifold.oldFrictionVector1 = externalManifold.getFrictionVector1().clone();
+				internalManifold.oldFrictionVector1 = externalManifold.getFrictionVector1();
-				internalManifold.oldFrictionvec2 = externalManifold.getFrictionvec2().clone();
+				internalManifold.oldFrictionvec2 = externalManifold.getFrictionvec2();
 				//Log.error("]]]    internalManifold.oldFrictionVector1=" + internalManifold.oldFrictionVector1);
 				//Log.error("]]]    internalManifold.oldFrictionvec2=" + internalManifold.oldFrictionvec2);
 				// If warm starting is active
@@ -569,7 +568,7 @@ public class ContactSolver {
 					internalManifold.friction1Impulse = 0.0f;
 					internalManifold.friction2Impulse = 0.0f;
 					internalManifold.frictionTwistImpulse = 0.0f;
-					internalManifold.rollingResistanceImpulse = new Vector3f(0, 0, 0);
+					internalManifold.rollingResistanceImpulse = Vector3f.ZERO;
 				}
 			}
 		}
@@ -588,43 +587,43 @@ public class ContactSolver {
 	
 	/**
 	 *  Set the rained velocities arrays
-	 * @param _rainedLinearVelocities Constrained Linear velocities Table pointer (not free)
+	 * @param rainedLinearVelocities Constrained Linear velocities Table pointer (not free)
-	 * @param _rainedAngularVelocities Constrained angular velocities Table pointer (not free)
+	 * @param rainedAngularVelocities Constrained angular velocities Table pointer (not free)
 	 */
-	public void setConstrainedVelocitiesArrays(final Vector3f[] _rainedLinearVelocities, final Vector3f[] _rainedAngularVelocities) {
+	public void setConstrainedVelocitiesArrays(final Vector3f[] rainedLinearVelocities, final Vector3f[] rainedAngularVelocities) {
-		assert (_rainedLinearVelocities != null);
+		assert (rainedLinearVelocities != null);
-		assert (_rainedAngularVelocities != null);
+		assert (rainedAngularVelocities != null);
-		this.linearVelocities = _rainedLinearVelocities;
+		this.linearVelocities = rainedLinearVelocities;
-		this.angularVelocities = _rainedAngularVelocities;
+		this.angularVelocities = rainedAngularVelocities;
 	}
 	
 	/**
 	 * Activate or deactivate the solving of friction raints at the center of
 	 * the contact manifold instead of solving them at each contact point
-	 * @param _isActive Enable or not the center inertie
+	 * @param isActive Enable or not the center inertie
 	 */
-	public void setIsSolveFrictionAtContactManifoldCenterActive(final boolean _isActive) {
+	public void setIsSolveFrictionAtContactManifoldCenterActive(final boolean isActive) {
-		this.isSolveFrictionAtContactManifoldCenterActive = _isActive;
+		this.isSolveFrictionAtContactManifoldCenterActive = isActive;
 	}
 	
 	/**
 	 *  Activate or Deactivate the split impulses for contacts
-	 * @param _isActive status to set.
+	 * @param isActive status to set.
 	 */
-	public void setIsSplitImpulseActive(final boolean _isActive) {
+	public void setIsSplitImpulseActive(final boolean isActive) {
-		this.isSplitImpulseActive = _isActive;
+		this.isSplitImpulseActive = isActive;
 	}
 	
 	/**
 	 *  Set the split velocities arrays
-	 * @param _splitLinearVelocities Split linear velocities Table pointer (not free)
+	 * @param splitLinearVelocities Split linear velocities Table pointer (not free)
-	 * @param _splitAngularVelocities Split angular velocities Table pointer (not free)
+	 * @param splitAngularVelocities Split angular velocities Table pointer (not free)
 	 */
-	public void setSplitVelocitiesArrays(final Vector3f[] _splitLinearVelocities, final Vector3f[] _splitAngularVelocities) {
+	public void setSplitVelocitiesArrays(final Vector3f[] splitLinearVelocities, final Vector3f[] splitAngularVelocities) {
-		assert (_splitLinearVelocities != null);
+		assert (splitLinearVelocities != null);
-		assert (_splitAngularVelocities != null);
+		assert (splitAngularVelocities != null);
-		this.splitLinearVelocities = _splitLinearVelocities;
+		this.splitLinearVelocities = splitLinearVelocities;
-		this.splitAngularVelocities = _splitAngularVelocities;
+		this.splitAngularVelocities = splitAngularVelocities;
 	}
 	
 	/**
@@ -653,7 +652,7 @@ public class ContactSolver {
 				final ContactPointSolver contactPoint = contactManifold.contacts[iii];
 				// --------- Penetration --------- //
 				// Compute J*v
-				Vector3f deltaV = v2.addNew(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1));
+				Vector3f deltaV = v2.add(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1));
 				final float deltaVDotN = deltaV.dot(contactPoint.normal);
 				//Log.warning("        contactPoint.R1=" + contactPoint.r1);
 				//Log.warning("        contactPoint.R2=" + contactPoint.r2);
@@ -666,19 +665,19 @@ public class ContactSolver {
 				//Log.warning("        contactPoint.penetrationDepth=" + FMath.floatToString(contactPoint.penetrationDepth));
 				//Log.warning("        contactPoint.normal=" + contactPoint.normal);
 				//Log.warning("        deltaV=" + deltaV);
-				float Jv = deltaVDotN;
+				float jv = deltaVDotN;
 				// Compute the bias "b" of the raint
-				final float beta = this.isSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
+				final float beta = this.isSplitImpulseActive ? ContactSolver.BETA_SPLIT_IMPULSE : ContactSolver.BETA;
 				//Log.warning("        beta=" + FMath.floatToString(beta));
 				//Log.warning("        BETA=" + FMath.floatToString(BETA));
 				//Log.warning("        SLOP=" + FMath.floatToString(SLOP));
 				//Log.warning("        this.timeStep=" + FMath.floatToString(this.timeStep));
 				//Log.warning("        contactPoint.restitutionBias=" + FMath.floatToString(contactPoint.restitutionBias));
 				float biasPenetrationDepth = 0.0f;
-				if (contactPoint.penetrationDepth > SLOP) {
+				if (contactPoint.penetrationDepth > ContactSolver.SLOP) {
 					//Log.warning("        (beta / this.timeStep)=" + FMath.floatToString((beta / this.timeStep)));
 					//Log.warning("        FMath.max(0.0f, contactPoint.penetrationDepth - SLOP)=" + FMath.floatToString(FMath.max(0.0f, contactPoint.penetrationDepth - SLOP)));
-					biasPenetrationDepth = -(beta / this.timeStep) * FMath.max(0.0f, contactPoint.penetrationDepth - SLOP);
+					biasPenetrationDepth = -(beta / this.timeStep) * FMath.max(0.0f, contactPoint.penetrationDepth - ContactSolver.SLOP);
 					//Log.warning("        biasPenetrationDepth=" + FMath.floatToString(biasPenetrationDepth));
 				}
 				//Log.warning("        contactPoint.restitutionBias=" + FMath.floatToString(contactPoint.restitutionBias));
@@ -688,9 +687,9 @@ public class ContactSolver {
 				// Compute the Lagrange multiplier lambda
 				float deltaLambda;
 				if (this.isSplitImpulseActive) {
-					deltaLambda = -(Jv + contactPoint.restitutionBias) * contactPoint.inversePenetrationMass;
+					deltaLambda = -(jv + contactPoint.restitutionBias) * contactPoint.inversePenetrationMass;
 				} else {
-					deltaLambda = -(Jv + b) * contactPoint.inversePenetrationMass;
+					deltaLambda = -(jv + b) * contactPoint.inversePenetrationMass;
 				}
 				//Log.warning("        deltaLambda=" + FMath.floatToString(deltaLambda));
 				float lambdaTemp = contactPoint.penetrationImpulse;
@@ -717,10 +716,10 @@ public class ContactSolver {
 					final Vector3f w1Split = this.splitAngularVelocities[contactManifold.indexBody1];
 					final Vector3f v2Split = this.splitLinearVelocities[contactManifold.indexBody2];
 					final Vector3f w2Split = this.splitAngularVelocities[contactManifold.indexBody2];
-					final Vector3f deltaVSplit = v2Split.addNew(w2Split.cross(contactPoint.r2)).less(v1Split).less(w1Split.cross(contactPoint.r1));
+					final Vector3f deltaVSplit = v2Split.add(w2Split.cross(contactPoint.r2)).less(v1Split).less(w1Split.cross(contactPoint.r1));
 					//Log.warning("        deltaVSplit=" + deltaVSplit);
-					final float JvSplit = deltaVSplit.dot(contactPoint.normal);
+					final float jvSplit = deltaVSplit.dot(contactPoint.normal);
-					final float deltaLambdaSplit = -(JvSplit + biasPenetrationDepth) * contactPoint.inversePenetrationMass;
+					final float deltaLambdaSplit = -(jvSplit + biasPenetrationDepth) * contactPoint.inversePenetrationMass;
 					final float lambdaTempSplit = contactPoint.penetrationSplitImpulse;
 					contactPoint.penetrationSplitImpulse = FMath.max(contactPoint.penetrationSplitImpulse + deltaLambdaSplit, 0.0f);
 					deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit;
@@ -736,10 +735,10 @@ public class ContactSolver {
 				if (!this.isSolveFrictionAtContactManifoldCenterActive) {
 					// --------- Friction 1 --------- //
 					// Compute J*v
-					deltaV = v2.addNew(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1));
+					deltaV = v2.add(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1));
-					Jv = deltaV.dot(contactPoint.frictionVector1);
+					jv = deltaV.dot(contactPoint.frictionVector1);
 					// Compute the Lagrange multiplier lambda
-					deltaLambda = -Jv;
+					deltaLambda = -jv;
 					deltaLambda *= contactPoint.inverseFriction1Mass;
 					//Log.warning("        deltaLambda=" + FMath.floatToString(deltaLambda));
 					float frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse;
@@ -759,10 +758,10 @@ public class ContactSolver {
 					applyImpulse(impulseFriction1, contactManifold);
 					// --------- Friction 2 --------- //
 					// Compute J*v
-					deltaV = v2.addNew(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1));
+					deltaV = v2.add(w2.cross(contactPoint.r2)).less(v1).less(w1.cross(contactPoint.r1));
-					Jv = deltaV.dot(contactPoint.frictionvec2);
+					jv = deltaV.dot(contactPoint.frictionvec2);
 					// Compute the Lagrange multiplier lambda
-					deltaLambda = -Jv;
+					deltaLambda = -jv;
 					deltaLambda *= contactPoint.inverseFriction2Mass;
 					//Log.warning("        deltaLambda=" + FMath.floatToString(deltaLambda));
 					frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse;
@@ -781,15 +780,15 @@ public class ContactSolver {
 					// --------- Rolling resistance raint --------- //
 					if (contactManifold.rollingResistanceFactor > 0) {
 						// Compute J*v
-						final Vector3f JvRolling = w2.lessNew(w1);
+						final Vector3f jvRolling = w2.less(w1);
 						// Compute the Lagrange multiplier lambda
-						Vector3f deltaLambdaRolling = contactManifold.inverseRollingResistance.multiplyNew((JvRolling.multiplyNew(-1)));
+						Vector3f deltaLambdaRolling = contactManifold.inverseRollingResistance.multiply(jvRolling.multiply(-1));
 						final float rollingLimit = contactManifold.rollingResistanceFactor * contactPoint.penetrationImpulse;
 						final Vector3f lambdaTempRolling = contactPoint.rollingResistanceImpulse;
-						contactPoint.rollingResistanceImpulse = contactPoint.rollingResistanceImpulse.addNew(deltaLambdaRolling).clampNew(rollingLimit);
+						contactPoint.rollingResistanceImpulse = contactPoint.rollingResistanceImpulse.add(deltaLambdaRolling).clamp(rollingLimit);
-						deltaLambdaRolling = contactPoint.rollingResistanceImpulse.lessNew(lambdaTempRolling);
+						deltaLambdaRolling = contactPoint.rollingResistanceImpulse.less(lambdaTempRolling);
 						// Compute the impulse P=J^T * lambda
-						final Impulse impulseRolling = new Impulse(new Vector3f(0.0f, 0.0f, 0.0f), deltaLambdaRolling.multiplyNew(-1), new Vector3f(0.0f, 0.0f, 0.0f), deltaLambdaRolling);
+						final Impulse impulseRolling = new Impulse(Vector3f.ZERO, deltaLambdaRolling.multiply(-1), Vector3f.ZERO, deltaLambdaRolling);
 						//Log.warning("            impulseRolling.a1=" + impulseRolling.angularImpulseBody1);
 						//Log.warning("            impulseRolling.a2=" + impulseRolling.angularImpulseBody2);
 						//Log.warning("            impulseRolling.i1=" + impulseRolling.linearImpulseBody1);
@@ -808,14 +807,14 @@ public class ContactSolver {
 				
 				// ------ First friction raint at the center of the contact manifol ------ //
 				// Compute J*v
-				Vector3f deltaV = v2.addNew(w2.cross(contactManifold.r2Friction)).less(v1).less(w1.cross(contactManifold.r1Friction));
+				Vector3f deltaV = v2.add(w2.cross(contactManifold.r2Friction)).less(v1).less(w1.cross(contactManifold.r1Friction));
-				float Jv = deltaV.dot(contactManifold.frictionVector1);
+				float jv = deltaV.dot(contactManifold.frictionVector1);
 				//Log.info("            v2=" + v2);
 				//Log.warning("                Jv=" + FMath.floatToString(Jv));
 				//Log.warning("                contactManifold.frictionVector1=" + contactManifold.frictionVector1);
 				//Log.warning("                contactManifold.inverseFriction1Mass=" + FMath.floatToString(contactManifold.inverseFriction1Mass));
 				// Compute the Lagrange multiplier lambda
-				float deltaLambda = -Jv * contactManifold.inverseFriction1Mass;
+				float deltaLambda = -jv * contactManifold.inverseFriction1Mass;
 				//Log.warning("                contactManifold.frictionCoefficient=" + FMath.floatToString(contactManifold.frictionCoefficient));
 				//Log.warning("                sumPenetrationImpulse=" + FMath.floatToString(sumPenetrationImpulse));
 				//Log.info("            v2=" + v2);
@@ -828,10 +827,10 @@ public class ContactSolver {
 				deltaLambda = contactManifold.friction1Impulse - lambdaTemp;
 				//Log.warning("                deltaLambda=" + FMath.floatToString(deltaLambda));
 				// Compute the impulse P=J^T * lambda
-				Vector3f linearImpulseBody1 = contactManifold.frictionVector1.multiplyNew(-1).multiply(deltaLambda);
+				Vector3f linearImpulseBody1 = contactManifold.frictionVector1.multiply(-1).multiply(deltaLambda);
-				Vector3f angularImpulseBody1 = contactManifold.r1CrossT1.multiplyNew(-1).multiply(deltaLambda);
+				Vector3f angularImpulseBody1 = contactManifold.r1CrossT1.multiply(-1).multiply(deltaLambda);
-				Vector3f linearImpulseBody2 = contactManifold.frictionVector1.multiplyNew(deltaLambda);
+				Vector3f linearImpulseBody2 = contactManifold.frictionVector1.multiply(deltaLambda);
-				Vector3f angularImpulseBody2 = contactManifold.r2CrossT1.multiplyNew(deltaLambda);
+				Vector3f angularImpulseBody2 = contactManifold.r2CrossT1.multiply(deltaLambda);
 				
 				//Log.info("            v2=" + v2);
 				final Impulse impulseFriction1 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2);
@@ -849,13 +848,13 @@ public class ContactSolver {
 				//Log.info("            contactManifold.r1Friction=" + contactManifold.r1Friction);
 				// ------ Second friction raint at the center of the contact manifol ----- //
 				// Compute J*v
-				deltaV = v2.addNew(w2.cross(contactManifold.r2Friction)).less(v1).less(w1.cross(contactManifold.r1Friction));
+				deltaV = v2.add(w2.cross(contactManifold.r2Friction)).less(v1).less(w1.cross(contactManifold.r1Friction));
 				//Log.warning(">>>>            deltaV=" + deltaV);
-				Jv = deltaV.dot(contactManifold.frictionvec2);
+				jv = deltaV.dot(contactManifold.frictionvec2);
 				//Log.warning(">>>>            Jv=" + FMath.floatToString(Jv));
 				//Log.warning(">>>>            contactManifold.inverseFriction2Mass=" + FMath.floatToString(contactManifold.inverseFriction2Mass));
 				// Compute the Lagrange multiplier lambda
-				deltaLambda = -Jv * contactManifold.inverseFriction2Mass;
+				deltaLambda = -jv * contactManifold.inverseFriction2Mass;
 				//Log.warning(">>>>            deltaLambda=" + FMath.floatToString(deltaLambda));
 				frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
 				lambdaTemp = contactManifold.friction2Impulse;
@@ -867,10 +866,10 @@ public class ContactSolver {
 				deltaLambda = contactManifold.friction2Impulse - lambdaTemp;
 				//Log.warning(">>>>            deltaLambda=" + FMath.floatToString(deltaLambda));
 				// Compute the impulse P=J^T * lambda
-				linearImpulseBody1 = contactManifold.frictionvec2.multiplyNew(-deltaLambda);
+				linearImpulseBody1 = contactManifold.frictionvec2.multiply(-deltaLambda);
-				angularImpulseBody1 = contactManifold.r1CrossT2.multiplyNew(-deltaLambda);
+				angularImpulseBody1 = contactManifold.r1CrossT2.multiply(-deltaLambda);
-				linearImpulseBody2 = contactManifold.frictionvec2.multiplyNew(deltaLambda);
+				linearImpulseBody2 = contactManifold.frictionvec2.multiply(deltaLambda);
-				angularImpulseBody2 = contactManifold.r2CrossT2.multiplyNew(deltaLambda);
+				angularImpulseBody2 = contactManifold.r2CrossT2.multiply(deltaLambda);
 				
 				final Impulse impulseFriction2 = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2);
 				//Log.warning("            impulseFriction2.a1=" + impulseFriction2.angularImpulseBody1);
@@ -885,13 +884,13 @@ public class ContactSolver {
 				//Log.info("            w1=" + w1);
 				// ------ Twist friction raint at the center of the contact manifol ------ //
 				// Compute J*v
-				deltaV = w2.lessNew(w1);
+				deltaV = w2.less(w1);
 				//Log.warning("            deltaV=" + deltaV);
 				//Log.warning("            contactManifold.normal=" + contactManifold.normal);
-				Jv = deltaV.dot(contactManifold.normal);
+				jv = deltaV.dot(contactManifold.normal);
 				//Log.warning("            Jv=" + FMath.floatToString(Jv));
 				//Log.warning("            contactManifold.inverseTwistFrictionMass=" + contactManifold.inverseTwistFrictionMass);
-				deltaLambda = -Jv * (contactManifold.inverseTwistFrictionMass);
+				deltaLambda = -jv * (contactManifold.inverseTwistFrictionMass);
 				//Log.warning("            deltaLambda=" + FMath.floatToString(deltaLambda));
 				frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse;
 				//Log.warning("            contactManifold.frictionCoefficient=" + contactManifold.frictionCoefficient);
@@ -904,10 +903,10 @@ public class ContactSolver {
 				deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp;
 				//Log.warning("            deltaLambda=" + deltaLambda);
 				// Compute the impulse P=J^T * lambda
-				linearImpulseBody1 = new Vector3f(0.0f, 0.0f, 0.0f);
+				linearImpulseBody1 = Vector3f.ZERO;
-				angularImpulseBody1 = contactManifold.normal.multiplyNew(-deltaLambda);
+				angularImpulseBody1 = contactManifold.normal.multiply(-deltaLambda);
-				linearImpulseBody2 = new Vector3f(0.0f, 0.0f, 0.0f);
+				linearImpulseBody2 = Vector3f.ZERO;
-				angularImpulseBody2 = contactManifold.normal.multiplyNew(deltaLambda);
+				angularImpulseBody2 = contactManifold.normal.multiply(deltaLambda);
 				
 				final Impulse impulseTwistFriction = new Impulse(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2);
 				//Log.warning("            impulseTwistFriction.a1=" + impulseTwistFriction.angularImpulseBody1);
@@ -919,18 +918,18 @@ public class ContactSolver {
 				// --------- Rolling resistance raint at the center of the contact manifold --------- //
 				if (contactManifold.rollingResistanceFactor > 0) {
 					// Compute J*v
-					final Vector3f JvRolling = w2.lessNew(w1);
+					final Vector3f jvRolling = w2.less(w1);
 					// Compute the Lagrange multiplier lambda
-					Vector3f deltaLambdaRolling = contactManifold.inverseRollingResistance.multiplyNew(JvRolling.multiplyNew(-1));
+					Vector3f deltaLambdaRolling = contactManifold.inverseRollingResistance.multiply(jvRolling.multiply(-1));
 					final float rollingLimit = contactManifold.rollingResistanceFactor * sumPenetrationImpulse;
 					final Vector3f lambdaTempRolling = contactManifold.rollingResistanceImpulse;
-					contactManifold.rollingResistanceImpulse = (contactManifold.rollingResistanceImpulse.addNew(deltaLambdaRolling)).clampNew(rollingLimit);
+					contactManifold.rollingResistanceImpulse = (contactManifold.rollingResistanceImpulse.add(deltaLambdaRolling)).clamp(rollingLimit);
-					deltaLambdaRolling = contactManifold.rollingResistanceImpulse.lessNew(lambdaTempRolling);
+					deltaLambdaRolling = contactManifold.rollingResistanceImpulse.less(lambdaTempRolling);
 					// Compute the impulse P=J^T * lambda
-					angularImpulseBody1 = deltaLambdaRolling.multiplyNew(-1);
+					angularImpulseBody1 = deltaLambdaRolling.multiply(-1);
 					angularImpulseBody2 = deltaLambdaRolling;
 					
-					final Impulse impulseRolling = new Impulse(new Vector3f(0.0f, 0.0f, 0.0f), angularImpulseBody1, new Vector3f(0.0f, 0.0f, 0.0f), angularImpulseBody2);
+					final Impulse impulseRolling = new Impulse(Vector3f.ZERO, angularImpulseBody1, Vector3f.ZERO, angularImpulseBody2);
 					//Log.warning("            impulseRolling.a1=" + impulseRolling.angularImpulseBody1);
 					//Log.warning("            impulseRolling.a2=" + impulseRolling.angularImpulseBody2);
 					//Log.warning("            impulseRolling.i1=" + impulseRolling.linearImpulseBody1);
@@ -955,16 +954,16 @@ public class ContactSolver {
 				contactPoint.externalContact.setPenetrationImpulse(contactPoint.penetrationImpulse);
 				contactPoint.externalContact.setFrictionImpulse1(contactPoint.friction1Impulse);
 				contactPoint.externalContact.setFrictionImpulse2(contactPoint.friction2Impulse);
-				contactPoint.externalContact.setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse.clone());
+				contactPoint.externalContact.setRollingResistanceImpulse(contactPoint.rollingResistanceImpulse);
-				contactPoint.externalContact.setFrictionVector1(contactPoint.frictionVector1.clone());
+				contactPoint.externalContact.setFrictionVector1(contactPoint.frictionVector1);
-				contactPoint.externalContact.setFrictionvec2(contactPoint.frictionvec2.clone());
+				contactPoint.externalContact.setFrictionvec2(contactPoint.frictionvec2);
 			}
 			manifold.externalContactManifold.setFrictionImpulse1(manifold.friction1Impulse);
 			manifold.externalContactManifold.setFrictionImpulse2(manifold.friction2Impulse);
 			manifold.externalContactManifold.setFrictionTwistImpulse(manifold.frictionTwistImpulse);
-			manifold.externalContactManifold.setRollingResistanceImpulse(manifold.rollingResistanceImpulse.clone());
+			manifold.externalContactManifold.setRollingResistanceImpulse(manifold.rollingResistanceImpulse);
-			manifold.externalContactManifold.setFrictionVector1(manifold.frictionVector1.clone());
+			manifold.externalContactManifold.setFrictionVector1(manifold.frictionVector1);
-			manifold.externalContactManifold.setFrictionvec2(manifold.frictionvec2.clone());
+			manifold.externalContactManifold.setFrictionvec2(manifold.frictionvec2);
 		}
 	}
 	
@@ -996,8 +995,8 @@ public class ContactSolver {
 					if (!this.isSolveFrictionAtContactManifoldCenterActive) {
 						// Project the old friction impulses (with old friction vectors) into
 						// the new friction vectors to get the new friction impulses
-						final Vector3f oldFrictionImpulse = contactPoint.oldFrictionVector1.multiplyNew(contactPoint.friction1Impulse)
+						final Vector3f oldFrictionImpulse = contactPoint.oldFrictionVector1.multiply(contactPoint.friction1Impulse)
-								.add(contactPoint.oldFrictionvec2.multiplyNew(contactPoint.friction2Impulse));
+								.add(contactPoint.oldFrictionvec2.multiply(contactPoint.friction2Impulse));
 						contactPoint.friction1Impulse = oldFrictionImpulse.dot(contactPoint.frictionVector1);
 						contactPoint.friction2Impulse = oldFrictionImpulse.dot(contactPoint.frictionvec2);
 						// --------- Friction 1 --------- //
@@ -1013,7 +1012,7 @@ public class ContactSolver {
 						// ------ Rolling resistance------ //
 						if (contactManifold.rollingResistanceFactor > 0) {
 							// Compute the impulse P = J^T * lambda
-							final Impulse impulseRollingResistance = new Impulse(new Vector3f(0.0f, 0.0f, 0.0f), contactPoint.rollingResistanceImpulse.multiplyNew(-1), new Vector3f(0.0f, 0.0f, 0.0f),
+							final Impulse impulseRollingResistance = new Impulse(Vector3f.ZERO, contactPoint.rollingResistanceImpulse.multiply(-1), Vector3f.ZERO,
 									contactPoint.rollingResistanceImpulse);
 							// Apply the impulses to the bodies of the raint
 							applyImpulse(impulseRollingResistance, contactManifold);
@@ -1025,7 +1024,7 @@ public class ContactSolver {
 					contactPoint.penetrationImpulse = 0.0f;
 					contactPoint.friction1Impulse = 0.0f;
 					contactPoint.friction2Impulse = 0.0f;
-					contactPoint.rollingResistanceImpulse = new Vector3f(0.0f, 0.0f, 0.0f);
+					contactPoint.rollingResistanceImpulse = Vector3f.ZERO;
 				}
 			}
 			// If we solve the friction raints at the center of the contact manifold and there is
@@ -1034,8 +1033,8 @@ public class ContactSolver {
 				
 				// Project the old friction impulses (with old friction vectors) into the new friction
 				// vectors to get the new friction impulses
-				final Vector3f oldFrictionImpulse = contactManifold.oldFrictionVector1.multiplyNew(contactManifold.friction1Impulse)
+				final Vector3f oldFrictionImpulse = contactManifold.oldFrictionVector1.multiply(contactManifold.friction1Impulse)
-						.add(contactManifold.oldFrictionvec2.multiplyNew(contactManifold.friction2Impulse));
+						.add(contactManifold.oldFrictionvec2.multiply(contactManifold.friction2Impulse));
 				
 				//Log.error("]]]    oldFrictionImpulse=" + oldFrictionImpulse);
 				contactManifold.friction1Impulse = oldFrictionImpulse.dot(contactManifold.frictionVector1);
@@ -1044,10 +1043,10 @@ public class ContactSolver {
 				//Log.error("]]]    contactManifold.friction2Impulse=" + contactManifold.friction2Impulse);
 				// ------ First friction raint at the center of the contact manifold ------ //
 				// Compute the impulse P = J^T * lambda
-				Vector3f linearImpulseBody1 = contactManifold.frictionVector1.multiplyNew(-1).multiply(contactManifold.friction1Impulse);
+				Vector3f linearImpulseBody1 = contactManifold.frictionVector1.multiply(-1).multiply(contactManifold.friction1Impulse);
-				Vector3f angularImpulseBody1 = contactManifold.r1CrossT1.multiplyNew(-1).multiply(contactManifold.friction1Impulse);
+				Vector3f angularImpulseBody1 = contactManifold.r1CrossT1.multiply(-1).multiply(contactManifold.friction1Impulse);
-				Vector3f linearImpulseBody2 = contactManifold.frictionVector1.multiplyNew(contactManifold.friction1Impulse);
+				Vector3f linearImpulseBody2 = contactManifold.frictionVector1.multiply(contactManifold.friction1Impulse);
-				Vector3f angularImpulseBody2 = contactManifold.r2CrossT1.multiplyNew(contactManifold.friction1Impulse);
+				Vector3f angularImpulseBody2 = contactManifold.r2CrossT1.multiply(contactManifold.friction1Impulse);
 				
 				//Log.error("]]]    linearImpulseBody1=" + linearImpulseBody1);
 				//Log.error("]]]    angularImpulseBody1=" + angularImpulseBody1);
@@ -1058,10 +1057,10 @@ public class ContactSolver {
 				applyImpulse(impulseFriction1, contactManifold);
 				// ------ Second friction raint at the center of the contact manifold ----- //
 				// Compute the impulse P = J^T * lambda
-				linearImpulseBody1 = contactManifold.frictionvec2.multiplyNew(-1).multiply(contactManifold.friction2Impulse);
+				linearImpulseBody1 = contactManifold.frictionvec2.multiply(-1).multiply(contactManifold.friction2Impulse);
-				angularImpulseBody1 = contactManifold.r1CrossT2.multiplyNew(-1).multiply(contactManifold.friction2Impulse);
+				angularImpulseBody1 = contactManifold.r1CrossT2.multiply(-1).multiply(contactManifold.friction2Impulse);
-				linearImpulseBody2 = contactManifold.frictionvec2.multiplyNew(contactManifold.friction2Impulse);
+				linearImpulseBody2 = contactManifold.frictionvec2.multiply(contactManifold.friction2Impulse);
-				angularImpulseBody2 = contactManifold.r2CrossT2.multiplyNew(contactManifold.friction2Impulse);
+				angularImpulseBody2 = contactManifold.r2CrossT2.multiply(contactManifold.friction2Impulse);
 				
 				//Log.error("]]]    linearImpulseBody1=" + linearImpulseBody1);
 				//Log.error("]]]    angularImpulseBody1=" + angularImpulseBody1);
@@ -1072,10 +1071,10 @@ public class ContactSolver {
 				applyImpulse(impulseFriction2, contactManifold);
 				// ------ Twist friction raint at the center of the contact manifold ------ //
 				// Compute the impulse P = J^T * lambda
-				linearImpulseBody1 = new Vector3f(0.0f, 0.0f, 0.0f);
+				linearImpulseBody1 = Vector3f.ZERO;
-				angularImpulseBody1 = contactManifold.normal.multiplyNew(contactManifold.frictionTwistImpulse);
+				angularImpulseBody1 = contactManifold.normal.multiply(contactManifold.frictionTwistImpulse);
-				linearImpulseBody2 = new Vector3f(0.0f, 0.0f, 0.0f);
+				linearImpulseBody2 = Vector3f.ZERO;
-				angularImpulseBody2 = contactManifold.normal.multiplyNew(contactManifold.frictionTwistImpulse);
+				angularImpulseBody2 = contactManifold.normal.multiply(contactManifold.frictionTwistImpulse);
 				
 				//Log.error("]]]    linearImpulseBody1=" + linearImpulseBody1);
 				//Log.error("]]]    angularImpulseBody1=" + angularImpulseBody1);
@@ -1086,23 +1085,21 @@ public class ContactSolver {
 				applyImpulse(impulseTwistFriction, contactManifold);
 				// ------ Rolling resistance at the center of the contact manifold ------ //
 				// Compute the impulse P = J^T * lambda
-				angularImpulseBody1 = contactManifold.rollingResistanceImpulse.multiplyNew(-1);
+				angularImpulseBody1 = contactManifold.rollingResistanceImpulse.multiply(-1);
 				angularImpulseBody2 = contactManifold.rollingResistanceImpulse;
 				
 				//Log.error("]]]    angularImpulseBody1=" + angularImpulseBody1);
 				//Log.error("]]]    angularImpulseBody2=" + angularImpulseBody2);
-				final Impulse impulseRollingResistance = new Impulse(new Vector3f(0.0f, 0.0f, 0.0f), angularImpulseBody1, new Vector3f(0.0f, 0.0f, 0.0f), angularImpulseBody2);
+				final Impulse impulseRollingResistance = new Impulse(Vector3f.ZERO, angularImpulseBody1, Vector3f.ZERO, angularImpulseBody2);
 				// Apply the impulses to the bodies of the raint
 				applyImpulse(impulseRollingResistance, contactManifold);
-			} else
+			} else {
-			
-			{
 				// If it is a new contact manifold
 				// Initialize the accumulated impulses to zero
 				contactManifold.friction1Impulse = 0.0f;
 				contactManifold.friction2Impulse = 0.0f;
 				contactManifold.frictionTwistImpulse = 0.0f;
-				contactManifold.rollingResistanceImpulse = new Vector3f(0.0f, 0.0f, 0.0f);
+				contactManifold.rollingResistanceImpulse = Vector3f.ZERO;
 			}
 		}
 	}

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

@@ -42,41 +42,40 @@ import org.atriasoft.etk.math.Vector3f;
 public class DynamicsWorld extends CollisionWorld {
 	private static int kkk = 0;
 	protected ContactSolver contactSolver; //!< Contact solver
-	protected ConstraintSolver raintSolver; //!< Constraint solver
+	protected Vector3f gravity = Vector3f.ZERO; //!< Gravity vector of the world
-	protected int nbVelocitySolverIterations; //!< Number of iterations for the velocity solver of the Sequential Impulses technique
-	protected int nbPositionSolverIterations; //!< Number of iterations for the position solver of the Sequential Impulses technique
-	protected boolean isSleepingEnabled; //!< True if the spleeping technique for inactive bodies is enabled
-	protected List<RigidBody> rigidBodies = new ArrayList<>(); //!< All the rigid bodies of the physics world
-	protected List<Joint> joints = new ArrayList<>(); //!< All the joints of the world
-	protected Vector3f gravity = new Vector3f(); //!< Gravity vector of the world
-	public float timeStep; //!< Current frame time step (in seconds)
 	protected boolean isGravityEnabled; //!< True if the gravity force is on
-	protected Vector3f[] rainedLinearVelocities; //!< Array of rained linear velocities (state of the linear velocities after solving the constraints)
-	protected Vector3f[] rainedAngularVelocities; //!< Array of rained angular velocities (state of the angular velocities after solving the constraints)
-	protected Vector3f[] splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse)
-	protected Vector3f[] splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse)
-	protected Vector3f[] rainedPositions; //!< Array of rained rigid bodies position (for position error correction)
-	protected Quaternion[] rainedOrientations; //!< Array of rained rigid bodies orientation (for position error correction)
-	protected Map<RigidBody, Integer> mapBodyToConstrainedVelocityIndex = new HashMap<RigidBody, Integer>(); //!< Map body to their index in the rained velocities array
 	protected List<Island> islands = new ArrayList<>(); //!< Array with all the islands of awaken bodies
+	protected boolean isSleepingEnabled; //!< True if the spleeping technique for inactive bodies is enabled
+	protected List<Joint> joints = new ArrayList<>(); //!< All the joints of the world
+	protected Map<RigidBody, Integer> mapBodyToConstrainedVelocityIndex = new HashMap<>(); //!< Map body to their index in the rained velocities array
+	protected int nbPositionSolverIterations; //!< Number of iterations for the position solver of the Sequential Impulses technique
+	protected int nbVelocitySolverIterations; //!< Number of iterations for the velocity solver of the Sequential Impulses technique
 	protected int numberBodiesCapacity; //!< Current allocated capacity for the bodies
-	
+	protected Vector3f[] rainedAngularVelocities; //!< Array of rained angular velocities (state of the angular velocities after solving the constraints)
-	protected float sleepLinearVelocity; //!< Sleep linear velocity threshold
+	protected Vector3f[] rainedLinearVelocities; //!< Array of rained linear velocities (state of the linear velocities after solving the constraints)
+	protected Quaternion[] rainedOrientations; //!< Array of rained rigid bodies orientation (for position error correction)
+	protected Vector3f[] rainedPositions; //!< Array of rained rigid bodies position (for position error correction)
+	protected ConstraintSolver raintSolver; //!< Constraint solver
+	protected List<RigidBody> rigidBodies = new ArrayList<>(); //!< All the rigid bodies of the physics world
 	protected float sleepAngularVelocity; //!< Sleep angular velocity threshold
+	protected float sleepLinearVelocity; //!< Sleep linear velocity threshold
+	protected Vector3f[] splitAngularVelocities; //!< Split angular velocities for the position contact solver (split impulse)
+	
+	protected Vector3f[] splitLinearVelocities; //!< Split linear velocities for the position contact solver (split impulse)
 	protected float timeBeforeSleep; //!< Time (in seconds) before a body is put to sleep if its velocity becomes smaller than the sleep velocity.
+	public float timeStep; //!< Current frame time step (in seconds)
 	
 	/**
 	 *  Constructor
 	 * @param gravity Gravity vector in the world (in meters per second squared)
 	 */
-	public DynamicsWorld(final Vector3f _gravity) {
+	public DynamicsWorld(final Vector3f gravity) {
-		super();
 		this.contactSolver = new ContactSolver(this.mapBodyToConstrainedVelocityIndex);
 		this.raintSolver = new ConstraintSolver(this.mapBodyToConstrainedVelocityIndex);
 		this.nbVelocitySolverIterations = Configuration.DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS;
 		this.nbPositionSolverIterations = Configuration.DEFAULT_POSITION_SOLVER_NB_ITERATIONS;
 		this.isSleepingEnabled = Configuration.SPLEEPING_ENABLED;
-		this.gravity = _gravity;
+		this.gravity = gravity;
 		this.isGravityEnabled = true;
 		this.numberBodiesCapacity = 0;
 		this.sleepLinearVelocity = Configuration.DEFAULT_SLEEP_LINEAR_VELOCITY;
@@ -86,17 +85,17 @@ public class DynamicsWorld extends CollisionWorld {
 	
 	/**
 	 *  Add the joint to the list of joints of the two bodies involved in the joint
-	 * @param[in,out] _joint Joint to add at the body.
+	 * @param joint Joint to add at the body.
 	 */
-	protected void addJointToBody(final Joint _joint) {
+	protected void addJointToBody(final Joint joint) {
-		if (_joint == null) {
+		if (joint == null) {
 			//Log.warning("Request add null joint");
 			return;
 		}
 		// Add the joint at the beginning of the linked list of joints of the first body
-		_joint.getBody1().jointsList = new JointListElement(_joint, _joint.getBody1().jointsList);
+		joint.getBody1().jointsList = new JointListElement(joint, joint.getBody1().jointsList);
 		// Add the joint at the beginning of the linked list of joints of the second body
-		_joint.getBody2().jointsList = new JointListElement(_joint, _joint.getBody2().jointsList);
+		joint.getBody2().jointsList = new JointListElement(joint, joint.getBody2().jointsList);
 	}
 	
 	/**
@@ -219,37 +218,37 @@ public class DynamicsWorld extends CollisionWorld {
 	
 	/**
 	 *  Create a joint between two bodies in the world and return a pointer to the new joint
-	 * @param _jointInfo The information that is necessary to create the joint
+	 * @param jointInfo The information that is necessary to create the joint
 	 * @return A pointer to the joint that has been created in the world
 	 */
-	public Joint createJoint(final JointInfo _jointInfo) {
+	public Joint createJoint(final JointInfo jointInfo) {
 		Joint newJoint = null;
 		// Allocate memory to create the new joint
-		switch (_jointInfo.type) {
+		switch (jointInfo.type) {
 			// Ball-and-Socket joint
 			case BALLSOCKETJOINT:
-				newJoint = new BallAndSocketJoint((BallAndSocketJointInfo) _jointInfo);
+				newJoint = new BallAndSocketJoint((BallAndSocketJointInfo) jointInfo);
 				break;
 			// Slider joint
 			case SLIDERJOINT:
-				newJoint = new SliderJoint((SliderJointInfo) _jointInfo);
+				newJoint = new SliderJoint((SliderJointInfo) jointInfo);
 				break;
 			// Hinge joint
 			case HINGEJOINT:
-				newJoint = new HingeJoint((HingeJointInfo) _jointInfo);
+				newJoint = new HingeJoint((HingeJointInfo) jointInfo);
 				break;
 			// Fixed joint
 			case FIXEDJOINT:
-				newJoint = new FixedJoint((FixedJointInfo) _jointInfo);
+				newJoint = new FixedJoint((FixedJointInfo) jointInfo);
 				break;
 			default:
 				assert (false);
 				return null;
 		}
 		// If the collision between the two bodies of the raint is disabled
-		if (!_jointInfo.isCollisionEnabled) {
+		if (!jointInfo.isCollisionEnabled) {
 			// Add the pair of bodies in the set of body pairs that cannot collide with each other
-			this.collisionDetection.addNoCollisionPair(_jointInfo.body1, _jointInfo.body2);
+			this.collisionDetection.addNoCollisionPair(jointInfo.body1, jointInfo.body2);
 		}
 		// Add the joint into the world
 		this.joints.add(newJoint);
@@ -261,16 +260,16 @@ public class DynamicsWorld extends CollisionWorld {
 	
 	/**
 	 *  Create a rigid body into the physics world
-	 * @param _transform Transform3Dation from body local-space to world-space
+	 * @param transform Transform3Dation from body local-space to world-space
 	 * @return A pointer to the body that has been created in the world
 	 */
-	public RigidBody createRigidBody(final Transform3D _transform) {
+	public RigidBody createRigidBody(final Transform3D transform) {
 		// Compute the body ID
 		final int bodyID = computeNextAvailableBodyID();
 		// Largest index cannot be used (it is used for invalid index)
 		assert (bodyID < Integer.MAX_VALUE);
 		// Create the rigid body
-		final RigidBody rigidBody = new RigidBody(_transform, this, bodyID);
+		final RigidBody rigidBody = new RigidBody(transform, this, bodyID);
 		assert (rigidBody != null);
 		// Add the rigid body to the physics world
 		this.bodies.add(rigidBody);
@@ -281,58 +280,58 @@ public class DynamicsWorld extends CollisionWorld {
 	
 	/**
 	 * Destroy a joint
-	 * @param[in,out] _joint Pointer to the joint you want to destroy
+	 * @param joint Pointer to the joint you want to destroy
 	 */
-	public void destroyJoint(Joint _joint) {
+	public void destroyJoint(Joint joint) {
-		if (_joint == null) {
+		if (joint == null) {
 			//Log.warning("Request destroy null joint");
 			return;
 		}
 		// If the collision between the two bodies of the raint was disabled
-		if (!_joint.isCollisionEnabled()) {
+		if (!joint.isCollisionEnabled()) {
 			// Remove the pair of bodies from the set of body pairs that cannot collide with each other
-			this.collisionDetection.removeNoCollisionPair(_joint.getBody1(), _joint.getBody2());
+			this.collisionDetection.removeNoCollisionPair(joint.getBody1(), joint.getBody2());
 		}
 		// Wake up the two bodies of the joint
-		_joint.getBody1().setIsSleeping(false);
+		joint.getBody1().setIsSleeping(false);
-		_joint.getBody2().setIsSleeping(false);
+		joint.getBody2().setIsSleeping(false);
 		// Remove the joint from the world
-		this.joints.remove(_joint);
+		this.joints.remove(joint);
 		// Remove the joint from the joint list of the bodies involved in the joint
-		_joint.getBody1().removeJointFromjointsList(_joint);
+		joint.getBody1().removeJointFromjointsList(joint);
-		_joint.getBody2().removeJointFromjointsList(_joint);
+		joint.getBody2().removeJointFromjointsList(joint);
-		_joint = null;
+		joint = null;
 	}
 	
 	/**
 	 *  Destroy a rigid body and all the joints which it beints
-	 * @param[in,out] _rigidBody Pointer to the body you want to destroy
+	 * @param rigidBody Pointer to the body you want to destroy
 	 */
-	public void destroyRigidBody(RigidBody _rigidBody) {
+	public void destroyRigidBody(RigidBody rigidBody) {
 		// Remove all the collision shapes of the body
-		_rigidBody.removeAllCollisionShapes();
+		rigidBody.removeAllCollisionShapes();
 		// Add the body ID to the list of free IDs
-		this.freeBodiesIDs.add(_rigidBody.getID());
+		this.freeBodiesIDs.add(rigidBody.getID());
 		// Destroy all the joints in which the rigid body to be destroyed is involved
-		for (JointListElement element = _rigidBody.jointsList; element != null; element = element.next) {
+		for (JointListElement element = rigidBody.jointsList; element != null; element = element.next) {
 			destroyJoint(element.joint);
 		}
 		// Reset the contact manifold list of the body
-		_rigidBody.resetContactManifoldsList();
+		rigidBody.resetContactManifoldsList();
 		// Remove the rigid body from the list of rigid bodies
-		this.bodies.remove(_rigidBody);
+		this.bodies.remove(rigidBody);
-		this.rigidBodies.remove(_rigidBody);
+		this.rigidBodies.remove(rigidBody);
-		_rigidBody = null;
+		rigidBody = null;
 	}
 	
 	/**
 	 *  Enable/Disable the sleeping technique.
 	 * The sleeping technique is used to put bodies that are not moving into sleep
 	 * to speed up the simulation.
-	 * @param _isSleepingEnabled True if you want to enable the sleeping technique and false otherwise
+	 * @param isSleepingEnabled True if you want to enable the sleeping technique and false otherwise
 	 */
-	public void enableSleeping(final boolean _isSleepingEnabled) {
+	public void enableSleeping(final boolean isSleepingEnabled) {
-		this.isSleepingEnabled = _isSleepingEnabled;
+		this.isSleepingEnabled = isSleepingEnabled;
 		if (!this.isSleepingEnabled) {
 			// For each body of the world
 			for (final RigidBody it : this.rigidBodies) {
@@ -443,7 +442,7 @@ public class DynamicsWorld extends CollisionWorld {
 	protected void initVelocityArrays() {
 		// Allocate memory for the bodies velocity arrays
 		final int nbBodies = this.rigidBodies.size();
-		// this.numberBodiesCapacity = 0; // TODO: Remove this, this is a test of portage
+		// this.numberBodiesCapacity = 0; // TODO Remove this, this is a test of portage
 		if (this.numberBodiesCapacity < nbBodies) {
 			this.numberBodiesCapacity = nbBodies;
 			this.splitLinearVelocities = new Vector3f[this.numberBodiesCapacity];
@@ -453,22 +452,22 @@ public class DynamicsWorld extends CollisionWorld {
 			this.rainedPositions = new Vector3f[this.numberBodiesCapacity];
 			this.rainedOrientations = new Quaternion[this.numberBodiesCapacity];
 			for (int iii = 0; iii < this.numberBodiesCapacity; iii++) {
-				this.splitLinearVelocities[iii] = Vector3f.zero();
+				this.splitLinearVelocities[iii] = Vector3f.ZERO;
-				this.splitAngularVelocities[iii] = Vector3f.zero();
+				this.splitAngularVelocities[iii] = Vector3f.ZERO;
-				this.rainedLinearVelocities[iii] = Vector3f.zero();
+				this.rainedLinearVelocities[iii] = Vector3f.ZERO;
-				this.rainedAngularVelocities[iii] = Vector3f.zero();
+				this.rainedAngularVelocities[iii] = Vector3f.ZERO;
-				this.rainedPositions[iii] = Vector3f.zero();
+				this.rainedPositions[iii] = Vector3f.ZERO;
-				this.rainedOrientations[iii] = Quaternion.identity();
+				this.rainedOrientations[iii] = Quaternion.IDENTITY;
 			}
 		} else {
 			// Reset the velocities arrays
 			for (int iii = 0; iii < this.numberBodiesCapacity; iii++) {
-				this.splitLinearVelocities[iii].setZero();
+				this.splitLinearVelocities[iii] = Vector3f.ZERO;
-				this.splitAngularVelocities[iii].setZero();
+				this.splitAngularVelocities[iii] = Vector3f.ZERO;
-				this.rainedLinearVelocities[iii].setZero();
+				this.rainedLinearVelocities[iii] = Vector3f.ZERO;
-				this.rainedAngularVelocities[iii].setZero();
+				this.rainedAngularVelocities[iii] = Vector3f.ZERO;
-				this.rainedPositions[iii].setZero();
+				this.rainedPositions[iii] = Vector3f.ZERO;
-				this.rainedOrientations[iii].setIdentity();
+				this.rainedOrientations[iii] = Quaternion.IDENTITY;
 			}
 		}
 		// Initialize the map of body indexes in the velocity arrays
@@ -496,38 +495,38 @@ public class DynamicsWorld extends CollisionWorld {
 				//Log.error("  [" + b + "/" + bodies.size() + "]");
 				// Get the rained velocity
 				final int indexArray = this.mapBodyToConstrainedVelocityIndex.get(bodies.get(b));
-				final Vector3f newLinVelocity = this.rainedLinearVelocities[indexArray].clone();
+				Vector3f newLinVelocity = this.rainedLinearVelocities[indexArray];
-				final Vector3f newAngVelocity = this.rainedAngularVelocities[indexArray].clone();
+				Vector3f newAngVelocity = this.rainedAngularVelocities[indexArray];
 				//Log.error("      newAngVelocity = " + newAngVelocity);
 				// Add the split impulse velocity from Contact Solver (only used
 				// to update the position)
 				if (this.contactSolver.isSplitImpulseActive()) {
-					newLinVelocity.add(this.splitLinearVelocities[indexArray]);
+					newLinVelocity = newLinVelocity.add(this.splitLinearVelocities[indexArray]);
-					newAngVelocity.add(this.splitAngularVelocities[indexArray]);
+					newAngVelocity = newAngVelocity.add(this.splitAngularVelocities[indexArray]);
 				}
 				// Get current position and orientation of the body
 				final Vector3f currentPosition = bodies.get(b).centerOfMassWorld;
 				//Log.error("      bodies.get(b).getTransform() = " + bodies.get(b).getTransform());
 				final Quaternion currentOrientation = bodies.get(b).getTransform().getOrientation();
 				// Update the new rained position and orientation of the body
-				this.rainedPositions[indexArray] = newLinVelocity.multiplyNew(this.timeStep).add(currentPosition);
+				this.rainedPositions[indexArray] = newLinVelocity.multiply(this.timeStep).add(currentPosition);
 				//Log.error("      currentOrientation = " + currentOrientation);
 				//Log.error("      newAngVelocity = " + newAngVelocity);
 				//Log.error("      this.timeStep = " + FMath.floatToString(this.timeStep));
-				this.rainedOrientations[indexArray] = currentOrientation.addNew(new Quaternion(0, newAngVelocity).multiplyNew(currentOrientation).multiply(0.5f * this.timeStep));
+				this.rainedOrientations[indexArray] = currentOrientation.add(new Quaternion(0, newAngVelocity).multiply(currentOrientation).multiply(0.5f * this.timeStep));
 				//Log.error("      this.rainedPositions[indexArray] = " + this.rainedPositions[indexArray]);
 				//Log.error("      this.rainedOrientations[indexArray] = " + this.rainedOrientations[indexArray]);
 			}
 		}
 		for (int iii = 1; iii < this.rainedPositions.length; iii++) {
-			Log.error(kkk + "         this.rainedPositions[" + iii + "] = " + this.rainedPositions[iii]);
+			Log.error(DynamicsWorld.kkk + "         this.rainedPositions[" + iii + "] = " + this.rainedPositions[iii]);
-			Log.error(kkk + "      this.rainedOrientations[" + iii + "] = " + this.rainedOrientations[iii]);
+			Log.error(DynamicsWorld.kkk + "      this.rainedOrientations[" + iii + "] = " + this.rainedOrientations[iii]);
 		}
 		//Log.error("------------------------------------------------------------------------------------------------");
 		//Log.error("------------------------------------------------------------------------------------------------");
 		//Log.error("------------------------------------------------------------------------------------------------");
 		//Log.error("------------------------------------------------------------------------------------------------");
-		if (kkk++ == 98) {
+		if (DynamicsWorld.kkk++ == 98) {
 			//System.exit(-1);
 		}
 	}
@@ -552,25 +551,25 @@ public class DynamicsWorld extends CollisionWorld {
 				//Log.info("     body : " + bbb + "/" + bodies.size());
 				// Insert the body into the map of rained velocities
 				final RigidBody tmpval = bodies.get(bbb);
-				int indexBody = this.mapBodyToConstrainedVelocityIndex.get(tmpval);
+				final int indexBody = this.mapBodyToConstrainedVelocityIndex.get(tmpval);
 				//Log.info("         indexBody=" + indexBody);
 				
 				assert (this.splitLinearVelocities[indexBody].isZero());
 				assert (this.splitAngularVelocities[indexBody].isZero());
 				// Integrate the external force to get the new velocity of the body
-				this.rainedLinearVelocities[indexBody] = bodies.get(bbb).getLinearVelocity().addNew(bodies.get(bbb).externalForce.multiplyNew(this.timeStep * bodies.get(bbb).massInverse));
+				this.rainedLinearVelocities[indexBody] = bodies.get(bbb).getLinearVelocity().add(bodies.get(bbb).externalForce.multiply(this.timeStep * bodies.get(bbb).massInverse));
 				//Log.info("         this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]);
 				this.rainedAngularVelocities[indexBody] = bodies.get(bbb).getAngularVelocity()
-						.addNew(bodies.get(bbb).getInertiaTensorInverseWorld().multiplyNew(bodies.get(bbb).externalTorque.multiplyNew(this.timeStep)));
+						.add(bodies.get(bbb).getInertiaTensorInverseWorld().multiply(bodies.get(bbb).externalTorque.multiply(this.timeStep)));
 				//Log.info("         this.rainedAngularVelocities[indexBody]=" + this.rainedAngularVelocities[indexBody]);
 				// If the gravity has to be applied to this rigid body
 				if (bodies.get(bbb).isGravityEnabled() && this.isGravityEnabled) {
 					// Integrate the gravity force
-					this.rainedLinearVelocities[indexBody].add(this.gravity.multiplyNew(this.timeStep * bodies.get(bbb).massInverse * bodies.get(bbb).getMass()));
+					this.rainedLinearVelocities[indexBody] = this.rainedLinearVelocities[indexBody].add(this.gravity.multiply(this.timeStep * bodies.get(bbb).massInverse * bodies.get(bbb).getMass()));
 				}
 				//Log.info("   G     this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]);
 				// Apply the velocity damping
-				// Damping force : F_c = -c' * v (c=damping factor)
+				// Damping force : Fc = -c' * v (c=damping factor)
 				// Equation	  : m * dv/dt = -c' * v
 				//				 => dv/dt = -c * v (with c=c'/m)
 				//				 => dv/dt + c * v = 0
@@ -590,11 +589,10 @@ public class DynamicsWorld extends CollisionWorld {
 				//Log.info("         linearDamping=" + FMath.floatToString(linearDamping));
 				final float angularDamping = FMath.pow(1.0f - angDampingFactor, this.timeStep);
 				//Log.info("         angularDamping=" + FMath.floatToString(angularDamping));
-				this.rainedLinearVelocities[indexBody].multiply(linearDamping);
+				this.rainedLinearVelocities[indexBody] = this.rainedLinearVelocities[indexBody].multiply(linearDamping);
 				//Log.info("         this.rainedLinearVelocities[indexBody]=" + this.rainedLinearVelocities[indexBody]);
-				this.rainedAngularVelocities[indexBody].multiply(angularDamping);
+				this.rainedAngularVelocities[indexBody] = this.rainedAngularVelocities[indexBody].multiply(angularDamping);
 				//Log.info("         this.rainedAngularVelocities[indexBody]=" + this.rainedAngularVelocities[indexBody]);
-				indexBody++;
 			}
 		}
 	}
@@ -620,18 +618,18 @@ public class DynamicsWorld extends CollisionWorld {
 	 */
 	protected void resetBodiesForceAndTorque() {
 		// For each body of the world
-		this.rigidBodies.forEach((obj) -> {
+		this.rigidBodies.forEach(obj -> {
-			obj.externalForce.setZero();
+			obj.externalForce = Vector3f.ZERO;
-			obj.externalTorque.setZero();
+			obj.externalTorque = Vector3f.ZERO;
 		});
 	}
 	
 	/**
 	 *  Set the position correction technique used for contacts
-	 * @param _technique Technique used for the position correction (Baumgarte or Split Impulses)
+	 * @param technique Technique used for the position correction (Baumgarte or Split Impulses)
 	 */
-	public void setContactsPositionCorrectionTechnique(final ContactsPositionCorrectionTechnique _technique) {
+	public void setContactsPositionCorrectionTechnique(final ContactsPositionCorrectionTechnique technique) {
-		if (_technique == ContactsPositionCorrectionTechnique.BAUMGARTE_CONTACTS) {
+		if (technique == ContactsPositionCorrectionTechnique.BAUMGARTE_CONTACTS) {
 			this.contactSolver.setIsSplitImpulseActive(false);
 		} else {
 			this.contactSolver.setIsSplitImpulseActive(true);
@@ -641,46 +639,46 @@ public class DynamicsWorld extends CollisionWorld {
 	/**
 	 *  Set an event listener object to receive events callbacks.
 	 * @note If you use null as an argument, the events callbacks will be disabled.
-	 * @param _eventListener Pointer to the event listener object that will receive
+	 * @param eventListener Pointer to the event listener object that will receive
 	 * event callbacks during the simulation
 	 */
-	public void setEventListener(final EventListener _eventListener) {
+	public void setEventListener(final EventListener eventListener) {
-		this.eventListener = _eventListener;
+		this.eventListener = eventListener;
 	}
 	
 	/**
 	 *  Set the gravity vector of the world
-	 * @param _gravity The gravity vector (in meter per seconds squared)
+	 * @param gravity The gravity vector (in meter per seconds squared)
 	 */
-	public void setGravity(final Vector3f _gravity) {
+	public void setGravity(final Vector3f gravity) {
-		this.gravity = _gravity;
+		this.gravity = gravity;
 	}
 	
 	/**
 	 *  Enable/Disable the gravity
-	 * @param _isGravityEnabled True if you want to enable the gravity in the world
+	 * @param isGravityEnabled True if you want to enable the gravity in the world
 	 * and false otherwise
 	 */
-	public void setIsGratityEnabled(final boolean _isGravityEnabled) {
+	public void setIsGratityEnabled(final boolean isGravityEnabled) {
-		this.isGravityEnabled = _isGravityEnabled;
+		this.isGravityEnabled = isGravityEnabled;
 	}
 	
 	/**
 	 *  Activate or deactivate the solving of friction raints at the center of the contact
 	 * manifold instead of solving them at each contact point
-	 * @param _isActive True if you want the friction to be solved at the center of
+	 * @param isActive True if you want the friction to be solved at the center of
 	 * the contact manifold and false otherwise
 	 */
-	public void setIsSolveFrictionAtContactManifoldCenterActive(final boolean _isActive) {
+	public void setIsSolveFrictionAtContactManifoldCenterActive(final boolean isActive) {
-		this.contactSolver.setIsSolveFrictionAtContactManifoldCenterActive(_isActive);
+		this.contactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
 	}
 	
 	/**
 	 *  Set the position correction technique used for joints
-	 * @param _technique Technique used for the joins position correction (Baumgarte or Non Linear Gauss Seidel)
+	 * @param technique Technique used for the joins position correction (Baumgarte or Non Linear Gauss Seidel)
 	 */
-	public void setJointsPositionCorrectionTechnique(final JointsPositionCorrectionTechnique _technique) {
+	public void setJointsPositionCorrectionTechnique(final JointsPositionCorrectionTechnique technique) {
-		if (_technique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
+		if (technique == JointsPositionCorrectionTechnique.BAUMGARTE_JOINTS) {
 			this.raintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(false);
 		} else {
 			this.raintSolver.setIsNonLinearGaussSeidelPositionCorrectionActive(true);
@@ -689,18 +687,18 @@ public class DynamicsWorld extends CollisionWorld {
 	
 	/**
 	 *  Set the number of iterations for the position raint solver
-	 * @param _nbIterations Number of iterations for the position solver
+	 * @param nbIterations Number of iterations for the position solver
 	 */
-	public void setNbIterationsPositionSolver(final int _nbIterations) {
+	public void setNbIterationsPositionSolver(final int nbIterations) {
-		this.nbPositionSolverIterations = _nbIterations;
+		this.nbPositionSolverIterations = nbIterations;
 	}
 	
 	/**
 	*  Set the number of iterations for the velocity raint solver
-	* @param _nbIterations Number of iterations for the velocity solver
+	* @param nbIterations Number of iterations for the velocity solver
 	*/
-	public void setNbIterationsVelocitySolver(final int _nbIterations) {
+	public void setNbIterationsVelocitySolver(final int nbIterations) {
-		this.nbVelocitySolverIterations = _nbIterations;
+		this.nbVelocitySolverIterations = nbIterations;
 	}
 	
 	/**
@@ -708,26 +706,26 @@ public class DynamicsWorld extends CollisionWorld {
 	 * When the velocity of a body becomes smaller than the sleep linear/angular
 	 * velocity for a given amount of time, the body starts sleeping and does not need
 	 * to be simulated anymore.
-	 * @param _sleepAngularVelocity The sleep angular velocity (in radian per second)
+	 * @param sleepAngularVelocity The sleep angular velocity (in radian per second)
 	 */
-	public void setSleepAngularVelocity(final float _sleepAngularVelocity) {
+	public void setSleepAngularVelocity(final float sleepAngularVelocity) {
-		if (_sleepAngularVelocity < 0.0f) {
+		if (sleepAngularVelocity < 0.0f) {
-			//Log.error("Can not set _sleepAngularVelocity=" + _sleepAngularVelocity + " < 0 ");
+			//Log.error("Can not set sleepAngularVelocity=" + sleepAngularVelocity + " < 0 ");
 			return;
 		}
-		this.sleepAngularVelocity = _sleepAngularVelocity;
+		this.sleepAngularVelocity = sleepAngularVelocity;
 	}
 	
 	/**
 	 *  Set the sleep linear velocity
-	 * @param _sleepLinearVelocity The new sleep linear velocity (in meters per second)
+	 * @param sleepLinearVelocity The new sleep linear velocity (in meters per second)
 	 */
-	public void setSleepLinearVelocity(final float _sleepLinearVelocity) {
+	public void setSleepLinearVelocity(final float sleepLinearVelocity) {
-		if (_sleepLinearVelocity < 0.0f) {
+		if (sleepLinearVelocity < 0.0f) {
-			//Log.error("Can not set _sleepLinearVelocity=" + _sleepLinearVelocity + " < 0 ");
+			//Log.error("Can not set sleepLinearVelocity=" + sleepLinearVelocity + " < 0 ");
 			return;
 		}
-		this.sleepLinearVelocity = _sleepLinearVelocity;
+		this.sleepLinearVelocity = sleepLinearVelocity;
 	}
 	
 	/**
@@ -823,60 +821,60 @@ public class DynamicsWorld extends CollisionWorld {
 	}
 	
 	@Override
-	public void testCollision(final CollisionBody _body1, final CollisionBody _body2, final CollisionCallback _callback) {
+	public void testCollision(final CollisionBody body1, final CollisionBody body2, final CollisionCallback callback) {
 		// Create the sets of shapes
-		final Set<DTree> shapes1 = new Set<DTree>(Set.getDTreeCoparator());
+		final Set<DTree> shapes1 = new Set<>(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());
+		final Set<DTree> shapes2 = new Set<>(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.reportCollisionBetweenShapes(_callback, shapes1, shapes2);
+		this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
 	}
 	
 	@Override
-	public void testCollision(final CollisionBody _body, final CollisionCallback _callback) {
+	public void testCollision(final CollisionBody body, final CollisionCallback callback) {
 		// Create the sets of shapes
-		final Set<DTree> shapes1 = new Set<DTree>(Set.getDTreeCoparator());
+		final Set<DTree> shapes1 = new Set<>(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());
+		final Set<DTree> emptySet = new Set<>(Set.getDTreeCoparator());
 		// Perform the collision detection and report contacts
-		this.collisionDetection.reportCollisionBetweenShapes(_callback, shapes1, emptySet);
+		this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, emptySet);
 	}
 	
 	/// Test and report collisions between all shapes of the world
 	@Override
-	public void testCollision(final CollisionCallback _callback) {
+	public void testCollision(final CollisionCallback callback) {
-		final Set<DTree> emptySet = new Set<DTree>(Set.getDTreeCoparator());
+		final Set<DTree> emptySet = new Set<>(Set.getDTreeCoparator());
 		// Perform the collision detection and report contacts
-		this.collisionDetection.reportCollisionBetweenShapes(_callback, emptySet, emptySet);
+		this.collisionDetection.reportCollisionBetweenShapes(callback, emptySet, emptySet);
 	}
 	
 	@Override
-	public void testCollision(final ProxyShape _shape, final CollisionCallback _callback) {
+	public void testCollision(final ProxyShape shape, final CollisionCallback callback) {
 		// Create the sets of shapes
-		final Set<DTree> shapes = new Set<DTree>(Set.getDTreeCoparator());
+		final Set<DTree> shapes = new Set<>(Set.getDTreeCoparator());
-		shapes.add(_shape.broadPhaseID);
+		shapes.add(shape.broadPhaseID);
-		final Set<DTree> emptySet = new Set<DTree>(Set.getDTreeCoparator());
+		final Set<DTree> emptySet = new Set<>(Set.getDTreeCoparator());
 		// Perform the collision detection and report contacts
-		this.collisionDetection.reportCollisionBetweenShapes(_callback, shapes, emptySet);
+		this.collisionDetection.reportCollisionBetweenShapes(callback, shapes, emptySet);
 	}
 	
 	@Override
-	public void testCollision(final ProxyShape _shape1, final ProxyShape _shape2, final CollisionCallback _callback) {
+	public void testCollision(final ProxyShape shape1, final ProxyShape shape2, final CollisionCallback callback) {
 		// Create the sets of shapes
-		final Set<DTree> shapes1 = new Set<DTree>(Set.getDTreeCoparator());
+		final Set<DTree> shapes1 = new Set<>(Set.getDTreeCoparator());
-		shapes1.add(_shape1.broadPhaseID);
+		shapes1.add(shape1.broadPhaseID);
-		final Set<DTree> shapes2 = new Set<DTree>(Set.getDTreeCoparator());
+		final Set<DTree> shapes2 = new Set<>(Set.getDTreeCoparator());
-		shapes2.add(_shape2.broadPhaseID);
+		shapes2.add(shape2.broadPhaseID);
 		// Perform the collision detection and report contacts
-		this.collisionDetection.reportCollisionBetweenShapes(_callback, shapes1, shapes2);
+		this.collisionDetection.reportCollisionBetweenShapes(callback, shapes1, shapes2);
 	}
 	
 	/**
@@ -972,17 +970,17 @@ public class DynamicsWorld extends CollisionWorld {
 			for (int b = 0; b < this.islands.get(islandIndex).getNbBodies(); b++) {
 				final int index = this.mapBodyToConstrainedVelocityIndex.get(bodies.get(b));
 				// Update the linear and angular velocity of the body
-				bodies.get(b).linearVelocity = this.rainedLinearVelocities[index].clone();
+				bodies.get(b).linearVelocity = this.rainedLinearVelocities[index];
-				if (bodies.get(b).isAngularReactionEnable() == true) {
+				if (bodies.get(b).isAngularReactionEnable()) {
-					bodies.get(b).angularVelocity = this.rainedAngularVelocities[index].clone();
+					bodies.get(b).angularVelocity = this.rainedAngularVelocities[index];
 				}
 				// Update the position of the center of mass of the body
-				bodies.get(b).centerOfMassWorld = this.rainedPositions[index].clone();
+				bodies.get(b).centerOfMassWorld = this.rainedPositions[index];
 				// Update the orientation of the body
 				//Log.error("<< ============= this.rainedOrientations[index] " + this.rainedOrientations[index]);
 				//Log.error("<< ============= this.rainedOrientations[index].safeNormalizeNew() " + this.rainedOrientations[index].safeNormalizeNew());
-				if (bodies.get(b).isAngularReactionEnable() == true) {
+				if (bodies.get(b).isAngularReactionEnable()) {
-					bodies.get(b).getTransform().setOrientation(this.rainedOrientations[index].safeNormalizeNew());
+					bodies.get(b).setTransform(bodies.get(b).getTransform().withOrientation(this.rainedOrientations[index].safeNormalize()));
 				}
 				// Update the transform of the body (using the new center of mass and new orientation)
 				bodies.get(b).updateTransformWithCenterOfMass();

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 {
+@SuppressWarnings("preview")
-	public final Vector3f linearImpulseBody1; //!< Linear impulse applied to the first body
+public record Impulse(
-	public final Vector3f angularImpulseBody1; //!< Angular impulse applied to the first body
+		Vector3f linearImpulseBody1, //!< Linear impulse applied to the first body
-	public final Vector3f linearImpulseBody2; //!< Linear impulse applied to the second body
+		Vector3f angularImpulseBody1, //!< Angular impulse applied to the first body
-	public final Vector3f angularImpulseBody2; //!< Angular impulse applied to the second 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

@@ -23,34 +23,34 @@ public class Island {
 	 */
 	public Island(final int nbMaxBodies, final int nbMaxContactManifolds, final int nbMaxJoints) {
 		// Allocate memory for the arrays
-		//this.bodies.reserve(_nbMaxBodies);
+		//this.bodies.reserve(nbMaxBodies);
-		//this.contactManifolds.reserve(_nbMaxContactManifolds);
+		//this.contactManifolds.reserve(nbMaxContactManifolds);
-		//this.joints.reserve(_nbMaxJoints);
+		//this.joints.reserve(nbMaxJoints);
 	}
 	
 	/** 
 		 * Add a body.
 		 */
-	public void addBody(final RigidBody _body) {
+	public void addBody(final RigidBody body) {
-		if (_body.isSleeping() == true) {
+		if (body.isSleeping()) {
 			Log.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) {
+	public void addContactManifold(final ContactManifold contactManifold) {
-		this.contactManifolds.add(_contactManifold);
+		this.contactManifolds.add(contactManifold);
 	}
 	
 	/** 
 		 * Add a joint.
 		 */
-	public void addJoint(final Joint _joint) {
+	public void addJoint(final Joint joint) {
-		this.joints.add(_joint);
+		this.joints.add(joint);
 	}
 	
 	/** 

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);

src/org/atriasoft/ephysics/mathematics/Mathematics.java

@@ -26,34 +26,29 @@ package org.atriasoft.ephysics.mathematics;
 
 import org.atriasoft.etk.math.Vector3f;
 
-/**
- *
- * @author Jason Sorensen <sorensenj@smert.net>
- */
 public class Mathematics {
-	
 	// Function to test if two real numbers are (almost) equal
 	// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
-	public static boolean ApproxEqual(final float a, final float b, final float epsilon) {
+	public static boolean approxEqual(final float a, final float b, final float epsilon) {
 		final float difference = a - b;
 		return (difference < epsilon && difference > -epsilon);
 	}
 	
-	public static float ArcCos(final float radians) {
+	public static float arcCos(final float radians) {
 		return (float) StrictMath.acos(radians);
 	}
 	
-	public static float ArcSin(final float radians) {
+	public static float arcSin(final float radians) {
 		return (float) StrictMath.asin(radians);
 	}
 	
-	public static float ArcTan2(final float a, final float b) {
+	public static float arcTan2(final float a, final float b) {
 		return (float) StrictMath.atan2(a, b);
 	}
 	
 	// Function that returns the result of the "value" clamped by
 	// two others values "lowerLimit" and "upperLimit"
-	public static float Clamp(final float value, final float lowerLimit, final float upperLimit) {
+	public static float clamp(final float value, final float lowerLimit, final float upperLimit) {
 		assert (lowerLimit <= upperLimit);
 		return Math.min(Math.max(value, lowerLimit), upperLimit);
 	}
@@ -62,9 +57,9 @@ public class Mathematics {
 	/// This method uses the technique described in the book Real-Time collision detection by
 	/// Christer Ericson.
 	public static void computeBarycentricCoordinatesInTriangle(final Vector3f a, final Vector3f b, final Vector3f c, final Vector3f p, Float u, Float v, Float w) {
-		final Vector3f v0 = b.lessNew(a);
+		final Vector3f v0 = b.less(a);
-		final Vector3f v1 = c.lessNew(a);
+		final Vector3f v1 = c.less(a);
-		final Vector3f v2 = p.lessNew(a);
+		final Vector3f v2 = p.less(a);
 		
 		final float d00 = v0.dot(v0);
 		final float d01 = v0.dot(v1);
@@ -78,19 +73,21 @@ public class Mathematics {
 		u = 1.0f - v - w;
 	}
 	
-	public static float Cos(final float radians) {
+	public static float cos(final float radians) {
 		return (float) StrictMath.cos(radians);
 	}
 	
-	public static float Sin(final float radians) {
+	public static float sin(final float radians) {
 		return (float) StrictMath.sin(radians);
 	}
 	
-	public static float Sqrt(final float a) {
+	public static float sqrt(final float a) {
 		return (float) StrictMath.sqrt(a);
 	}
 	
-	public static float Tan(final float radians) {
+	public static float tan(final float radians) {
 		return (float) StrictMath.tan(radians);
 	}
+	
+	private Mathematics() {}
 }

src/org/atriasoft/ephysics/mathematics/Matrix2f.java

@@ -27,11 +27,6 @@ package org.atriasoft.ephysics.mathematics;
 import org.atriasoft.etk.math.Constant;
 import org.atriasoft.etk.math.Vector2f;
 
-/**
- * This class represents a 2x2 matrix.
- *
- * @author Jason Sorensen <sorensenj@smert.net>
- */
 public class Matrix2f {
 	
 	// Rows of the matrix (m[row][column])
@@ -104,7 +99,8 @@ public class Matrix2f {
 	public Vector2f getColumn(final int index) {
 		if (index == 0) {
 			return new Vector2f(this.m00, this.m10);
-		} else if (index == 1) {
+		}
+		if (index == 1) {
 			return new Vector2f(this.m01, this.m11);
 		}
 		throw new IllegalArgumentException("Unknown column index: " + index);
@@ -119,7 +115,8 @@ public class Matrix2f {
 	public Vector2f getRow(final int index) {
 		if (index == 0) {
 			return new Vector2f(this.m00, this.m01);
-		} else if (index == 1) {
+		}
+		if (index == 1) {
 			return new Vector2f(this.m10, this.m11);
 		}
 		throw new IllegalArgumentException("Unknown row index: " + index);
@@ -206,7 +203,7 @@ public class Matrix2f {
 	
 	// Overloaded operator for multiplication with a vector
 	public Vector2f multiplyNew(final Vector2f vector) {
-		return new Vector2f(this.m00 * vector.x + this.m01 * vector.y, this.m10 * vector.x + this.m11 * vector.y);
+		return new Vector2f(this.m00 * vector.x() + this.m01 * vector.y(), this.m10 * vector.x() + this.m11 * vector.y());
 	}
 	
 	// Method to set all the values in the matrix

src/org/atriasoft/ephysics/mathematics/PairInt.java

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

src/org/atriasoft/ephysics/mathematics/Ray.java

@@ -3,27 +3,27 @@ package org.atriasoft.ephysics.mathematics;
 import org.atriasoft.etk.math.Vector3f;
 
 public class Ray {
+	public float maxFraction; //!< Maximum fraction value
 	public Vector3f point1; //!<First point of the ray (origin)
 	public Vector3f point2; //!< Second point of the ray
-	public float maxFraction; //!< Maximum fraction value
 	
 	public Ray(final Ray obj) {
-		this.point1 = new Vector3f(obj.point1);
+		this.point1 = obj.point1;
-		this.point2 = new Vector3f(obj.point2);
+		this.point2 = obj.point2;
 		this.maxFraction = obj.maxFraction;
 	}
 	
 	/// Constructor with arguments
-	public Ray(final Vector3f _p1, final Vector3f _p2) {
+	public Ray(final Vector3f p1, final Vector3f p2) {
-		this.point1 = _p1;
+		this.point1 = p1;
-		this.point2 = _p2;
+		this.point2 = p2;
 		this.maxFraction = 1.0f;
 	}
 	
-	public Ray(final Vector3f _p1, final Vector3f _p2, final float _maxFrac) {
+	public Ray(final Vector3f p1, final Vector3f p2, final float maxFrac) {
-		this.point1 = _p1;
+		this.point1 = p1;
-		this.point2 = _p2;
+		this.point2 = p2;
-		this.maxFraction = _maxFrac;
+		this.maxFraction = maxFrac;
 	}
 	
 	@Override
@@ -45,8 +45,9 @@ public class Ray {
 	}
 	
 	@Override
-	public String toString() {
+	public int hashCode() {
-		return super.toString();
+		// TODO Auto-generated method stub
+		return super.hashCode();
 	}
 	
 }

src/org/atriasoft/ephysics/mathematics/Set.java

@@ -10,14 +10,15 @@ import org.atriasoft.ephysics.body.CollisionBody;
 import org.atriasoft.ephysics.collision.broadphase.DTree;
 import org.atriasoft.ephysics.collision.broadphase.PairDTree;
 
-public class Set<TYPE> implements Iterable<TYPE> {
+public class Set<T> implements Iterable<T> {
 	public static Comparator<CollisionBody> getCollisionBodyCoparator() {
 		return new Comparator<CollisionBody>() {
 			@Override
 			public int compare(final CollisionBody a, final CollisionBody b) {
 				if (a.getID() < b.getID()) {
 					return -1;
-				} else if (a.getID() == b.getID()) {
+				}
+				if (a.getID() == b.getID()) {
 					return 0;
 				}
 				return -1;
@@ -31,7 +32,8 @@ public class Set<TYPE> implements Iterable<TYPE> {
 			public int compare(final DTree a, final DTree b) {
 				if (a.uid < b.uid) {
 					return -1;
-				} else if (a.uid == b.uid) {
+				}
+				if (a.uid == b.uid) {
 					return 0;
 				}
 				return -1;
@@ -45,7 +47,8 @@ public class Set<TYPE> implements Iterable<TYPE> {
 			public int compare(final Integer a, final Integer b) {
 				if (a < b) {
 					return -1;
-				} else if (a == b) {
+				}
+				if (a.equals(b)) {
 					return 0;
 				}
 				return -1;
@@ -56,15 +59,15 @@ public class Set<TYPE> implements Iterable<TYPE> {
 	public static Comparator<PairDTree> getPairDTreeCoparator() {
 		return new Comparator<PairDTree>() {
 			@Override
-			public int compare(final PairDTree _pair1, final PairDTree _pair2) {
+			public int compare(final PairDTree pair1, final PairDTree pair2) {
-				if (_pair1.first.uid < _pair2.first.uid) {
+				if (pair1.first().uid < pair2.first().uid) {
 					return -1;
 				}
-				if (_pair1.first.uid == _pair2.first.uid) {
+				if (pair1.first().uid == pair2.first().uid) {
-					if (_pair1.second.uid < _pair2.second.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;
@@ -77,15 +80,15 @@ public class Set<TYPE> implements Iterable<TYPE> {
 	public static Comparator<PairInt> getPairIntCoparator() {
 		return new Comparator<PairInt>() {
 			@Override
-			public int compare(final PairInt _pair1, final PairInt _pair2) {
+			public int compare(final PairInt pair1, final PairInt pair2) {
-				if (_pair1.first < _pair2.first) {
+				if (pair1.first() < pair2.first()) {
 					return -1;
 				}
-				if (_pair1.first == _pair2.first) {
+				if (pair1.first() == pair2.first()) {
-					if (_pair1.second < _pair2.second) {
+					if (pair1.second() < pair2.second()) {
 						return -1;
 					}
-					if (_pair1.second == _pair2.second) {
+					if (pair1.second() == pair2.second()) {
 						return 0;
 					}
 					return +1;
@@ -95,22 +98,22 @@ public class Set<TYPE> implements Iterable<TYPE> {
 		};
 	}
 	
-	protected final List<TYPE> data = new ArrayList<>(); //!< Data of the Set ==> the Set table is composed of pointer, this permit to have high speed when resize the vector ...
+	protected Comparator<T> comparator = null;
 	
-	protected Comparator<TYPE> comparator = null;
+	protected final List<T> data = new ArrayList<>(); //!< Data of the Set ==> the Set table is composed of pointer, this permit to have high speed when resize the vector ...
 	
 	/*
 		new Comparator<TYPE>() {
 		@Override
-		public int compare(final TYPE _pair1, final TYPE _pair2) {
+		public int compare(final TYPE pair1, final TYPE pair2) {
-			if (_pair1.first < _pair2.first) {
+			if (pair1.first < pair2.first) {
 				return -1;
 			}
-			if (_pair1.first == _pair2.first) {
+			if (pair1.first == pair2.first) {
-				if (_pair1.second < _pair2.second) {
+				if (pair1.second < pair2.second) {
 					return -1;
 				}
-				if (_pair1.second == _pair2.second) {
+				if (pair1.second == pair2.second) {
 					return 0;
 				}
 				return +1;
@@ -120,35 +123,35 @@ public class Set<TYPE> implements Iterable<TYPE> {
 	};
 	*/
 	/**
-	 * @brief Constructor of the Set table.
+	 * Constructor of the Set table.
 	 */
-	public Set(final Comparator<TYPE> comparator) {
+	public Set(final Comparator<T> comparator) {
 		this.comparator = comparator;
 	}
 	
 	/**
-	 * @brief Add an element OR set an element value
+	 * Add an element OR set an element value
 	 * @note add and set is the same function.
-	 * @param[in] _key Name of the value to set in the Set table.
+	 * @param key Name of the value to set in the Set table.
-	 * @param[in] _value Value to set in the Set table.
+	 * @param value Value to set in the Set table.
 	 */
-	public void add(final TYPE _key) {
+	public void add(final T key) {
 		for (int iii = 0; iii < this.data.size(); ++iii) {
-			if (_key == this.data.get(iii)) {
+			if (key == this.data.get(iii)) {
 				return;
 			}
 			// For multiple element:
-			if (this.comparator.compare(_key, this.data.get(iii)) == 0) {
+			if (this.comparator.compare(key, this.data.get(iii)) == 0) {
 				// Find a position
-				this.data.add(iii, _key);
+				this.data.add(iii, key);
 				return;
 			}
 		}
-		this.data.add(_key);
+		this.data.add(key);
 	}
 	
 	/**
-	 * @brief Remove all entry in the Set table.
+	 * Remove all entry in the Set table.
 	 * @note It does not delete pointer if your value is a pointer type...
 	 */
 	public void clear() {
@@ -156,15 +159,15 @@ public class Set<TYPE> implements Iterable<TYPE> {
 	}
 	
 	/**
-	 * @brief Count the number of occurence of a specific element.
+	 * Count the number of occurence of a specific element.
-	 * @param[in] _key Name of the element to count iterence
+	 * @param key Name of the element to count iterence
 	 * @return 0 No element was found
 	 * @return 1 One element was found
 	 */
-	public int count(final TYPE _key) {
+	public int count(final T key) {
-		// TODO: search in a dichotomic way.
+		// TODO  search in a dichotomic way.
 		for (int iii = 0; iii < this.data.size(); iii++) {
-			if (this.comparator.compare(this.data.get(iii), _key) == 0) {
+			if (this.comparator.compare(this.data.get(iii), key) == 0) {
 				return 1;
 			}
 		}
@@ -175,17 +178,17 @@ public class Set<TYPE> implements Iterable<TYPE> {
 		this.data.remove(id);
 	}
 	
-	public void erase(final TYPE id) {
+	public void erase(final T id) {
 		this.data.remove(id);
 	}
 	
 	/**
-	 * @brief Check if an element exist or not
+	 * Check if an element exist or not
-	 * @param[in] _key Name of the Set requested
+	 * @param key Name of the Set requested
 	 * @return true if the element exist
 	 */
-	public boolean exist(final TYPE _key) {
+	public boolean exist(final T key) {
-		final int it = find(_key);
+		final int it = find(key);
 		if (it == -1) {
 			return false;
 		}
@@ -193,14 +196,14 @@ public class Set<TYPE> implements Iterable<TYPE> {
 	}
 	
 	/**
-	 * @brief Find an element position the the Set table
+	 * Find an element position the the Set table
-	 * @param[in] _key Name of the element to find
+	 * @param key Name of the element to find
 	 * @return Iterator on the element find or end()
 	 */
-	public int find(final TYPE _key) {
+	public int find(final T key) {
-		// TODO: search in a dichotomic way.
+		// TODO  search in a dichotomic way.
 		for (int iii = 0; iii < this.data.size(); iii++) {
-			if (this.comparator.compare(this.data.get(iii), _key) == 0) {
+			if (this.comparator.compare(this.data.get(iii), key) == 0) {
 				return iii;
 			}
 		}
@@ -208,16 +211,16 @@ public class Set<TYPE> implements Iterable<TYPE> {
 	}
 	
 	/**
-	 * @brief Get Element an a special position
+	 * Get Element an a special position
-	 * @param[in] _position Position in the Set
+	 * @param position Position in the Set
 	 * @return An reference on the selected element
 	 */
-	public TYPE get(final int _position) {
+	public T get(final int position) {
-		return this.data.get(_position);
+		return this.data.get(position);
 	}
 	
 	/**
-	 * @brief Check if the container have some element
+	 * Check if the container have some element
 	 * @return true The container is empty
 	 * @return false The container have some element
 	 */
@@ -231,85 +234,85 @@ public class Set<TYPE> implements Iterable<TYPE> {
 	}
 	
 	/**
-	 * @brief Remove the last element of the vector
+	 * Remove the last element of the vector
 	 */
 	public void popBack() {
 		this.data.remove(this.data.size() - 1);
 	}
 	
 	/**
-	 * @brief Remove the first element of the vector
+	 * Remove the first element of the vector
 	 */
 	public void popFront() {
 		this.data.remove(0);
 	}
 	
 	/**
-	 * @brief: QuickSort implementation of sorting vector (all elements.
+	 * QuickSort implementation of sorting vector (all elements.
-	 * @param[in,out] _data Vector to sort.
+	 * @param data Vector to sort.
-	 * @param[in] _high Comparator function of this element.
+	 * @param high Comparator function of this element.
 	 */
-	void quickSort(final List<TYPE> _data, final Comparator<TYPE> _comparator) {
+	void quickSort(final List<T> data, final Comparator<T> comparator) {
-		quickSort2(_data, 0, _data.size() - 1, _comparator);
+		quickSort2(data, 0, data.size() - 1, comparator);
 	}
 	
 	/**
-	 * @brief: QuickSort implementation of sorting vector.
+	 * QuickSort implementation of sorting vector.
-	 * @param[in,out] _data Vector to sort.
+	 * @param data Vector to sort.
-	 * @param[in] _low Lowest element to sort.
+	 * @param low Lowest element to sort.
-	 * @param[in] _high Highest element to sort.
+	 * @param high Highest element to sort.
-	 * @param[in] _high Comparator function of this element.
+	 * @param high Comparator function of this element.
 	 */
-	void quickSort(final List<TYPE> _data, final int _low, int _high, final Comparator<TYPE> _comparator) {
+	void quickSort(final List<T> data, final int low, int high, final Comparator<T> comparator) {
-		if (_high >= _data.size()) {
+		if (high >= data.size()) {
-			_high = _data.size() - 1;
+			high = data.size() - 1;
 		}
-		/*if (_low >= _data.size()) {
+		/*if (low >= data.size()) {
-			_low = _data.size()-1;
+			low = data.size()-1;
 		}*/
-		quickSort2(_data, _low, _high, _comparator);
+		quickSort2(data, low, high, comparator);
 	}
 	
-	private void quickSort2(final List<TYPE> _data, final int _low, final int _high, final Comparator<TYPE> _comparator) {
+	private void quickSort2(final List<T> data, final int low, final int high, final Comparator<T> comparator) {
-		if (_low >= _high) {
+		if (low >= high) {
 			return;
 		}
 		// pi is partitioning index, arr[p] is now at right place
-		final int pi = quickSortPartition(_data, _low, _high, _comparator);
+		final int pi = quickSortPartition(data, low, high, comparator);
 		// Separately sort elements before partition and after partition
 		//if (pi != 0) {
-		quickSort2(_data, _low, pi - 1, _comparator);
+		quickSort2(data, low, pi - 1, comparator);
 		//}
-		quickSort2(_data, pi + 1, _high, _comparator);
+		quickSort2(data, pi + 1, high, comparator);
 	}
 	
-	private int quickSortPartition(final List<TYPE> _data, final int _low, final int _high, final Comparator<TYPE> _comparator) {
+	private int quickSortPartition(final List<T> data, final int low, final int high, final Comparator<T> comparator) {
-		int iii = (_low - 1);
+		int iii = (low - 1);
-		for (int jjj = _low; jjj < _high; ++jjj) {
+		for (int jjj = low; jjj < high; ++jjj) {
-			if (_comparator.compare(_data.get(jjj), _data.get(_high)) < 0) {
+			if (comparator.compare(data.get(jjj), data.get(high)) < 0) {
 				iii++;
-				Collections.swap(_data, iii, jjj);
+				Collections.swap(data, iii, jjj);
 			}
 		}
-		Collections.swap(_data, iii + 1, _high);
+		Collections.swap(data, iii + 1, high);
 		return (iii + 1);
 	}
 	
-	public void remove(final TYPE obj) {
+	public void remove(final T obj) {
 		this.data.remove(obj);
 	}
 	
 	/**
-	 * @brief Set the comparator of the set.
+	 * Set the comparator of the set.
-	 * @param[in] _comparator comparing function.
+	 * @param comparator comparing function.
 	 */
-	public void setComparator(final Comparator<TYPE> _comparator) {
+	public void setComparator(final Comparator<T> comparator) {
-		this.comparator = _comparator;
+		this.comparator = comparator;
 		sort();
 	}
 	
 	/**
-		 * @brief Get the number of element in the Set table
+		 * Get the number of element in the Set table
 		 * @return number of elements
 		 */
 	public int size() {
@@ -317,7 +320,7 @@ public class Set<TYPE> implements Iterable<TYPE> {
 	}
 	
 	/**
-	 * @brief Order the Set with the corect functor
+	 * Order the Set with the corect functor
 	 */
 	private void sort() {
 		quickSort(this.data, this.comparator);

src/org/atriasoft/ephysics/mathematics/SetInteger.java

@@ -6,74 +6,74 @@ import java.util.Comparator;
 import java.util.List;
 
 public class SetInteger {
-	private static void quickSort2(final List<Integer> _data, final int _low, final int _high, final Comparator<Integer> _comparator) {
+	private static void quickSort2(final List<Integer> data, final int low, final int high, final Comparator<Integer> comparator) {
-		if (_low >= _high) {
+		if (low >= high) {
 			return;
 		}
 		// pi is partitioning index, arr[p] is now at right place
-		final int pi = quickSortPartition(_data, _low, _high, _comparator);
+		final int pi = SetInteger.quickSortPartition(data, low, high, comparator);
 		// Separately sort elements before partition and after partition
 		//if (pi != 0) {
-		quickSort2(_data, _low, pi - 1, _comparator);
+		SetInteger.quickSort2(data, low, pi - 1, comparator);
 		//}
-		quickSort2(_data, pi + 1, _high, _comparator);
+		SetInteger.quickSort2(data, pi + 1, high, comparator);
 	}
 	
-	private static int quickSortPartition(final List<Integer> _data, final int _low, final int _high, final Comparator<Integer> _comparator) {
+	private static int quickSortPartition(final List<Integer> data, final int low, final int high, final Comparator<Integer> comparator) {
-		int iii = (_low - 1);
+		int iii = (low - 1);
-		for (int jjj = _low; jjj < _high; ++jjj) {
+		for (int jjj = low; jjj < high; ++jjj) {
-			if (_comparator.compare(_data.get(jjj), _data.get(_high)) < 0) {
+			if (comparator.compare(data.get(jjj), data.get(high)) < 0) {
 				iii++;
-				Collections.swap(_data, iii, jjj);
+				Collections.swap(data, iii, jjj);
 			}
 		}
-		Collections.swap(_data, iii + 1, _high);
+		Collections.swap(data, iii + 1, high);
 		return (iii + 1);
 	}
 	
-	private final List<Integer> data = new ArrayList<>(); //!< Data of the Set ==> the Set table is composed of pointer, this permit to have high speed when resize the vector ...
-	
 	private Comparator<Integer> comparator = new Comparator<Integer>() {
 		@Override
 		public int compare(final Integer a, final Integer b) {
 			if (a < b) {
 				return -1;
-			} else if (a == b) {
+			}
+			if (a.equals(b)) {
 				return 0;
 			}
 			return -1;
 		}
 	};
 	
+	private final List<Integer> data = new ArrayList<>(); //!< Data of the Set ==> the Set table is composed of pointer, this permit to have high speed when resize the vector ...
+	
 	/**
-	 * @brief Constructor of the Set table.
+	 * Constructor of the Set table.
 	 */
 	public SetInteger() {
 		
 	}
 	
 	/**
-	 * @brief Add an element OR set an element value
+	 * Add an element OR set an element value
 	 * @note add and set is the same function.
-	 * @param[in] _key Name of the value to set in the Set table.
+	 * @param key Name of the value to set in the Set table.
-	 * @param[in] _value Value to set in the Set table.
 	 */
-	public void add(final Integer _key) {
+	public void add(final Integer key) {
 		for (int iii = 0; iii < this.data.size(); ++iii) {
-			if (_key == this.data.get(iii)) {
+			if (key.equals(this.data.get(iii))) {
 				return;
 			}
-			if (this.comparator.compare(_key, this.data.get(iii)) == 0) {
+			if (this.comparator.compare(key, this.data.get(iii)) == 0) {
 				// Find a position
-				this.data.add(iii, _key);
+				this.data.add(iii, key);
 				return;
 			}
 		}
-		this.data.add(_key);
+		this.data.add(key);
 	}
 	
 	/**
-	 * @brief Remove all entry in the Set table.
+	 * Remove all entry in the Set table.
 	 * @note It does not delete pointer if your value is a pointer type...
 	 */
 	public void clear() {
@@ -81,15 +81,15 @@ public class SetInteger {
 	}
 	
 	/**
-	 * @brief Count the number of occurence of a specific element.
+	 * Count the number of occurence of a specific element.
-	 * @param[in] _key Name of the element to count iterence
+	 * @param key Name of the element to count iterence
 	 * @return 0 No element was found
 	 * @return 1 One element was found
 	 */
-	public int count(final Integer _key) {
+	public int count(final Integer key) {
-		// TODO: search in a dichotomic way.
+		// TODO search in a dichotomic way.
 		for (int iii = 0; iii < this.data.size(); iii++) {
-			if (this.comparator.compare(this.data.get(iii), _key) == 0) {
+			if (this.comparator.compare(this.data.get(iii), key) == 0) {
 				return 1;
 			}
 		}
@@ -97,12 +97,12 @@ public class SetInteger {
 	}
 	
 	/**
-	 * @brief Check if an element exist or not
+	 * Check if an element exist or not
-	 * @param[in] _key Name of the Set requested
+	 * @param key Name of the Set requested
 	 * @return true if the element exist
 	 */
-	public boolean exist(final Integer _key) {
+	public boolean exist(final Integer key) {
-		final int it = find(_key);
+		final int it = find(key);
 		if (it == -1) {
 			return false;
 		}
@@ -110,14 +110,14 @@ public class SetInteger {
 	}
 	
 	/**
-	 * @brief Find an element position the the Set table
+	 * Find an element position the the Set table
-	 * @param[in] _key Name of the element to find
+	 * @param key Name of the element to find
 	 * @return Iterator on the element find or end()
 	 */
-	public int find(final Integer _key) {
+	public int find(final Integer key) {
-		// TODO: search in a dichotomic way.
+		// TODO search in a dichotomic way.
 		for (int iii = 0; iii < this.data.size(); iii++) {
-			if (this.comparator.compare(this.data.get(iii), _key) == 0) {
+			if (this.comparator.compare(this.data.get(iii), key) == 0) {
 				return iii;
 			}
 		}
@@ -125,12 +125,12 @@ public class SetInteger {
 	}
 	
 	/**
-	 * @brief Get Element an a special position
+	 * Get Element an a special position
-	 * @param[in] _position Position in the Set
+	 * @param position Position in the Set
 	 * @return An reference on the selected element
 	 */
-	public Integer get(final int _position) {
+	public Integer get(final int position) {
-		return this.data.get(_position);
+		return this.data.get(position);
 	}
 	
 	public List<Integer> getRaw() {
@@ -138,7 +138,7 @@ public class SetInteger {
 	}
 	
 	/**
-	 * @brief Check if the container have some element
+	 * Check if the container have some element
 	 * @return true The container is empty
 	 * @return false The container have some element
 	 */
@@ -147,14 +147,14 @@ public class SetInteger {
 	}
 	
 	/**
-	 * @brief Remove the last element of the vector
+	 * Remove the last element of the vector
 	 */
 	public void popBack() {
 		this.data.remove(this.data.size() - 1);
 	}
 	
 	/**
-	 * @brief Remove the first element of the vector
+	 * Remove the first element of the vector
 	 */
 	public void popFront() {
 		this.data.remove(0);
@@ -162,41 +162,41 @@ public class SetInteger {
 	
 	/**
 	 * @brief: QuickSort implementation of sorting vector (all elements.
-	 * @param[in,out] _data Vector to sort.
+	 * @param data Vector to sort.
-	 * @param[in] _high Comparator function of this element.
+	 * @param high Comparator function of this element.
 	 */
-	void quickSort(final List<Integer> _data, final Comparator<Integer> _comparator) {
+	void quickSort(final List<Integer> data, final Comparator<Integer> comparator) {
-		quickSort2(_data, 0, _data.size() - 1, _comparator);
+		SetInteger.quickSort2(data, 0, data.size() - 1, comparator);
 	}
 	
 	/**
 	 * @brief: QuickSort implementation of sorting vector.
-	 * @param[in,out] _data Vector to sort.
+	 * @param data Vector to sort.
-	 * @param[in] _low Lowest element to sort.
+	 * @param low Lowest element to sort.
-	 * @param[in] _high Highest element to sort.
+	 * @param high Highest element to sort.
-	 * @param[in] _high Comparator function of this element.
+	 * @param high Comparator function of this element.
 	 */
-	void quickSort(final List<Integer> _data, final int _low, int _high, final Comparator<Integer> _comparator) {
+	void quickSort(final List<Integer> data, final int low, int high, final Comparator<Integer> comparator) {
-		if (_high >= _data.size()) {
+		if (high >= data.size()) {
-			_high = _data.size() - 1;
+			high = data.size() - 1;
 		}
-		/*if (_low >= _data.size()) {
+		/*if (low >= data.size()) {
-			_low = _data.size()-1;
+			low = data.size()-1;
 		}*/
-		quickSort2(_data, _low, _high, _comparator);
+		SetInteger.quickSort2(data, low, high, comparator);
 	}
 	
 	/**
-	 * @brief Set the comparator of the set.
+	 * Set the comparator of the set.
-	 * @param[in] _comparator comparing function.
+	 * @param comparator comparing function.
 	 */
-	public void setComparator(final Comparator<Integer> _comparator) {
+	public void setComparator(final Comparator<Integer> comparator) {
-		this.comparator = _comparator;
+		this.comparator = comparator;
 		sort();
 	}
 	
 	/**
-		 * @brief Get the number of element in the Set table
+		 * Get the number of element in the Set table
 		 * @return number of elements
 		 */
 	public int size() {
@@ -204,7 +204,7 @@ public class SetInteger {
 	}
 	
 	/**
-	 * @brief Order the Set with the corect functor
+	 * Order the Set with the corect functor
 	 */
 	private void sort() {
 		quickSort(this.data, this.comparator);

src/org/atriasoft/ephysics/mathematics/SetMultiple.java

@@ -2,40 +2,40 @@ package org.atriasoft.ephysics.mathematics;
 
 import java.util.Comparator;
 
-public class SetMultiple<TYPE> extends Set<TYPE> {
+public class SetMultiple<T> extends Set<T> {
 	/**
-	 * @brief Constructor of the Set table.
+	 * Constructor of the Set table.
 	 */
-	public SetMultiple(final Comparator<TYPE> comparator) {
+	public SetMultiple(final Comparator<T> comparator) {
 		super(comparator);
 	}
 	
 	/**
-	 * @brief Add an element OR set an element value
+	 * Add an element OR set an element value
 	 * @note add and set is the same function.
-	 * @param[in] _key Name of the value to set in the Set table.
+	 * @param key Name of the value to set in the Set table.
-	 * @param[in] _value Value to set in the Set table.
+	 * @param value Value to set in the Set table.
 	 */
 	@Override
-	public void add(final TYPE _key) {
+	public void add(final T key) {
 		for (int iii = 0; iii < this.data.size(); ++iii) {
-			if (_key == this.data.get(iii)) {
+			if (key == this.data.get(iii)) {
 				return;
 			}
-			final int compareValue = this.comparator.compare(_key, this.data.get(iii));
+			final int compareValue = this.comparator.compare(key, this.data.get(iii));
 			if (compareValue == 0) {
 				// Find a position
-				this.data.set(iii, _key);
+				this.data.set(iii, key);
 				return;
 			}
 			// for single Element
 			if (compareValue == 1) {
 				// Find a position
-				this.data.add(iii, _key);
+				this.data.add(iii, key);
 				return;
 			}
 		}
-		this.data.add(_key);
+		this.data.add(key);
 	}
 	
 }