Initial jbullet commit - workin ragdoll for profiles (GLES1, GL2ES12 and GL2)

git-svn-id: file:///usr/local/projects/SUN/JOGL/git-svn/../svn-server-sync/jogl-demos/branches/JOGL_2_SANDBOX@263 3298f667-5e0e-4b4a-8ed4-a3559d26a5f4

56 files changed, 10610 insertions, 0 deletions

diff --git a/src/jbullet/src/javabullet/demos/vehicle/VehicleDemo.java-nope b/src/jbullet/src/javabullet/demos/vehicle/VehicleDemo.java-nope
new file mode 100644
index 0000000..7c9efbd
--- /dev/null
+++ b/src/jbullet/src/javabullet/demos/vehicle/VehicleDemo.java-nope
@@ -0,0 +1,576 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.demos.vehicle;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.util.ArrayList;
+import java.util.List;
+import javabullet.collision.broadphase.BroadphaseInterface;
+import javabullet.collision.broadphase.SimpleBroadphase;
+import javabullet.collision.dispatch.CollisionDispatcher;
+import javabullet.collision.dispatch.CollisionObject;
+import javabullet.collision.dispatch.DefaultCollisionConfiguration;
+import javabullet.collision.shapes.BoxShape;
+import javabullet.collision.shapes.BvhTriangleMeshShape;
+import javabullet.collision.shapes.CollisionShape;
+import javabullet.collision.shapes.CompoundShape;
+import javabullet.collision.shapes.CylinderShapeX;
+import javabullet.collision.shapes.TriangleIndexVertexArray;
+import javabullet.demos.opengl.JOGL;
+import javabullet.demos.opengl.DemoApplication;
+import javabullet.demos.opengl.GLShapeDrawer;
+import javabullet.dynamics.DiscreteDynamicsWorld;
+import javabullet.dynamics.RigidBody;
+import javabullet.dynamics.constraintsolver.ConstraintSolver;
+import javabullet.dynamics.constraintsolver.SequentialImpulseConstraintSolver;
+import javabullet.dynamics.vehicle.DefaultVehicleRaycaster;
+import javabullet.dynamics.vehicle.RaycastVehicle;
+import javabullet.dynamics.vehicle.VehicleRaycaster;
+import javabullet.dynamics.vehicle.VehicleTuning;
+import javabullet.dynamics.vehicle.WheelInfo;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+import javax.media.opengl.*;
+import java.awt.event.*;
+
+/**
+ * VehicleDemo shows how to setup and use the built-in raycast vehicle.
+ * 
+ * @author jezek2
+ */
+public class VehicleDemo extends DemoApplication {
+
+	// By default, Bullet Vehicle uses Y as up axis.
+	// You can override the up axis, for example Z-axis up. Enable this define to see how to:
+	// //#define FORCE_ZAXIS_UP 1
+	
+	//#ifdef FORCE_ZAXIS_UP
+	//int rightIndex = 0; 
+	//int upIndex = 2; 
+	//int forwardIndex = 1;
+	//btVector3 wheelDirectionCS0(0,0,-1);
+	//btVector3 wheelAxleCS(1,0,0);
+	//#else
+	private static final int rightIndex = 0;
+	private static final int upIndex = 1;
+	private static final int forwardIndex = 2;
+	private static final Vector3f wheelDirectionCS0 = new Vector3f(0,-1,0);
+	private static final Vector3f wheelAxleCS = new Vector3f(-1,0,0);
+	//#endif
+	
+	private static final int maxProxies = 32766;
+	private static final int maxOverlap = 65535;
+
+	// RaycastVehicle is the interface for the constraint that implements the raycast vehicle
+	// notice that for higher-quality slow-moving vehicles, another approach might be better
+	// implementing explicit hinged-wheel constraints with cylinder collision, rather then raycasts
+	private static float gEngineForce = 0.f;
+	private static float gBreakingForce = 0.f;
+
+	private static float maxEngineForce = 1000.f;//this should be engine/velocity dependent
+	private static float maxBreakingForce = 100.f;
+
+	private static float gVehicleSteering = 0.f;
+	private static float steeringIncrement = 0.04f;
+	private static float steeringClamp = 0.3f;
+	private static float wheelRadius = 0.5f;
+	private static float wheelWidth = 0.4f;
+	private static float wheelFriction = 1000;//1e30f;
+	private static float suspensionStiffness = 20.f;
+	private static float suspensionDamping = 2.3f;
+	private static float suspensionCompression = 4.4f;
+	private static float rollInfluence = 0.1f;//1.0f;
+
+	private static final float suspensionRestLength = 0.6f;
+
+	private static final int CUBE_HALF_EXTENTS = 1;
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	public RigidBody carChassis;
+	public List<CollisionShape> collisionShapes = new ArrayList<CollisionShape>();
+	public BroadphaseInterface overlappingPairCache;
+	public CollisionDispatcher dispatcher;
+	public ConstraintSolver constraintSolver;
+	public DefaultCollisionConfiguration collisionConfiguration;
+	public TriangleIndexVertexArray indexVertexArrays;
+
+	public ByteBuffer vertices;
+
+	public VehicleTuning tuning = new VehicleTuning();
+	public VehicleRaycaster vehicleRayCaster;
+	public RaycastVehicle vehicle;
+
+	public float cameraHeight;
+
+	public float minCameraDistance;
+	public float maxCameraDistance;
+
+	public VehicleDemo() {
+		super();
+		carChassis = null;
+		cameraHeight = 4f;
+		minCameraDistance = 3f;
+		maxCameraDistance = 10f;
+		indexVertexArrays = null;
+		vertices = null;
+		vehicle = null;
+		cameraPosition.set(30, 30, 30);
+	}
+
+	public void initPhysics() {
+		//#ifdef FORCE_ZAXIS_UP
+		//m_cameraUp = btVector3(0,0,1);
+		//m_forwardAxis = 1;
+		//#endif
+
+		CollisionShape groundShape = new BoxShape(new Vector3f(50, 3, 50));
+		collisionShapes.add(groundShape);
+		collisionConfiguration = new DefaultCollisionConfiguration();
+		dispatcher = new CollisionDispatcher(collisionConfiguration);
+		Vector3f worldMin = new Vector3f(-1000, -1000, -1000);
+		Vector3f worldMax = new Vector3f(1000, 1000, 1000);
+		//overlappingPairCache = new AxisSweep3(worldMin,worldMax);
+		overlappingPairCache = new SimpleBroadphase();
+		constraintSolver = new SequentialImpulseConstraintSolver();
+		dynamicsWorld = new DiscreteDynamicsWorld(dispatcher, overlappingPairCache, constraintSolver, collisionConfiguration);
+		//#ifdef FORCE_ZAXIS_UP
+		//dynamicsWorld.setGravity(new Vector3f(0, 0, -10));
+		//#endif 
+
+		//m_dynamicsWorld->setGravity(btVector3(0,0,0));
+		Transform tr = new Transform();
+		tr.setIdentity();
+
+		// either use heightfield or triangle mesh
+		//#define  USE_TRIMESH_GROUND 1
+		//#ifdef USE_TRIMESH_GROUND
+
+		final float TRIANGLE_SIZE = 20f;
+
+		// create a triangle-mesh ground
+		int vertStride = 4 * 3 /* sizeof(btVector3) */;
+		int indexStride = 3 * 4 /* 3*sizeof(int) */;
+
+		final int NUM_VERTS_X = 20;
+		final int NUM_VERTS_Y = 20;
+		final int totalVerts = NUM_VERTS_X * NUM_VERTS_Y;
+
+		final int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
+
+		vertices = ByteBuffer.allocateDirect(totalVerts * vertStride).order(ByteOrder.nativeOrder());
+		ByteBuffer gIndices = ByteBuffer.allocateDirect(totalTriangles * 3 * 4).order(ByteOrder.nativeOrder());
+
+		Vector3f tmp = new Vector3f();
+		for (int i = 0; i < NUM_VERTS_X; i++) {
+			for (int j = 0; j < NUM_VERTS_Y; j++) {
+				float wl = 0.2f;
+				// height set to zero, but can also use curved landscape, just uncomment out the code
+				float height = 0f; // 20f * (float)Math.sin(i * wl) * (float)Math.cos(j * wl);
+				
+				//#ifdef FORCE_ZAXIS_UP
+				//m_vertices[i+j*NUM_VERTS_X].setValue(
+				//	(i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
+				//	(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE,
+				//	height
+				//	);
+				//#else
+				tmp.set(
+						(i - NUM_VERTS_X * 0.5f) * TRIANGLE_SIZE,
+						height,
+						(j - NUM_VERTS_Y * 0.5f) * TRIANGLE_SIZE);
+
+				int index = i + j * NUM_VERTS_X;
+				vertices.putFloat((index * 3 + 0) * 4, tmp.x);
+				vertices.putFloat((index * 3 + 1) * 4, tmp.y);
+				vertices.putFloat((index * 3 + 2) * 4, tmp.z);
+				//#endif
+			}
+		}
+
+		//int index=0;
+		gIndices.clear();
+		for (int i = 0; i < NUM_VERTS_X - 1; i++) {
+			for (int j = 0; j < NUM_VERTS_Y - 1; j++) {
+				gIndices.putInt(j * NUM_VERTS_X + i);
+				gIndices.putInt(j * NUM_VERTS_X + i + 1);
+				gIndices.putInt((j + 1) * NUM_VERTS_X + i + 1);
+
+				gIndices.putInt(j * NUM_VERTS_X + i);
+				gIndices.putInt((j + 1) * NUM_VERTS_X + i + 1);
+				gIndices.putInt((j + 1) * NUM_VERTS_X + i);
+			}
+		}
+		gIndices.flip();
+
+		indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
+				gIndices,
+				indexStride,
+				totalVerts, vertices, vertStride);
+
+		boolean useQuantizedAabbCompression = true;
+		groundShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression);
+
+		tr.origin.set(0, -4.5f, 0);
+
+		//#else
+//		//testing btHeightfieldTerrainShape
+//		int width=128;
+//		int length=128;
+//		unsigned char* heightfieldData = new unsigned char[width*length];
+//		{
+//			for (int i=0;i<width*length;i++)
+//			{
+//				heightfieldData[i]=0;
+//			}
+//		}
+//
+//		char*	filename="heightfield128x128.raw";
+//		FILE* heightfieldFile = fopen(filename,"r");
+//		if (!heightfieldFile)
+//		{
+//			filename="../../heightfield128x128.raw";
+//			heightfieldFile = fopen(filename,"r");
+//		}
+//		if (heightfieldFile)
+//		{
+//			int numBytes =fread(heightfieldData,1,width*length,heightfieldFile);
+//			//btAssert(numBytes);
+//			if (!numBytes)
+//			{
+//				printf("couldn't read heightfield at %s\n",filename);
+//			}
+//			fclose (heightfieldFile);
+//		}
+//
+//
+//		btScalar maxHeight = 20000.f;
+//
+//		bool useFloatDatam=false;
+//		bool flipQuadEdges=false;
+//
+//		btHeightfieldTerrainShape* heightFieldShape = new btHeightfieldTerrainShape(width,length,heightfieldData,maxHeight,upIndex,useFloatDatam,flipQuadEdges);;
+//		groundShape = heightFieldShape;
+//
+//		heightFieldShape->setUseDiamondSubdivision(true);
+//
+//		btVector3 localScaling(20,20,20);
+//		localScaling[upIndex]=1.f;
+//		groundShape->setLocalScaling(localScaling);
+//
+//		tr.setOrigin(btVector3(0,-64.5f,0));
+//
+		//#endif
+
+		collisionShapes.add(groundShape);
+
+		// create ground object
+		localCreateRigidBody(0, tr, groundShape);
+
+		//#ifdef FORCE_ZAXIS_UP
+		//	//   indexRightAxis = 0; 
+		//	//   indexUpAxis = 2; 
+		//	//   indexForwardAxis = 1; 
+		//btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,2.f, 0.5f));
+		//btCompoundShape* compound = new btCompoundShape();
+		//btTransform localTrans;
+		//localTrans.setIdentity();
+		// //localTrans effectively shifts the center of mass with respect to the chassis
+		//localTrans.setOrigin(btVector3(0,0,1));
+		//#else
+		CollisionShape chassisShape = new BoxShape(new Vector3f(1.0f, 0.5f, 2.0f));
+		collisionShapes.add(chassisShape);
+
+		CompoundShape compound = new CompoundShape();
+		collisionShapes.add(compound);
+		Transform localTrans = new Transform();
+		localTrans.setIdentity();
+		// localTrans effectively shifts the center of mass with respect to the chassis
+		localTrans.origin.set(0, 1, 0);
+		//#endif
+
+		compound.addChildShape(localTrans, chassisShape);
+
+		tr.origin.set(0, 0, 0);
+
+		carChassis = localCreateRigidBody(800, tr, compound); //chassisShape);
+		//m_carChassis->setDamping(0.2,0.2);
+
+		clientResetScene();
+
+		// create vehicle
+		{
+			vehicleRayCaster = new DefaultVehicleRaycaster(dynamicsWorld);
+			vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster);
+
+			// never deactivate the vehicle
+			carChassis.setActivationState(CollisionObject.DISABLE_DEACTIVATION);
+
+			dynamicsWorld.addVehicle(vehicle);
+
+			float connectionHeight = 1.2f;
+
+			boolean isFrontWheel = true;
+
+			// choose coordinate system
+			vehicle.setCoordinateSystem(rightIndex, upIndex, forwardIndex);
+
+			//#ifdef FORCE_ZAXIS_UP
+			//btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),2*CUBE_HALF_EXTENTS-wheelRadius, connectionHeight);
+			//#else
+			Vector3f connectionPointCS0 = new Vector3f(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2f * CUBE_HALF_EXTENTS - wheelRadius);
+			//#endif
+
+			vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
+			//#ifdef FORCE_ZAXIS_UP
+			//connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),2*CUBE_HALF_EXTENTS-wheelRadius, connectionHeight);
+			//#else
+			connectionPointCS0.set(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 2f * CUBE_HALF_EXTENTS - wheelRadius);
+			//#endif
+
+			vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
+			//#ifdef FORCE_ZAXIS_UP
+			//connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),-2*CUBE_HALF_EXTENTS+wheelRadius, connectionHeight);
+			//#else
+			connectionPointCS0.set(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2f * CUBE_HALF_EXTENTS + wheelRadius);
+			//#endif //FORCE_ZAXIS_UP
+			isFrontWheel = false;
+			vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
+			//#ifdef FORCE_ZAXIS_UP
+			//connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),-2*CUBE_HALF_EXTENTS+wheelRadius, connectionHeight);
+			//#else
+			connectionPointCS0.set(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2f * CUBE_HALF_EXTENTS + wheelRadius);
+			//#endif
+			vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel);
+
+			for (int i = 0; i < vehicle.getNumWheels(); i++) {
+				WheelInfo wheel = vehicle.getWheelInfo(i);
+				wheel.suspensionStiffness = suspensionStiffness;
+				wheel.wheelsDampingRelaxation = suspensionDamping;
+				wheel.wheelsDampingCompression = suspensionCompression;
+				wheel.frictionSlip = wheelFriction;
+				wheel.rollInfluence = rollInfluence;
+			}
+		}
+
+		setCameraDistance(26.f);
+	}
+	
+	// to be implemented by the demo
+	@Override
+	public void renderme() {
+		updateCamera();
+
+		CylinderShapeX wheelShape = new CylinderShapeX(new Vector3f(wheelWidth, wheelRadius, wheelRadius));
+		Vector3f wheelColor = new Vector3f(1, 0, 0);
+
+		for (int i = 0; i < vehicle.getNumWheels(); i++) {
+			// synchronize the wheels with the (interpolated) chassis worldtransform
+			vehicle.updateWheelTransform(i, true);
+			// draw wheels (cylinders)
+			Transform trans = vehicle.getWheelInfo(i).worldTransform;
+			GLShapeDrawer.drawOpenGL(glsrt, gl, trans, wheelShape, wheelColor, getDebugMode());
+		}
+
+		super.renderme();
+	}
+	
+	@Override
+    public void display(GLAutoDrawable drawable) {
+        gl  = drawable.getGL();
+		gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT); 
+
+		{			
+			int wheelIndex = 2;
+			vehicle.applyEngineForce(gEngineForce,wheelIndex);
+			vehicle.setBrake(gBreakingForce,wheelIndex);
+			wheelIndex = 3;
+			vehicle.applyEngineForce(gEngineForce,wheelIndex);
+			vehicle.setBrake(gBreakingForce,wheelIndex);
+
+			wheelIndex = 0;
+			vehicle.setSteeringValue(gVehicleSteering,wheelIndex);
+			wheelIndex = 1;
+			vehicle.setSteeringValue(gVehicleSteering,wheelIndex);
+		}
+
+		float dt = clock.getTimeMicroseconds() * 0.000001f;
+		clock.reset();
+		if (dynamicsWorld != null)
+		{
+			// during idle mode, just run 1 simulation step maximum
+			int maxSimSubSteps = idle ? 1 : 2;
+			if (isIdle())
+				dt = 1f/420f;
+
+			int numSimSteps = dynamicsWorld.stepSimulation(dt,maxSimSubSteps);
+			// optional but useful: debug drawing
+			dynamicsWorld.debugDrawWorld();
+
+			//#define VERBOSE_FEEDBACK
+			//#ifdef VERBOSE_FEEDBACK
+			//if (!numSimSteps)
+			//	printf("Interpolated transforms\n");
+			//else
+			//{
+			//	if (numSimSteps > maxSimSubSteps)
+			//	{
+			//		//detect dropping frames
+			//		printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps);
+			//	} else
+			//	{
+			//		printf("Simulated (%i) steps\n",numSimSteps);
+			//	}
+			//}
+			//#endif //VERBOSE_FEEDBACK
+		}
+
+		//#ifdef USE_QUICKPROF 
+		//btProfiler::beginBlock("render"); 
+		//#endif //USE_QUICKPROF 
+
+		renderme(); 
+
+		//#ifdef USE_QUICKPROF 
+		//btProfiler::endBlock("render"); 
+		//#endif 
+	}
+
+	@Override
+	public void clientResetScene() {
+		gVehicleSteering = 0f;
+		Transform tr = new Transform();
+		tr.setIdentity();
+		carChassis.setCenterOfMassTransform(tr);
+		carChassis.setLinearVelocity(new Vector3f(0, 0, 0));
+		carChassis.setAngularVelocity(new Vector3f(0, 0, 0));
+		dynamicsWorld.getBroadphase().getOverlappingPairCache().cleanProxyFromPairs(carChassis.getBroadphaseHandle(), getDynamicsWorld().getDispatcher());
+		if (vehicle != null) {
+			vehicle.resetSuspension();
+			for (int i = 0; i < vehicle.getNumWheels(); i++) {
+				// synchronize the wheels with the (interpolated) chassis worldtransform
+				vehicle.updateWheelTransform(i, true);
+			}
+		}
+	}
+
+	@Override
+	public void specialKeyboard(int key) {
+		//	printf("key = %i x=%i y=%i\n",key,x,y);
+
+		switch (key) {
+			case KeyEvent.VK_LEFT: {
+				gVehicleSteering += steeringIncrement;
+				if (gVehicleSteering > steeringClamp) {
+					gVehicleSteering = steeringClamp;
+				}
+				break;
+			}
+			case KeyEvent.VK_RIGHT: {
+				gVehicleSteering -= steeringIncrement;
+				if (gVehicleSteering < -steeringClamp) {
+					gVehicleSteering = -steeringClamp;
+				}
+				break;
+			}
+			case KeyEvent.VK_UP: {
+				gEngineForce = maxEngineForce;
+				gBreakingForce = 0.f;
+				break;
+			}
+			case KeyEvent.VK_DOWN: {
+				gBreakingForce = maxBreakingForce;
+				gEngineForce = 0.f;
+				break;
+			}
+			default:
+				super.specialKeyboard(key);
+				break;
+		}
+
+		//glutPostRedisplay();
+	}
+
+	@Override
+	public void updateCamera()
+	{
+
+		// //#define DISABLE_CAMERA 1
+		//#ifdef DISABLE_CAMERA
+		//DemoApplication::updateCamera();
+		//return;
+		//#endif //DISABLE_CAMERA
+
+		gl.glMatrixMode(gl.GL_PROJECTION);
+		gl.glLoadIdentity();
+
+		Transform chassisWorldTrans = new Transform();
+
+		// look at the vehicle
+		carChassis.getMotionState().getWorldTransform(chassisWorldTrans);
+		cameraTargetPosition.set(chassisWorldTrans.origin);
+
+		// interpolate the camera height
+		//#ifdef FORCE_ZAXIS_UP
+		//m_cameraPosition[2] = (15.0*m_cameraPosition[2] + m_cameraTargetPosition[2] + m_cameraHeight)/16.0;
+		//#else
+		cameraPosition.y = (15.0f*cameraPosition.y + cameraTargetPosition.y + cameraHeight) / 16.0f;
+		//#endif
+
+		Vector3f camToObject = new Vector3f();
+		camToObject.sub(cameraTargetPosition, cameraPosition);
+
+		// keep distance between min and max distance
+		float cameraDistance = camToObject.length();
+		float correctionFactor = 0f;
+		if (cameraDistance < minCameraDistance)
+		{
+			correctionFactor = 0.15f*(minCameraDistance-cameraDistance)/cameraDistance;
+		}
+		if (cameraDistance > maxCameraDistance)
+		{
+			correctionFactor = 0.15f*(maxCameraDistance-cameraDistance)/cameraDistance;
+		}
+		Vector3f tmp = new Vector3f();
+		tmp.scale(correctionFactor, camToObject);
+		cameraPosition.sub(tmp);
+
+		// update OpenGL camera settings
+		gl.glFrustum(-1.0, 1.0, -1.0, 1.0, 1.0, 10000.0);
+
+		glu.gluLookAt(cameraPosition.x,cameraPosition.y,cameraPosition.z,
+				  cameraTargetPosition.x,cameraTargetPosition.y, cameraTargetPosition.z,
+				  cameraUp.x,cameraUp.y,cameraUp.z);
+		gl.glMatrixMode(gl.GL_MODELVIEW);
+	}
+	
+	public static void main(String[] args) {
+		VehicleDemo vehicleDemo = new VehicleDemo();
+		vehicleDemo.initPhysics();
+
+		JOGL.main(args, 640, 480, "Bullet Vehicle Demo", vehicleDemo);
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/DiscreteDynamicsWorld.java b/src/jbullet/src/javabullet/dynamics/DiscreteDynamicsWorld.java
new file mode 100644
index 0000000..237d4ec
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/DiscreteDynamicsWorld.java
@@ -0,0 +1,985 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics;
+
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+import javabullet.BulletGlobals;
+import javabullet.collision.broadphase.BroadphaseInterface;
+import javabullet.collision.broadphase.CollisionFilterGroups;
+import javabullet.collision.broadphase.Dispatcher;
+import javabullet.collision.broadphase.DispatcherInfo;
+import javabullet.collision.dispatch.CollisionConfiguration;
+import javabullet.collision.dispatch.CollisionObject;
+import javabullet.collision.dispatch.CollisionWorld;
+import javabullet.collision.dispatch.SimulationIslandManager;
+import javabullet.collision.narrowphase.PersistentManifold;
+import javabullet.collision.shapes.CollisionShape;
+import javabullet.collision.shapes.InternalTriangleIndexCallback;
+import javabullet.collision.shapes.TriangleCallback;
+import javabullet.dynamics.constraintsolver.ConstraintSolver;
+import javabullet.dynamics.constraintsolver.ContactSolverInfo;
+import javabullet.dynamics.constraintsolver.SequentialImpulseConstraintSolver;
+import javabullet.dynamics.constraintsolver.TypedConstraint;
+import javabullet.dynamics.vehicle.RaycastVehicle;
+import javabullet.linearmath.DebugDrawModes;
+import javabullet.linearmath.IDebugDraw;
+import javabullet.linearmath.MiscUtil;
+import javabullet.linearmath.ScalarUtil;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.TransformUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ * DiscreteDynamicsWorld provides discrete rigid body simulation.
+ * Those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController.
+ * 
+ * @author jezek2
+ */
+public class DiscreteDynamicsWorld extends DynamicsWorld {
+
+	protected ConstraintSolver constraintSolver;
+	protected SimulationIslandManager islandManager;
+	protected final List<TypedConstraint> constraints = new ArrayList<TypedConstraint>();
+	protected final Vector3f gravity = new Vector3f(0f, -10f, 0f);
+
+	//for variable timesteps
+	protected float localTime = 1f / 60f;
+	//for variable timesteps
+
+	protected boolean ownsIslandManager;
+	protected boolean ownsConstraintSolver;
+
+	protected ContactSolverInfo solverInfo = new ContactSolverInfo();
+	protected List<RaycastVehicle> vehicles = new ArrayList<RaycastVehicle>();
+	protected int profileTimings = 0;
+	
+	public DiscreteDynamicsWorld(Dispatcher dispatcher, BroadphaseInterface pairCache, ConstraintSolver constraintSolver, CollisionConfiguration collisionConfiguration) {
+		super(dispatcher, pairCache, collisionConfiguration);
+		this.constraintSolver = constraintSolver;
+
+		if (this.constraintSolver == null) {
+			this.constraintSolver = new SequentialImpulseConstraintSolver();
+			ownsConstraintSolver = true;
+		}
+		else {
+			ownsConstraintSolver = false;
+		}
+
+		{
+			islandManager = new SimulationIslandManager();
+		}
+
+		ownsIslandManager = true;
+	}
+
+	protected void saveKinematicState(float timeStep) {
+		for (int i = 0; i < collisionObjects.size(); i++) {
+			CollisionObject colObj = collisionObjects.get(i);
+			RigidBody body = RigidBody.upcast(colObj);
+			if (body != null) {
+				//Transform predictedTrans = new Transform();
+				if (body.getActivationState() != CollisionObject.ISLAND_SLEEPING) {
+					if (body.isKinematicObject()) {
+						// to calculate velocities next frame
+						body.saveKinematicState(timeStep);
+					}
+				}
+			}
+		}
+	}
+
+	@Override
+	public void debugDrawWorld() {
+		stack.vectors.push();
+		try {
+			if (getDebugDrawer() != null && (getDebugDrawer().getDebugMode() & (DebugDrawModes.DRAW_WIREFRAME | DebugDrawModes.DRAW_AABB)) != 0) {
+				int i;
+
+				// todo: iterate over awake simulation islands!
+				for (i = 0; i < collisionObjects.size(); i++) {
+					CollisionObject colObj = collisionObjects.get(i);
+					if (getDebugDrawer() != null && (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_WIREFRAME) != 0) {
+						Vector3f color = stack.vectors.get(255f, 255f, 255f);
+						switch (colObj.getActivationState()) {
+							case CollisionObject.ACTIVE_TAG:
+								color.set(255f, 255f, 255f);
+								break;
+							case CollisionObject.ISLAND_SLEEPING:
+								color.set(0f, 255f, 0f);
+								break;
+							case CollisionObject.WANTS_DEACTIVATION:
+								color.set(0f, 255f, 255f);
+								break;
+							case CollisionObject.DISABLE_DEACTIVATION:
+								color.set(255f, 0f, 0f);
+								break;
+							case CollisionObject.DISABLE_SIMULATION:
+								color.set(255f, 255f, 0f);
+								break;
+							default: {
+								color.set(255f, 0f, 0f);
+							}
+						}
+
+						debugDrawObject(colObj.getWorldTransform(), colObj.getCollisionShape(), color);
+					}
+					if (debugDrawer != null && (debugDrawer.getDebugMode() & DebugDrawModes.DRAW_AABB) != 0) {
+						Vector3f minAabb = stack.vectors.get(), maxAabb = stack.vectors.get();
+						Vector3f colorvec = stack.vectors.get(1f, 0f, 0f);
+						colObj.getCollisionShape().getAabb(colObj.getWorldTransform(), minAabb, maxAabb);
+						debugDrawer.drawAabb(minAabb, maxAabb, colorvec);
+					}
+				}
+
+				Vector3f wheelColor = stack.vectors.get();
+				Vector3f wheelPosWS = stack.vectors.get();
+				Vector3f axle = stack.vectors.get();
+				Vector3f tmp = stack.vectors.get();
+
+				for (i = 0; i < vehicles.size(); i++) {
+					for (int v = 0; v < vehicles.get(i).getNumWheels(); v++) {
+						wheelColor.set(0, 255, 255);
+						if (vehicles.get(i).getWheelInfo(v).raycastInfo.isInContact) {
+							wheelColor.set(0, 0, 255);
+						}
+						else {
+							wheelColor.set(255, 0, 255);
+						}
+
+						wheelPosWS.set(vehicles.get(i).getWheelInfo(v).worldTransform.origin);
+
+						axle.set(
+								vehicles.get(i).getWheelInfo(v).worldTransform.basis.getElement(0, vehicles.get(i).getRightAxis()),
+								vehicles.get(i).getWheelInfo(v).worldTransform.basis.getElement(1, vehicles.get(i).getRightAxis()),
+								vehicles.get(i).getWheelInfo(v).worldTransform.basis.getElement(2, vehicles.get(i).getRightAxis()));
+
+
+						//m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
+						//debug wheels (cylinders)
+						tmp.add(wheelPosWS, axle);
+						debugDrawer.drawLine(wheelPosWS, tmp, wheelColor);
+						debugDrawer.drawLine(wheelPosWS, vehicles.get(i).getWheelInfo(v).raycastInfo.contactPointWS, wheelColor);
+					}
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void clearForces() {
+		// todo: iterate over awake simulation islands!
+		for (int i = 0; i < collisionObjects.size(); i++) {
+			CollisionObject colObj = collisionObjects.get(i);
+
+			RigidBody body = RigidBody.upcast(colObj);
+			if (body != null) {
+				body.clearForces();
+			}
+		}
+	}
+	
+	/**
+	 * Apply gravity, call this once per timestep.
+	 */
+	public void applyGravity() {
+		// todo: iterate over awake simulation islands!
+		for (int i = 0; i < collisionObjects.size(); i++) {
+			CollisionObject colObj = collisionObjects.get(i);
+
+			RigidBody body = RigidBody.upcast(colObj);
+			if (body != null && body.isActive()) {
+				body.applyGravity();
+			}
+		}
+	}
+
+	protected void synchronizeMotionStates() {
+		stack.transforms.push();
+		try {
+			Transform interpolatedTransform = stack.transforms.get();
+
+			// todo: iterate over awake simulation islands!
+			for (int i = 0; i < collisionObjects.size(); i++) {
+				CollisionObject colObj = collisionObjects.get(i);
+
+				RigidBody body = RigidBody.upcast(colObj);
+				if (body != null && body.getMotionState() != null && !body.isStaticOrKinematicObject()) {
+					// we need to call the update at least once, even for sleeping objects
+					// otherwise the 'graphics' transform never updates properly
+					// so todo: add 'dirty' flag
+					//if (body->getActivationState() != ISLAND_SLEEPING)
+					{
+						TransformUtil.integrateTransform(body.getInterpolationWorldTransform(),
+								body.getInterpolationLinearVelocity(), body.getInterpolationAngularVelocity(), localTime, interpolatedTransform);
+						body.getMotionState().setWorldTransform(interpolatedTransform);
+					}
+				}
+			}
+
+			if (getDebugDrawer() != null && (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_WIREFRAME) != 0) {
+				for (int i = 0; i < vehicles.size(); i++) {
+					for (int v = 0; v < vehicles.get(i).getNumWheels(); v++) {
+						// synchronize the wheels with the (interpolated) chassis worldtransform
+						vehicles.get(i).updateWheelTransform(v, true);
+					}
+				}
+			}
+		}
+		finally {
+			stack.transforms.pop();
+		}
+	}
+
+    public long nanoTime() {
+		// JAU: Orig: return System.nanoTime();
+		return System.currentTimeMillis()*1000000;
+    }
+
+	@Override
+	public int stepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) {
+		startProfiling(timeStep);
+
+		long t0 = nanoTime();
+		
+		BulletGlobals.pushProfile("stepSimulation");
+		try {
+			int numSimulationSubSteps = 0;
+
+			if (maxSubSteps != 0) {
+				// fixed timestep with interpolation
+				localTime += timeStep;
+				if (localTime >= fixedTimeStep) {
+					numSimulationSubSteps = (int) (localTime / fixedTimeStep);
+					localTime -= numSimulationSubSteps * fixedTimeStep;
+				}
+			}
+			else {
+				//variable timestep
+				fixedTimeStep = timeStep;
+				localTime = timeStep;
+				if (ScalarUtil.fuzzyZero(timeStep)) {
+					numSimulationSubSteps = 0;
+					maxSubSteps = 0;
+				}
+				else {
+					numSimulationSubSteps = 1;
+					maxSubSteps = 1;
+				}
+			}
+
+			// process some debugging flags
+			if (getDebugDrawer() != null) {
+				BulletGlobals.gDisableDeactivation = (getDebugDrawer().getDebugMode() & DebugDrawModes.NO_DEACTIVATION) != 0;
+			}
+			if (numSimulationSubSteps != 0) {
+				saveKinematicState(fixedTimeStep);
+
+				applyGravity();
+
+				// clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
+				int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
+
+				for (int i = 0; i < clampedSimulationSteps; i++) {
+					internalSingleStepSimulation(fixedTimeStep);
+					synchronizeMotionStates();
+				}
+			}
+
+			synchronizeMotionStates();
+
+			clearForces();
+
+			// TODO: CProfileManager::Increment_Frame_Counter();
+
+			return numSimulationSubSteps;
+		}
+		finally {
+			BulletGlobals.popProfile();
+			
+			BulletGlobals.stepSimulationTime = (nanoTime() - t0) / 1000000;
+		}
+	}
+
+	protected void internalSingleStepSimulation(float timeStep) {
+		BulletGlobals.pushProfile("internalSingleStepSimulation");
+		try {
+			// apply gravity, predict motion
+			predictUnconstraintMotion(timeStep);
+
+			DispatcherInfo dispatchInfo = getDispatchInfo();
+
+			dispatchInfo.timeStep = timeStep;
+			dispatchInfo.stepCount = 0;
+			dispatchInfo.debugDraw = getDebugDrawer();
+
+			// perform collision detection
+			performDiscreteCollisionDetection();
+
+			calculateSimulationIslands();
+
+			getSolverInfo().timeStep = timeStep;
+
+			// solve contact and other joint constraints
+			solveConstraints(getSolverInfo());
+
+			//CallbackTriggers();
+
+			// integrate transforms
+			integrateTransforms(timeStep);
+
+			// update vehicle simulation
+			updateVehicles(timeStep);
+
+			updateActivationState(timeStep);
+		}
+		finally {
+			BulletGlobals.popProfile();
+		}
+	}
+
+	@Override
+	public void setGravity(Vector3f gravity) {
+		this.gravity.set(gravity);
+		for (int i = 0; i < collisionObjects.size(); i++) {
+			CollisionObject colObj = collisionObjects.get(i);
+			RigidBody body = RigidBody.upcast(colObj);
+			if (body != null) {
+				body.setGravity(gravity);
+			}
+		}
+	}
+
+	@Override
+	public void removeRigidBody(RigidBody body) {
+		removeCollisionObject(body);
+	}
+
+	@Override
+	public void addRigidBody(RigidBody body) {
+		if (!body.isStaticOrKinematicObject()) {
+			body.setGravity(gravity);
+		}
+
+		if (body.getCollisionShape() != null) {
+			boolean isDynamic = !(body.isStaticObject() || body.isKinematicObject());
+			short collisionFilterGroup = isDynamic ? (short) CollisionFilterGroups.DEFAULT_FILTER : (short) CollisionFilterGroups.STATIC_FILTER;
+			short collisionFilterMask = isDynamic ? (short) CollisionFilterGroups.ALL_FILTER : (short) (CollisionFilterGroups.ALL_FILTER ^ CollisionFilterGroups.STATIC_FILTER);
+
+			addCollisionObject(body, collisionFilterGroup, collisionFilterMask);
+		}
+	}
+
+	public void addRigidBody(RigidBody body, short group, short mask) {
+		if (!body.isStaticOrKinematicObject()) {
+			body.setGravity(gravity);
+		}
+
+		if (body.getCollisionShape() != null) {
+			addCollisionObject(body, group, mask);
+		}
+	}
+
+	protected void updateVehicles(float timeStep) {
+		BulletGlobals.pushProfile("updateVehicles");
+		try {
+			for (int i = 0; i < vehicles.size(); i++) {
+				RaycastVehicle vehicle = vehicles.get(i);
+				vehicle.updateVehicle(timeStep);
+			}
+		}
+		finally {
+			BulletGlobals.popProfile();
+		}
+	}
+
+	protected void updateActivationState(float timeStep) {
+		BulletGlobals.pushProfile("updateActivationState");
+		try {
+			for (int i = 0; i < collisionObjects.size(); i++) {
+				CollisionObject colObj = collisionObjects.get(i);
+				RigidBody body = RigidBody.upcast(colObj);
+				if (body != null) {
+					body.updateDeactivation(timeStep);
+
+					if (body.wantsSleeping()) {
+						if (body.isStaticOrKinematicObject()) {
+							body.setActivationState(CollisionObject.ISLAND_SLEEPING);
+						}
+						else {
+							if (body.getActivationState() == CollisionObject.ACTIVE_TAG) {
+								body.setActivationState(CollisionObject.WANTS_DEACTIVATION);
+							}
+						}
+					}
+					else {
+						if (body.getActivationState() != CollisionObject.DISABLE_DEACTIVATION) {
+							body.setActivationState(CollisionObject.ACTIVE_TAG);
+						}
+					}
+				}
+			}
+		}
+		finally {
+			BulletGlobals.popProfile();
+		}
+	}
+
+	@Override
+	public void addConstraint(TypedConstraint constraint, boolean disableCollisionsBetweenLinkedBodies) {
+		constraints.add(constraint);
+		if (disableCollisionsBetweenLinkedBodies) {
+			constraint.getRigidBodyA().addConstraintRef(constraint);
+			constraint.getRigidBodyB().addConstraintRef(constraint);
+		}
+	}
+
+	@Override
+	public void removeConstraint(TypedConstraint constraint) {
+		constraints.remove(constraint);
+		constraint.getRigidBodyA().removeConstraintRef(constraint);
+		constraint.getRigidBodyB().removeConstraintRef(constraint);
+	}
+	
+	@Override
+	public void addVehicle(RaycastVehicle vehicle) {
+		vehicles.add(vehicle);
+	}
+	
+	@Override
+	public void removeVehicle(RaycastVehicle vehicle) {
+		vehicles.remove(vehicle);
+	}
+	
+	private static int getConstraintIslandId(TypedConstraint lhs) {
+		int islandId;
+
+		CollisionObject rcolObj0 = lhs.getRigidBodyA();
+		CollisionObject rcolObj1 = lhs.getRigidBodyB();
+		islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
+		return islandId;
+	}
+	
+	private static class InplaceSolverIslandCallback implements SimulationIslandManager.IslandCallback {
+		public ContactSolverInfo solverInfo;
+		public ConstraintSolver solver;
+		public List<TypedConstraint> sortedConstraints;
+		public int numConstraints;
+		public IDebugDraw debugDrawer;
+		//public StackAlloc* m_stackAlloc;
+		public Dispatcher dispatcher;
+
+		public void init(ContactSolverInfo solverInfo, ConstraintSolver solver, List<TypedConstraint> sortedConstraints, int numConstraints, IDebugDraw debugDrawer, Dispatcher dispatcher) {
+			this.solverInfo = solverInfo;
+			this.solver = solver;
+			this.sortedConstraints = sortedConstraints;
+			this.numConstraints = numConstraints;
+			this.debugDrawer = debugDrawer;
+			this.dispatcher = dispatcher;
+		}
+
+		public void processIsland(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifolds, int manifolds_offset, int numManifolds, int islandId) {
+			if (islandId < 0) {
+				// we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
+				solver.solveGroup(bodies, numBodies, manifolds, manifolds_offset, numManifolds, sortedConstraints, 0, numConstraints, solverInfo, debugDrawer/*,m_stackAlloc*/, dispatcher);
+			}
+			else {
+				// also add all non-contact constraints/joints for this island
+				//List<TypedConstraint> startConstraint = null;
+				int startConstraint_idx = -1;
+				int numCurConstraints = 0;
+				int i;
+
+				// find the first constraint for this island
+				for (i = 0; i < numConstraints; i++) {
+					if (getConstraintIslandId(sortedConstraints.get(i)) == islandId) {
+						//startConstraint = &m_sortedConstraints[i];
+						//startConstraint = sortedConstraints.subList(i, sortedConstraints.size());
+						startConstraint_idx = i;
+						break;
+					}
+				}
+				// count the number of constraints in this island
+				for (; i < numConstraints; i++) {
+					if (getConstraintIslandId(sortedConstraints.get(i)) == islandId) {
+						numCurConstraints++;
+					}
+				}
+
+				solver.solveGroup(bodies, numBodies, manifolds, manifolds_offset, numManifolds, sortedConstraints, startConstraint_idx, numCurConstraints, solverInfo, debugDrawer/*,m_stackAlloc*/, dispatcher);
+			}
+		}
+	}
+
+	private List<TypedConstraint> sortedConstraints = new ArrayList<TypedConstraint>();
+	private InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback();
+	
+	protected void solveConstraints(ContactSolverInfo solverInfo) {
+		BulletGlobals.pushProfile("solveConstraints");
+		try {
+			// sorted version of all btTypedConstraint, based on islandId
+			sortedConstraints.clear();
+			for (int i=0; i<constraints.size(); i++) {
+				sortedConstraints.add(constraints.get(i));
+			}
+			//Collections.sort(sortedConstraints, sortConstraintOnIslandPredicate);
+			MiscUtil.heapSort(sortedConstraints, sortConstraintOnIslandPredicate);
+
+			List<TypedConstraint> constraintsPtr = getNumConstraints() != 0 ? sortedConstraints : null;
+
+			solverCallback.init(solverInfo, constraintSolver, constraintsPtr, sortedConstraints.size(), debugDrawer/*,m_stackAlloc*/, dispatcher1);
+
+			constraintSolver.prepareSolve(getCollisionWorld().getNumCollisionObjects(), getCollisionWorld().getDispatcher().getNumManifolds());
+
+			// solve all the constraints for this island
+			islandManager.buildAndProcessIslands(getCollisionWorld().getDispatcher(), getCollisionWorld().getCollisionObjectArray(), solverCallback);
+
+			constraintSolver.allSolved(solverInfo, debugDrawer/*, m_stackAlloc*/);
+		}
+		finally {
+			BulletGlobals.popProfile();
+		}
+	}
+
+	protected void calculateSimulationIslands() {
+		BulletGlobals.pushProfile("calculateSimulationIslands");
+		try {
+			getSimulationIslandManager().updateActivationState(getCollisionWorld(), getCollisionWorld().getDispatcher());
+
+			{
+				int i;
+				int numConstraints = constraints.size();
+				for (i = 0; i < numConstraints; i++) {
+					TypedConstraint constraint = constraints.get(i);
+
+					RigidBody colObj0 = constraint.getRigidBodyA();
+					RigidBody colObj1 = constraint.getRigidBodyB();
+
+					if (((colObj0 != null) && ((colObj0).mergesSimulationIslands())) &&
+							((colObj1 != null) && ((colObj1).mergesSimulationIslands()))) {
+						if (colObj0.isActive() || colObj1.isActive()) {
+
+							getSimulationIslandManager().getUnionFind().unite((colObj0).getIslandTag(),
+									(colObj1).getIslandTag());
+						}
+					}
+				}
+			}
+
+			// Store the island id in each body
+			getSimulationIslandManager().storeIslandActivationState(getCollisionWorld());
+		}
+		finally {
+			BulletGlobals.popProfile();
+		}
+	}
+
+	protected void integrateTransforms(float timeStep) {
+		BulletGlobals.pushProfile("integrateTransforms");
+		stack.transforms.push();
+		try {
+			Transform predictedTrans = stack.transforms.get();
+			for (int i = 0; i < collisionObjects.size(); i++) {
+				CollisionObject colObj = collisionObjects.get(i);
+				RigidBody body = RigidBody.upcast(colObj);
+				if (body != null) {
+					if (body.isActive() && (!body.isStaticOrKinematicObject())) {
+						body.predictIntegratedTransform(timeStep, predictedTrans);
+						body.proceedToTransform(predictedTrans);
+					}
+				}
+			}
+		}
+		finally {
+			stack.transforms.pop();
+			BulletGlobals.popProfile();
+		}
+	}
+	
+	protected void predictUnconstraintMotion(float timeStep) {
+		BulletGlobals.pushProfile("predictUnconstraintMotion");
+		try {
+			for (int i = 0; i < collisionObjects.size(); i++) {
+				CollisionObject colObj = collisionObjects.get(i);
+				RigidBody body = RigidBody.upcast(colObj);
+				if (body != null) {
+					if (!body.isStaticOrKinematicObject()) {
+						if (body.isActive()) {
+							body.integrateVelocities(timeStep);
+							// damping
+							body.applyDamping(timeStep);
+
+							body.predictIntegratedTransform(timeStep, body.getInterpolationWorldTransform());
+						}
+					}
+				}
+			}
+		}
+		finally {
+			BulletGlobals.popProfile();
+		}
+	}
+	
+	protected void startProfiling(float timeStep) {
+		// TODO
+	}
+
+	protected void debugDrawSphere(float radius, Transform transform, Vector3f color) {
+		stack.vectors.push();
+		try {
+			Vector3f start = stack.vectors.get(transform.origin);
+
+			Vector3f xoffs = stack.vectors.get(radius, 0, 0);
+			transform.basis.transform(xoffs);
+			Vector3f yoffs = stack.vectors.get(0, radius, 0);
+			transform.basis.transform(yoffs);
+			Vector3f zoffs = stack.vectors.get(0, 0, radius);
+			transform.basis.transform(zoffs);
+
+			Vector3f tmp1 = stack.vectors.get();
+			Vector3f tmp2 = stack.vectors.get();
+
+			// XY
+			tmp1.sub(start, xoffs);
+			tmp2.add(start, yoffs);
+			getDebugDrawer().drawLine(tmp1, tmp2, color);
+			tmp1.add(start, yoffs);
+			tmp2.add(start, xoffs);
+			getDebugDrawer().drawLine(tmp1, tmp2, color);
+			tmp1.add(start, xoffs);
+			tmp2.sub(start, yoffs);
+			getDebugDrawer().drawLine(tmp1, tmp2, color);
+			tmp1.sub(start, yoffs);
+			tmp2.sub(start, xoffs);
+			getDebugDrawer().drawLine(tmp1, tmp2, color);
+
+			// XZ
+			tmp1.sub(start, xoffs);
+			tmp2.add(start, zoffs);
+			getDebugDrawer().drawLine(tmp1, tmp2, color);
+			tmp1.add(start, zoffs);
+			tmp2.add(start, xoffs);
+			getDebugDrawer().drawLine(tmp1, tmp2, color);
+			tmp1.add(start, xoffs);
+			tmp2.sub(start, zoffs);
+			getDebugDrawer().drawLine(tmp1, tmp2, color);
+			tmp1.sub(start, zoffs);
+			tmp2.sub(start, xoffs);
+			getDebugDrawer().drawLine(tmp1, tmp2, color);
+
+			// YZ
+			tmp1.sub(start, yoffs);
+			tmp2.add(start, zoffs);
+			getDebugDrawer().drawLine(tmp1, tmp2, color);
+			tmp1.add(start, zoffs);
+			tmp2.add(start, yoffs);
+			getDebugDrawer().drawLine(tmp1, tmp2, color);
+			tmp1.add(start, yoffs);
+			tmp2.sub(start, zoffs);
+			getDebugDrawer().drawLine(tmp1, tmp2, color);
+			tmp1.sub(start, zoffs);
+			tmp2.sub(start, yoffs);
+			getDebugDrawer().drawLine(tmp1, tmp2, color);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public void debugDrawObject(Transform worldTransform, CollisionShape shape, Vector3f color) {
+		stack.vectors.push();
+		try {
+			Vector3f tmp = stack.vectors.get();
+
+			// Draw a small simplex at the center of the object
+			{
+				Vector3f start = stack.vectors.get(worldTransform.origin);
+
+				tmp.set(1f, 0f, 0f);
+				worldTransform.basis.transform(tmp);
+				tmp.add(start);
+				getDebugDrawer().drawLine(start, tmp, stack.vectors.get(1f, 0f, 0f));
+
+				tmp.set(0f, 1f, 0f);
+				worldTransform.basis.transform(tmp);
+				tmp.add(start);
+				getDebugDrawer().drawLine(start, tmp, stack.vectors.get(0f, 1f, 0f));
+
+				tmp.set(0f, 0f, 1f);
+				worldTransform.basis.transform(tmp);
+				tmp.add(start);
+				getDebugDrawer().drawLine(start, tmp, stack.vectors.get(0f, 0f, 1f));
+			}
+
+			// TODO: debugDrawObject
+	//		if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
+	//		{
+	//			const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
+	//			for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
+	//			{
+	//				btTransform childTrans = compoundShape->getChildTransform(i);
+	//				const btCollisionShape* colShape = compoundShape->getChildShape(i);
+	//				debugDrawObject(worldTransform*childTrans,colShape,color);
+	//			}
+	//
+	//		} else
+	//		{
+	//			switch (shape->getShapeType())
+	//			{
+	//
+	//			case SPHERE_SHAPE_PROXYTYPE:
+	//				{
+	//					const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
+	//					btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
+	//
+	//					debugDrawSphere(radius, worldTransform, color);
+	//					break;
+	//				}
+	//			case MULTI_SPHERE_SHAPE_PROXYTYPE:
+	//				{
+	//					const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
+	//
+	//					for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
+	//					{
+	//						btTransform childTransform = worldTransform;
+	//						childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
+	//						debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
+	//					}
+	//
+	//					break;
+	//				}
+	//			case CAPSULE_SHAPE_PROXYTYPE:
+	//				{
+	//					const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
+	//
+	//					btScalar radius = capsuleShape->getRadius();
+	//					btScalar halfHeight = capsuleShape->getHalfHeight();
+	//
+	//					// Draw the ends
+	//					{
+	//						btTransform childTransform = worldTransform;
+	//						childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0);
+	//						debugDrawSphere(radius, childTransform, color);
+	//					}
+	//
+	//					{
+	//						btTransform childTransform = worldTransform;
+	//						childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0);
+	//						debugDrawSphere(radius, childTransform, color);
+	//					}
+	//
+	//					// Draw some additional lines
+	//					btVector3 start = worldTransform.getOrigin();
+	//					getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color);
+	//					getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color);
+	//					getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color);
+	//					getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), color);
+	//
+	//					break;
+	//				}
+	//			case CONE_SHAPE_PROXYTYPE:
+	//				{
+	//					const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
+	//					btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
+	//					btScalar height = coneShape->getHeight();//+coneShape->getMargin();
+	//					btVector3 start = worldTransform.getOrigin();
+	//
+	//					int upAxis= coneShape->getConeUpIndex();
+	//
+	//
+	//					btVector3	offsetHeight(0,0,0);
+	//					offsetHeight[upAxis] = height * btScalar(0.5);
+	//					btVector3	offsetRadius(0,0,0);
+	//					offsetRadius[(upAxis+1)%3] = radius;
+	//					btVector3	offset2Radius(0,0,0);
+	//					offset2Radius[(upAxis+2)%3] = radius;
+	//
+	//					getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+	//					getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+	//					getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
+	//					getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
+	//
+	//
+	//
+	//					break;
+	//
+	//				}
+	//			case CYLINDER_SHAPE_PROXYTYPE:
+	//				{
+	//					const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
+	//					int upAxis = cylinder->getUpAxis();
+	//					btScalar radius = cylinder->getRadius();
+	//					btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
+	//					btVector3 start = worldTransform.getOrigin();
+	//					btVector3	offsetHeight(0,0,0);
+	//					offsetHeight[upAxis] = halfHeight;
+	//					btVector3	offsetRadius(0,0,0);
+	//					offsetRadius[(upAxis+1)%3] = radius;
+	//					getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
+	//					getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
+	//					break;
+	//				}
+	//			default:
+	//				{
+	//
+	//					if (shape->isConcave())
+	//					{
+	//						btConcaveShape* concaveMesh = (btConcaveShape*) shape;
+	//
+	//						//todo pass camera, for some culling
+	//						btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+	//						btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+	//
+	//						DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+	//						concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
+	//
+	//					}
+	//
+	//					if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
+	//					{
+	//						btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
+	//						//todo: pass camera for some culling			
+	//						btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+	//						btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
+	//						//DebugDrawcallback drawCallback;
+	//						DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
+	//						convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+	//					}
+	//
+	//
+	//					/// for polyhedral shapes
+	//					if (shape->isPolyhedral())
+	//					{
+	//						btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
+	//
+	//						int i;
+	//						for (i=0;i<polyshape->getNumEdges();i++)
+	//						{
+	//							btPoint3 a,b;
+	//							polyshape->getEdge(i,a,b);
+	//							btVector3 wa = worldTransform * a;
+	//							btVector3 wb = worldTransform * b;
+	//							getDebugDrawer()->drawLine(wa,wb,color);
+	//
+	//						}
+	//
+	//
+	//					}
+	//				}
+	//			}
+	//		}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	@Override
+	public void setConstraintSolver(ConstraintSolver solver) {
+		if (ownsConstraintSolver) {
+			//btAlignedFree( m_constraintSolver);
+		}
+		ownsConstraintSolver = false;
+		constraintSolver = solver;
+	}
+
+	@Override
+	public ConstraintSolver getConstraintSolver() {
+		return constraintSolver;
+	}
+
+	@Override
+	public int getNumConstraints() {
+		return constraints.size();
+	}
+
+	@Override
+	public TypedConstraint getConstraint(int index) {
+		return constraints.get(index);
+	}
+	
+	public SimulationIslandManager getSimulationIslandManager() {
+		return islandManager;
+	}
+
+	public CollisionWorld getCollisionWorld() {
+		return this;
+	}
+
+	@Override
+	public DynamicsWorldType getWorldType() {
+		return DynamicsWorldType.DISCRETE_DYNAMICS_WORLD;
+	}
+	
+	public ContactSolverInfo getSolverInfo() {
+		return solverInfo;
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	private static final Comparator<TypedConstraint> sortConstraintOnIslandPredicate = new Comparator<TypedConstraint>() {
+		public int compare(TypedConstraint lhs, TypedConstraint rhs) {
+			int rIslandId0, lIslandId0;
+			rIslandId0 = getConstraintIslandId(rhs);
+			lIslandId0 = getConstraintIslandId(lhs);
+			return lIslandId0 < rIslandId0? -1 : +1;
+		}
+	};
+	
+	private static class DebugDrawcallback implements TriangleCallback, InternalTriangleIndexCallback {
+		private IDebugDraw debugDrawer;
+		private final Vector3f color = new Vector3f();
+		private final Transform worldTrans = new Transform();
+
+		public DebugDrawcallback(IDebugDraw debugDrawer, Transform worldTrans, Vector3f color) {
+			this.debugDrawer = debugDrawer;
+			this.worldTrans.set(worldTrans);
+			this.color.set(color);
+		}
+
+		public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) {
+			processTriangle(triangle,partId,triangleIndex);
+		}
+
+		private final Vector3f wv0 = new Vector3f(),wv1 = new Vector3f(),wv2 = new Vector3f();
+		
+		public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) {
+			wv0.set(triangle[0]);
+			worldTrans.transform(wv0);
+			wv1.set(triangle[1]);
+			worldTrans.transform(wv1);
+			wv2.set(triangle[2]);
+			worldTrans.transform(wv2);
+
+			debugDrawer.drawLine(wv0, wv1, color);
+			debugDrawer.drawLine(wv1, wv2, color);
+			debugDrawer.drawLine(wv2, wv0, color);
+		}
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/dynamics/DynamicsWorld.java b/src/jbullet/src/javabullet/dynamics/DynamicsWorld.java
new file mode 100644
index 0000000..ef9c324
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/DynamicsWorld.java
@@ -0,0 +1,104 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics;
+
+import javabullet.collision.broadphase.BroadphaseInterface;
+import javabullet.collision.broadphase.Dispatcher;
+import javabullet.collision.dispatch.CollisionConfiguration;
+import javabullet.collision.dispatch.CollisionWorld;
+import javabullet.dynamics.constraintsolver.ConstraintSolver;
+import javabullet.dynamics.constraintsolver.TypedConstraint;
+import javabullet.dynamics.vehicle.RaycastVehicle;
+import javax.vecmath.Vector3f;
+
+/**
+ * DynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous.
+ * 
+ * @author jezek2
+ */
+public abstract class DynamicsWorld extends CollisionWorld {
+
+	public DynamicsWorld(Dispatcher dispatcher, BroadphaseInterface broadphasePairCache, CollisionConfiguration collisionConfiguration) {
+		super(dispatcher, broadphasePairCache, collisionConfiguration);
+	}
+
+	public final int stepSimulation(float timeStep) {
+		return stepSimulation(timeStep, 1, 1f / 60f);
+	}
+
+	public final int stepSimulation(float timeStep, int maxSubSteps) {
+		return stepSimulation(timeStep, maxSubSteps, 1f / 60f);
+	}
+
+	/**
+	 * stepSimulation proceeds the simulation over timeStep units.
+	 * if maxSubSteps > 0, it will interpolate time steps.
+	 */
+	public abstract int stepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep);
+
+	public abstract void debugDrawWorld();
+
+	public final void addConstraint(TypedConstraint constraint) {
+		addConstraint(constraint, false);
+	}
+	
+	public void addConstraint(TypedConstraint constraint, boolean disableCollisionsBetweenLinkedBodies) {
+	}
+
+	public void removeConstraint(TypedConstraint constraint) {
+	}
+
+	public void addVehicle(RaycastVehicle vehicle) {
+	}
+
+	public void removeVehicle(RaycastVehicle vehicle) {
+	}
+
+	/**
+	 * Once a rigidbody is added to the dynamics world, it will get this gravity assigned.
+	 * Existing rigidbodies in the world get gravity assigned too, during this method.
+	 */
+	public abstract void setGravity(Vector3f gravity);
+
+	public abstract void addRigidBody(RigidBody body);
+
+	public abstract void removeRigidBody(RigidBody body);
+
+	public abstract void setConstraintSolver(ConstraintSolver solver);
+
+	public abstract ConstraintSolver getConstraintSolver();
+
+	public int getNumConstraints() {
+		return 0;
+	}
+
+	public TypedConstraint getConstraint(int index) {
+		return null;
+	}
+
+	public abstract DynamicsWorldType getWorldType();
+
+	public abstract void clearForces();
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/DynamicsWorldType.java b/src/jbullet/src/javabullet/dynamics/DynamicsWorldType.java
new file mode 100644
index 0000000..011977d
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/DynamicsWorldType.java
@@ -0,0 +1,34 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics;
+
+/**
+ *
+ * @author jezek2
+ */
+public enum DynamicsWorldType {
+	SIMPLE_DYNAMICS_WORLD,
+	DISCRETE_DYNAMICS_WORLD,
+	CONTINUOUS_DYNAMICS_WORLD
+}
diff --git a/src/jbullet/src/javabullet/dynamics/RigidBody.java b/src/jbullet/src/javabullet/dynamics/RigidBody.java
new file mode 100644
index 0000000..6e1a632
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/RigidBody.java
@@ -0,0 +1,639 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics;
+
+import java.util.ArrayList;
+import java.util.List;
+import javabullet.BulletGlobals;
+import javabullet.collision.broadphase.BroadphaseProxy;
+import javabullet.collision.dispatch.CollisionFlags;
+import javabullet.collision.dispatch.CollisionObject;
+import javabullet.collision.shapes.CollisionShape;
+import javabullet.dynamics.constraintsolver.TypedConstraint;
+import javabullet.linearmath.MatrixUtil;
+import javabullet.linearmath.MiscUtil;
+import javabullet.linearmath.MotionState;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.TransformUtil;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Quat4f;
+import javax.vecmath.Vector3f;
+
+/**
+ * RigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
+ * It is recommended for performance and memory use to share btCollisionShape objects whenever possible.<p>
+ * 
+ * There are 3 types of rigid bodies:<br>
+ * - A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics.<br>
+ * - B) Fixed objects with zero mass. They are not moving (basically collision objects)<br>
+ * - C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.<p>
+ * 
+ * Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time.<p>
+ * Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects).
+ * 
+ * @author jezek2
+ */
+public class RigidBody extends CollisionObject {
+
+	private static final float MAX_ANGVEL = BulletGlobals.SIMD_HALF_PI;
+	
+	private final Matrix3f invInertiaTensorWorld = new Matrix3f();
+	private final Vector3f linearVelocity = new Vector3f();
+	private final Vector3f angularVelocity = new Vector3f();
+	private float inverseMass;
+	private float angularFactor;
+
+	private final Vector3f gravity = new Vector3f();
+	private final Vector3f invInertiaLocal = new Vector3f();
+	private final Vector3f totalForce = new Vector3f();
+	private final Vector3f totalTorque = new Vector3f();
+	
+	private float linearDamping;
+	private float angularDamping;
+
+	private boolean additionalDamping;
+	private float additionalDampingFactor;
+	private float additionalLinearDampingThresholdSqr;
+	private float additionalAngularDampingThresholdSqr;
+	private float additionalAngularDampingFactor;
+
+	private float linearSleepingThreshold;
+	private float angularSleepingThreshold;
+
+	// optionalMotionState allows to automatic synchronize the world transform for active objects
+	private MotionState optionalMotionState;
+
+	// keep track of typed constraints referencing this rigid body
+	private final List<TypedConstraint> constraintRefs = new ArrayList<TypedConstraint>();
+
+	// for experimental overriding of friction/contact solver func
+	public int contactSolverType;
+	public int frictionSolverType;
+	
+	private static int uniqueId = 0;
+	public int debugBodyId;
+	
+	public RigidBody(RigidBodyConstructionInfo constructionInfo) {
+		setupRigidBody(constructionInfo);
+	}
+
+	public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape) {
+		this(mass, motionState, collisionShape, BulletGlobals.ZERO_VECTOR3);
+	}
+	
+	public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape, Vector3f localInertia) {
+		RigidBodyConstructionInfo cinfo = new RigidBodyConstructionInfo(mass, motionState, collisionShape, localInertia);
+		setupRigidBody(cinfo);
+	}
+	
+	private void setupRigidBody(RigidBodyConstructionInfo constructionInfo) {
+		linearVelocity.set(0f, 0f, 0f);
+		angularVelocity.set(0f, 0f, 0f);
+		angularFactor = 1f;
+		gravity.set(0f, 0f, 0f);
+		totalForce.set(0f, 0f, 0f);
+		totalTorque.set(0f, 0f, 0f);
+		linearDamping = 0f;
+		angularDamping = 0.5f;
+		linearSleepingThreshold = constructionInfo.linearSleepingThreshold;
+		angularSleepingThreshold = constructionInfo.angularSleepingThreshold;
+		optionalMotionState = constructionInfo.motionState;
+		contactSolverType = 0;
+		frictionSolverType = 0;
+		additionalDamping = constructionInfo.additionalDamping;
+
+		additionalLinearDampingThresholdSqr = constructionInfo.additionalLinearDampingThresholdSqr;
+		additionalAngularDampingThresholdSqr = constructionInfo.additionalAngularDampingThresholdSqr;
+		additionalAngularDampingFactor = constructionInfo.additionalAngularDampingFactor;
+
+		if (optionalMotionState != null)
+		{
+			optionalMotionState.getWorldTransform(worldTransform);
+		} else
+		{
+			worldTransform.set(constructionInfo.startWorldTransform);
+		}
+
+		interpolationWorldTransform.set(worldTransform);
+		interpolationLinearVelocity.set(0f, 0f, 0f);
+		interpolationAngularVelocity.set(0f, 0f, 0f);
+
+		// moved to CollisionObject
+		friction = constructionInfo.friction;
+		restitution = constructionInfo.restitution;
+
+		collisionShape = constructionInfo.collisionShape;
+		debugBodyId = uniqueId++;
+
+		// internalOwner is to allow upcasting from collision object to rigid body
+		internalOwner = this;
+
+		setMassProps(constructionInfo.mass, constructionInfo.localInertia);
+		setDamping(constructionInfo.linearDamping, constructionInfo.angularDamping);
+		updateInertiaTensor();
+	}
+	
+	public void destroy() {
+		// No constraints should point to this rigidbody
+		// Remove constraints from the dynamics world before you delete the related rigidbodies. 
+		assert (constraintRefs.size() == 0);
+	}
+
+	public void proceedToTransform(Transform newTrans) {
+		setCenterOfMassTransform(newTrans);
+	}
+	
+	/**
+	 * To keep collision detection and dynamics separate we don't store a rigidbody pointer,
+	 * but a rigidbody is derived from CollisionObject, so we can safely perform an upcast.
+	 */
+	public static RigidBody upcast(CollisionObject colObj) {
+		return (RigidBody) colObj.getInternalOwner();
+	}
+
+	/**
+	 * Continuous collision detection needs prediction.
+	 */
+	public void predictIntegratedTransform(float timeStep, Transform predictedTransform) {
+		TransformUtil.integrateTransform(worldTransform, linearVelocity, angularVelocity, timeStep, predictedTransform);
+	}
+	
+	public void saveKinematicState(float timeStep) {
+		//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
+		if (timeStep != 0f) {
+			//if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
+			if (getMotionState() != null) {
+				getMotionState().getWorldTransform(worldTransform);
+			}
+			//Vector3f linVel = new Vector3f(), angVel = new Vector3f();
+
+			TransformUtil.calculateVelocity(interpolationWorldTransform, worldTransform, timeStep, linearVelocity, angularVelocity);
+			interpolationLinearVelocity.set(linearVelocity);
+			interpolationAngularVelocity.set(angularVelocity);
+			interpolationWorldTransform.set(worldTransform);
+		//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
+		}
+	}
+	
+	public void applyGravity() {
+		if (isStaticOrKinematicObject())
+			return;
+
+		applyCentralForce(gravity);
+	}
+	
+	public void setGravity(Vector3f acceleration) {
+		if (inverseMass != 0f) {
+			gravity.scale(1f / inverseMass, acceleration);
+		}
+	}
+
+	public Vector3f getGravity() {
+		return gravity;
+	}
+
+	public void setDamping(float lin_damping, float ang_damping) {
+		linearDamping = MiscUtil.GEN_clamped(lin_damping, 0f, 1f);
+		angularDamping = MiscUtil.GEN_clamped(ang_damping, 0f, 1f);
+	}
+
+	/**
+	 * Damps the velocity, using the given linearDamping and angularDamping.
+	 */
+	public void applyDamping(float timeStep) {
+		stack.vectors.push();
+		try {
+			linearVelocity.scale(MiscUtil.GEN_clamped((1f - timeStep * linearDamping), 0f, 1f));
+			angularVelocity.scale(MiscUtil.GEN_clamped((1f - timeStep * angularDamping), 0f, 1f));
+
+			if (additionalDamping) {
+				// Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
+				// Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
+				if ((angularVelocity.lengthSquared() < additionalAngularDampingThresholdSqr) &&
+						(linearVelocity.lengthSquared() < additionalLinearDampingThresholdSqr)) {
+					angularVelocity.scale(additionalDampingFactor);
+					linearVelocity.scale(additionalDampingFactor);
+				}
+
+				float speed = linearVelocity.length();
+				if (speed < linearDamping) {
+					float dampVel = 0.005f;
+					if (speed > dampVel) {
+						Vector3f dir = stack.vectors.get(linearVelocity);
+						dir.normalize();
+						dir.scale(dampVel);
+						linearVelocity.sub(dir);
+					}
+					else {
+						linearVelocity.set(0f, 0f, 0f);
+					}
+				}
+
+				float angSpeed = angularVelocity.length();
+				if (angSpeed < angularDamping) {
+					float angDampVel = 0.005f;
+					if (angSpeed > angDampVel) {
+						Vector3f dir = stack.vectors.get(angularVelocity);
+						dir.normalize();
+						dir.scale(angDampVel);
+						angularVelocity.sub(dir);
+					}
+					else {
+						angularVelocity.set(0f, 0f, 0f);
+					}
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void setMassProps(float mass, Vector3f inertia) {
+		if (mass == 0f) {
+			collisionFlags |= CollisionFlags.STATIC_OBJECT;
+			inverseMass = 0f;
+		}
+		else {
+			collisionFlags &= (~CollisionFlags.STATIC_OBJECT);
+			inverseMass = 1f / mass;
+		}
+
+		invInertiaLocal.set(inertia.x != 0f ? 1f / inertia.x : 0f,
+				inertia.y != 0f ? 1f / inertia.y : 0f,
+				inertia.z != 0f ? 1f / inertia.z : 0f);
+	}
+
+	public float getInvMass() {
+		return inverseMass;
+	}
+
+	public Matrix3f getInvInertiaTensorWorld() {
+		return invInertiaTensorWorld;
+	}
+	
+	public void integrateVelocities(float step) {
+		if (isStaticOrKinematicObject()) {
+			return;
+		}
+
+		stack.vectors.push();
+		try {
+			linearVelocity.scaleAdd(inverseMass * step, totalForce, linearVelocity);
+			Vector3f tmp = stack.vectors.get(totalTorque);
+			invInertiaTensorWorld.transform(tmp);
+			angularVelocity.scaleAdd(step, tmp, angularVelocity);
+
+			// clamp angular velocity. collision calculations will fail on higher angular velocities	
+			float angvel = angularVelocity.length();
+			if (angvel * step > MAX_ANGVEL) {
+				angularVelocity.scale((MAX_ANGVEL / step) / angvel);
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void setCenterOfMassTransform(Transform xform) {
+		if (isStaticOrKinematicObject()) {
+			interpolationWorldTransform.set(worldTransform);
+		}
+		else {
+			interpolationWorldTransform.set(xform);
+		}
+		interpolationLinearVelocity.set(getLinearVelocity());
+		interpolationAngularVelocity.set(getAngularVelocity());
+		worldTransform.set(xform);
+		updateInertiaTensor();
+	}
+
+	public void applyCentralForce(Vector3f force) {
+		totalForce.add(force);
+	}
+	
+	public Vector3f getInvInertiaDiagLocal() {
+		return invInertiaLocal;
+	}
+
+	public void setInvInertiaDiagLocal(Vector3f diagInvInertia) {
+		invInertiaLocal.set(diagInvInertia);
+	}
+
+	public void setSleepingThresholds(float linear, float angular) {
+		linearSleepingThreshold = linear;
+		angularSleepingThreshold = angular;
+	}
+
+	public void applyTorque(Vector3f torque) {
+		totalTorque.add(torque);
+	}
+
+	public void applyForce(Vector3f force, Vector3f rel_pos) {
+		stack.vectors.push();
+		try {
+			applyCentralForce(force);
+			Vector3f tmp = stack.vectors.get();
+			tmp.cross(rel_pos, force);
+			applyTorque(tmp);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void applyCentralImpulse(Vector3f impulse) {
+		linearVelocity.scaleAdd(inverseMass, impulse, linearVelocity);
+	}
+	
+	public void applyTorqueImpulse(Vector3f torque) {
+		stack.vectors.push();
+		try {
+			Vector3f tmp = stack.vectors.get(torque);
+			invInertiaTensorWorld.transform(tmp);
+			angularVelocity.add(tmp);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void applyImpulse(Vector3f impulse, Vector3f rel_pos) {
+		stack.vectors.push();
+		try {
+			if (inverseMass != 0f) {
+				applyCentralImpulse(impulse);
+				if (angularFactor != 0f) {
+					Vector3f tmp = stack.vectors.get();
+					tmp.cross(rel_pos, impulse);
+					tmp.scale(angularFactor);
+					applyTorqueImpulse(tmp);
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	/**
+	 * Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position.
+	 */
+	public void internalApplyImpulse(Vector3f linearComponent, Vector3f angularComponent, float impulseMagnitude) {
+		if (inverseMass != 0f) {
+			linearVelocity.scaleAdd(impulseMagnitude, linearComponent, linearVelocity);
+			if (angularFactor != 0f) {
+				angularVelocity.scaleAdd(impulseMagnitude * angularFactor, angularComponent, angularVelocity);
+			}
+		}
+	}
+
+	public void clearForces() {
+		totalForce.set(0f, 0f, 0f);
+		totalTorque.set(0f, 0f, 0f);
+	}
+	
+	public void updateInertiaTensor() {
+		stack.matrices.push();
+		try {
+			Matrix3f mat1 = stack.matrices.get();
+			MatrixUtil.scale(mat1, worldTransform.basis, invInertiaLocal);
+
+			Matrix3f mat2 = stack.matrices.get(worldTransform.basis);
+			mat2.transpose();
+
+			invInertiaTensorWorld.mul(mat1, mat2);
+		}
+		finally {
+			stack.matrices.pop();
+		}
+	}
+	
+	public Vector3f getCenterOfMassPosition() {
+		return worldTransform.origin;
+	}
+
+	public Quat4f getOrientation() {
+		stack.quats.push();
+		try {
+			Quat4f orn = stack.quats.get();
+			MatrixUtil.getRotation(worldTransform.basis, orn);
+			return stack.quats.returning(orn);
+		}
+		finally {
+			stack.quats.pop();
+		}
+	}
+	
+	public Transform getCenterOfMassTransform() {
+		return worldTransform;
+	}
+
+	public Vector3f getLinearVelocity() {
+		return linearVelocity;
+	}
+
+	public Vector3f getAngularVelocity() {
+		return angularVelocity;
+	}
+
+	public void setLinearVelocity(Vector3f lin_vel) {
+		assert (collisionFlags != CollisionFlags.STATIC_OBJECT);
+		linearVelocity.set(lin_vel);
+	}
+
+	public void setAngularVelocity(Vector3f ang_vel) {
+		assert (collisionFlags != CollisionFlags.STATIC_OBJECT);
+		angularVelocity.set(ang_vel);
+	}
+
+	public Vector3f getVelocityInLocalPoint(Vector3f rel_pos) {
+		stack.vectors.push();
+		try {
+			// we also calculate lin/ang velocity for kinematic objects
+			Vector3f vec = stack.vectors.get();
+			vec.cross(angularVelocity, rel_pos);
+			vec.add(linearVelocity);
+			return stack.vectors.returning(vec);
+
+			//for kinematic objects, we could also use use:
+			//		return 	(m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void translate(Vector3f v) {
+		worldTransform.origin.add(v);
+	}
+	
+	public void getAabb(Vector3f aabbMin, Vector3f aabbMax) {
+		getCollisionShape().getAabb(worldTransform, aabbMin, aabbMax);
+	}
+
+	public float computeImpulseDenominator(Vector3f pos, Vector3f normal) {
+		stack.vectors.push();
+		try {
+			Vector3f r0 = stack.vectors.get();
+			r0.sub(pos, getCenterOfMassPosition());
+
+			Vector3f c0 = stack.vectors.get();
+			c0.cross(r0, normal);
+
+			Vector3f tmp = stack.vectors.get();
+			MatrixUtil.transposeTransform(tmp, c0, getInvInertiaTensorWorld());
+
+			Vector3f vec = stack.vectors.get();
+			vec.cross(tmp, r0);
+
+			return inverseMass + normal.dot(vec);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public float computeAngularImpulseDenominator(Vector3f axis) {
+		stack.vectors.push();
+		try {
+			Vector3f vec = stack.vectors.get();
+			MatrixUtil.transposeTransform(vec, axis, getInvInertiaTensorWorld());
+			return axis.dot(vec);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void updateDeactivation(float timeStep) {
+		if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) {
+			return;
+		}
+
+		if ((getLinearVelocity().lengthSquared() < linearSleepingThreshold * linearSleepingThreshold) &&
+				(getAngularVelocity().lengthSquared() < angularSleepingThreshold * angularSleepingThreshold)) {
+			deactivationTime += timeStep;
+		}
+		else {
+			deactivationTime = 0f;
+			setActivationState(0);
+		}
+	}
+
+	public boolean wantsSleeping() {
+		if (getActivationState() == DISABLE_DEACTIVATION) {
+			return false;
+		}
+
+		// disable deactivation
+		if (BulletGlobals.gDisableDeactivation || (BulletGlobals.gDeactivationTime == 0f)) {
+			return false;
+		}
+
+		if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) {
+			return true;
+		}
+
+		if (deactivationTime > BulletGlobals.gDeactivationTime) {
+			return true;
+		}
+		return false;
+	}
+	
+	public BroadphaseProxy getBroadphaseProxy() {
+		return broadphaseHandle;
+	}
+
+	public void setNewBroadphaseProxy(BroadphaseProxy broadphaseProxy) {
+		this.broadphaseHandle = broadphaseProxy;
+	}
+
+	public MotionState getMotionState() {
+		return optionalMotionState;
+	}
+
+	public void setMotionState(MotionState motionState) {
+		this.optionalMotionState = motionState;
+		if (optionalMotionState != null) {
+			motionState.getWorldTransform(worldTransform);
+		}
+	}
+
+	public void setAngularFactor(float angFac) {
+		angularFactor = angFac;
+	}
+
+	public float getAngularFactor() {
+		return angularFactor;
+	}
+
+	/**
+	 * Is this rigidbody added to a CollisionWorld/DynamicsWorld/Broadphase?
+	 */
+	public boolean isInWorld() {
+		return (getBroadphaseProxy() != null);
+	}
+
+	@Override
+	public boolean checkCollideWithOverride(CollisionObject co) {
+		// TODO: change to cast
+		RigidBody otherRb = RigidBody.upcast(co);
+		if (otherRb == null) {
+			return true;
+		}
+
+		for (int i = 0; i < constraintRefs.size(); ++i) {
+			TypedConstraint c = constraintRefs.get(i);
+			if (c.getRigidBodyA() == otherRb || c.getRigidBodyB() == otherRb) {
+				return false;
+			}
+		}
+
+		return true;
+	}
+
+	public void addConstraintRef(TypedConstraint c) {
+		int index = constraintRefs.indexOf(c);
+		if (index == -1) {
+			constraintRefs.add(c);
+		}
+
+		checkCollideWith = true;
+	}
+	
+	public void removeConstraintRef(TypedConstraint c) {
+		constraintRefs.remove(c);
+		checkCollideWith = (constraintRefs.size() > 0);
+	}
+
+	public TypedConstraint getConstraintRef(int index) {
+		return constraintRefs.get(index);
+	}
+
+	public int getNumConstraintRefs() {
+		return constraintRefs.size();
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/RigidBodyConstructionInfo.java b/src/jbullet/src/javabullet/dynamics/RigidBodyConstructionInfo.java
new file mode 100644
index 0000000..420eebc
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/RigidBodyConstructionInfo.java
@@ -0,0 +1,92 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics;
+
+import javabullet.BulletGlobals;
+import javabullet.collision.shapes.CollisionShape;
+import javabullet.linearmath.MotionState;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * RigidBodyConstructionInfo provides information to create a rigid body.<p>
+ * 
+ * Setting mass to zero creates a fixed (non-dynamic) rigid body. For dynamic objects, you can
+ * use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument).<p>
+ * 
+ * You can use the motion state to synchronize the world transform between physics and graphics objects.
+ * And if the motion state is provided, the rigid body will initialize its initial world transform from
+ * the motion state, startWorldTransform is only used when you don't provide a motion state.
+ * 
+ * @author jezek2
+ */
+public class RigidBodyConstructionInfo {
+
+	public float mass;
+
+	/**
+	 * When a motionState is provided, the rigid body will initialize its world transform
+	 * from the motion state. In this case, startWorldTransform is ignored.
+	 */
+	public MotionState motionState;
+	public final Transform startWorldTransform = new Transform();
+
+	public CollisionShape collisionShape;
+	public final Vector3f localInertia = new Vector3f();
+	public float linearDamping = 0f;
+	public float angularDamping = 0f;
+
+	/** Best simulation results when friction is non-zero. */
+	public float friction = 0.5f;
+	/** Best simulation results using zero restitution. */
+	public float restitution = 0f;
+
+	public float linearSleepingThreshold = 0.8f;
+	public float angularSleepingThreshold = 1.0f;
+
+	/**
+	 * Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
+	 * Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics
+	 * system has improved, this should become obsolete.
+	 */
+	public boolean additionalDamping = false;
+	public float additionalDampingFactor = 0.005f;
+	public float additionalLinearDampingThresholdSqr = 0.01f;
+	public float additionalAngularDampingThresholdSqr = 0.01f;
+	public float additionalAngularDampingFactor = 0.01f;
+
+	public RigidBodyConstructionInfo(float mass, MotionState motionState, CollisionShape collisionShape) {
+		this(mass, motionState, collisionShape, BulletGlobals.ZERO_VECTOR3);
+	}
+	
+	public RigidBodyConstructionInfo(float mass, MotionState motionState, CollisionShape collisionShape, Vector3f localInertia) {
+		this.mass = mass;
+		this.motionState = motionState;
+		this.collisionShape = collisionShape;
+		this.localInertia.set(localInertia);
+		
+		startWorldTransform.setIdentity();
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/SimpleDynamicsWorld.java b/src/jbullet/src/javabullet/dynamics/SimpleDynamicsWorld.java
new file mode 100644
index 0000000..33a4ca8
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/SimpleDynamicsWorld.java
@@ -0,0 +1,236 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics;
+
+import java.util.List;
+import javabullet.collision.broadphase.BroadphaseInterface;
+import javabullet.collision.broadphase.Dispatcher;
+import javabullet.collision.broadphase.DispatcherInfo;
+import javabullet.collision.dispatch.CollisionConfiguration;
+import javabullet.collision.dispatch.CollisionDispatcher;
+import javabullet.collision.dispatch.CollisionObject;
+import javabullet.collision.narrowphase.PersistentManifold;
+import javabullet.dynamics.constraintsolver.ConstraintSolver;
+import javabullet.dynamics.constraintsolver.ContactSolverInfo;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * SimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds.
+ * Please use DiscreteDynamicsWorld instead (or ContinuousDynamicsWorld once it is finished).
+ * 
+ * @author jezek2
+ */
+public class SimpleDynamicsWorld extends DynamicsWorld {
+
+	protected ConstraintSolver constraintSolver;
+	protected boolean ownsConstraintSolver;
+	protected final Vector3f gravity = new Vector3f(0f, 0f, -10f);
+	
+	public SimpleDynamicsWorld(Dispatcher dispatcher, BroadphaseInterface pairCache, ConstraintSolver constraintSolver, CollisionConfiguration collisionConfiguration) {
+		super(dispatcher, pairCache, collisionConfiguration);
+		this.constraintSolver = constraintSolver;
+		this.ownsConstraintSolver = false;
+	}
+
+	protected void predictUnconstraintMotion(float timeStep) {
+		for (int i = 0; i < collisionObjects.size(); i++) {
+			CollisionObject colObj = collisionObjects.get(i);
+			RigidBody body = RigidBody.upcast(colObj);
+			if (body != null) {
+				if (!body.isStaticObject()) {
+					if (body.isActive()) {
+						body.applyGravity();
+						body.integrateVelocities(timeStep);
+						body.applyDamping(timeStep);
+						body.predictIntegratedTransform(timeStep, body.getInterpolationWorldTransform());
+					}
+				}
+			}
+		}
+	}
+	
+	protected void integrateTransforms(float timeStep) {
+		stack.transforms.push();
+		try {
+			Transform predictedTrans = stack.transforms.get();
+			for (int i = 0; i < collisionObjects.size(); i++) {
+				CollisionObject colObj = collisionObjects.get(i);
+				RigidBody body = RigidBody.upcast(colObj);
+				if (body != null) {
+					if (body.isActive() && (!body.isStaticObject())) {
+						body.predictIntegratedTransform(timeStep, predictedTrans);
+						body.proceedToTransform(predictedTrans);
+					}
+				}
+			}
+		}
+		finally {
+			stack.transforms.pop();
+		}
+	}
+	
+	/**
+	 * maxSubSteps/fixedTimeStep for interpolation is currently ignored for SimpleDynamicsWorld, use DiscreteDynamicsWorld instead.
+	 */
+	@Override
+	public int stepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) {
+		// apply gravity, predict motion
+		predictUnconstraintMotion(timeStep);
+
+		DispatcherInfo dispatchInfo = getDispatchInfo();
+		dispatchInfo.timeStep = timeStep;
+		dispatchInfo.stepCount = 0;
+		dispatchInfo.debugDraw = getDebugDrawer();
+
+		// perform collision detection
+		performDiscreteCollisionDetection();
+
+		// solve contact constraints
+		int numManifolds = dispatcher1.getNumManifolds();
+		if (numManifolds != 0)
+		{
+			List<PersistentManifold> manifoldPtr = ((CollisionDispatcher)dispatcher1).getInternalManifoldPointer();
+
+			ContactSolverInfo infoGlobal = new ContactSolverInfo();
+			infoGlobal.timeStep = timeStep;
+			constraintSolver.prepareSolve(0,numManifolds);
+			constraintSolver.solveGroup(null,0,manifoldPtr, 0, numManifolds, null,0,0,infoGlobal,debugDrawer/*, m_stackAlloc*/,dispatcher1);
+			constraintSolver.allSolved(infoGlobal,debugDrawer/*, m_stackAlloc*/);
+		}
+
+		// integrate transforms
+		integrateTransforms(timeStep);
+
+		updateAabbs();
+
+		synchronizeMotionStates();
+
+		clearForces();
+
+		return 1;
+	}
+
+	@Override
+	public void clearForces() {
+		// todo: iterate over awake simulation islands!
+		for (int i = 0; i < collisionObjects.size(); i++) {
+			CollisionObject colObj = collisionObjects.get(i);
+
+			RigidBody body = RigidBody.upcast(colObj);
+			if (body != null) {
+				body.clearForces();
+			}
+		}
+	}
+
+	@Override
+	public void setGravity(Vector3f gravity) {
+		this.gravity.set(gravity);
+		for (int i = 0; i < collisionObjects.size(); i++) {
+			CollisionObject colObj = collisionObjects.get(i);
+			RigidBody body = RigidBody.upcast(colObj);
+			if (body != null) {
+				body.setGravity(gravity);
+			}
+		}
+	}
+
+	@Override
+	public void addRigidBody(RigidBody body) {
+		body.setGravity(gravity);
+
+		if (body.getCollisionShape() != null) {
+			addCollisionObject(body);
+		}
+	}
+
+	@Override
+	public void removeRigidBody(RigidBody body) {
+		removeCollisionObject(body);
+	}
+
+	@Override
+	public void updateAabbs() {
+		stack.pushCommonMath();
+		try {
+			Transform predictedTrans = stack.transforms.get();
+			Vector3f minAabb = stack.vectors.get(), maxAabb = stack.vectors.get();
+
+			for (int i = 0; i < collisionObjects.size(); i++) {
+				CollisionObject colObj = collisionObjects.get(i);
+				RigidBody body = RigidBody.upcast(colObj);
+				if (body != null) {
+					if (body.isActive() && (!body.isStaticObject())) {
+						colObj.getCollisionShape().getAabb(colObj.getWorldTransform(), minAabb, maxAabb);
+						BroadphaseInterface bp = getBroadphase();
+						bp.setAabb(body.getBroadphaseHandle(), minAabb, maxAabb, dispatcher1);
+					}
+				}
+			}
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	public void synchronizeMotionStates() {
+		// todo: iterate over awake simulation islands!
+		for (int i = 0; i < collisionObjects.size(); i++) {
+			CollisionObject colObj = collisionObjects.get(i);
+			RigidBody body = RigidBody.upcast(colObj);
+			if (body != null && body.getMotionState() != null) {
+				if (body.getActivationState() != CollisionObject.ISLAND_SLEEPING) {
+					body.getMotionState().setWorldTransform(body.getWorldTransform());
+				}
+			}
+		}
+	}
+
+	@Override
+	public void setConstraintSolver(ConstraintSolver solver) {
+		if (ownsConstraintSolver) {
+			//btAlignedFree(m_constraintSolver);
+		}
+
+		ownsConstraintSolver = false;
+		constraintSolver = solver;
+	}
+
+	@Override
+	public ConstraintSolver getConstraintSolver() {
+		return constraintSolver;
+	}
+	
+	@Override
+	public void debugDrawWorld() {
+		// TODO: throw new UnsupportedOperationException("Not supported yet.");
+	}
+
+	@Override
+	public DynamicsWorldType getWorldType() {
+		throw new UnsupportedOperationException("Not supported yet.");
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ConeTwistConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ConeTwistConstraint.java
new file mode 100644
index 0000000..4bf81c1
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ConeTwistConstraint.java
@@ -0,0 +1,419 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ * 
+ * Written by: Marcus Hennix
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+import javabullet.BulletGlobals;
+import javabullet.dynamics.RigidBody;
+import javabullet.linearmath.QuaternionUtil;
+import javabullet.linearmath.ScalarUtil;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.TransformUtil;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Quat4f;
+import javax.vecmath.Vector3f;
+
+/**
+ * ConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
+ * 
+ * @author jezek2
+ */
+public class ConeTwistConstraint extends TypedConstraint {
+
+	private JacobianEntry[] jac/*[3]*/ = new JacobianEntry[] { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() }; //3 orthogonal linear constraints
+
+	private final Transform rbAFrame = new Transform();
+	private final Transform rbBFrame = new Transform();
+
+	private float limitSoftness;
+	private float biasFactor;
+	private float relaxationFactor;
+
+	private float swingSpan1;
+	private float swingSpan2;
+	private float twistSpan;
+
+	private final Vector3f swingAxis = new Vector3f();
+	private final Vector3f twistAxis = new Vector3f();
+
+	private float kSwing;
+	private float kTwist;
+
+	private float twistLimitSign;
+	private float swingCorrection;
+	private float twistCorrection;
+
+	private float accSwingLimitImpulse;
+	private float accTwistLimitImpulse;
+
+	private boolean angularOnly = false;
+	private boolean solveTwistLimit;
+	private boolean solveSwingLimit;
+	
+	public ConeTwistConstraint() {
+		super(TypedConstraintType.CONETWIST_CONSTRAINT_TYPE);
+	}
+
+	public ConeTwistConstraint(RigidBody rbA, RigidBody rbB, Transform rbAFrame, Transform rbBFrame) {
+		super(TypedConstraintType.CONETWIST_CONSTRAINT_TYPE, rbA, rbB);
+		this.rbAFrame.set(rbAFrame);
+		this.rbBFrame.set(rbBFrame);
+
+		// flip axis for correct angles
+		this.rbBFrame.basis.m10 *= -1f;
+		this.rbBFrame.basis.m11 *= -1f;
+		this.rbBFrame.basis.m12 *= -1f;
+
+		swingSpan1 = 1e30f;
+		swingSpan2 = 1e30f;
+		twistSpan = 1e30f;
+		biasFactor = 0.3f;
+		relaxationFactor = 1.0f;
+
+		solveTwistLimit = false;
+		solveSwingLimit = false;
+	}
+
+	public ConeTwistConstraint(RigidBody rbA, Transform rbAFrame) {
+		super(TypedConstraintType.CONETWIST_CONSTRAINT_TYPE, rbA);
+		this.rbAFrame.set(rbAFrame);
+		this.rbBFrame.set(this.rbAFrame);
+
+		// flip axis for correct angles
+		this.rbBFrame.basis.m10 *= -1f;
+		this.rbBFrame.basis.m11 *= -1f;
+		this.rbBFrame.basis.m12 *= -1f;
+
+		this.rbBFrame.basis.m20 *= -1f;
+		this.rbBFrame.basis.m21 *= -1f;
+		this.rbBFrame.basis.m22 *= -1f;
+
+		swingSpan1 = 1e30f;
+		swingSpan2 = 1e30f;
+		twistSpan = 1e30f;
+		biasFactor = 0.3f;
+		relaxationFactor = 1.0f;
+
+		solveTwistLimit = false;
+		solveSwingLimit = false;
+	}
+	
+	@Override
+	public void buildJacobian() {
+		stack.pushCommonMath();
+		stack.quats.push();
+		try {
+			Vector3f tmp = stack.vectors.get();
+			Vector3f tmp1 = stack.vectors.get();
+			Vector3f tmp2 = stack.vectors.get();
+
+			appliedImpulse = 0f;
+
+			// set bias, sign, clear accumulator
+			swingCorrection = 0f;
+			twistLimitSign = 0f;
+			solveTwistLimit = false;
+			solveSwingLimit = false;
+			accTwistLimitImpulse = 0f;
+			accSwingLimitImpulse = 0f;
+
+			if (!angularOnly) {
+				Vector3f pivotAInW = stack.vectors.get(rbAFrame.origin);
+				rbA.getCenterOfMassTransform().transform(pivotAInW);
+
+				Vector3f pivotBInW = stack.vectors.get(rbBFrame.origin);
+				rbB.getCenterOfMassTransform().transform(pivotBInW);
+
+				Vector3f relPos = stack.vectors.get();
+				relPos.sub(pivotBInW, pivotAInW);
+
+				// TODO: stack
+				Vector3f[] normal/*[3]*/ = new Vector3f[]{stack.vectors.get(), stack.vectors.get(), stack.vectors.get()};
+				if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) {
+					normal[0].normalize(relPos);
+				}
+				else {
+					normal[0].set(1f, 0f, 0f);
+				}
+
+				TransformUtil.planeSpace1(normal[0], normal[1], normal[2]);
+
+				for (int i = 0; i < 3; i++) {
+					Matrix3f mat1 = stack.matrices.get(rbA.getCenterOfMassTransform().basis);
+					mat1.transpose();
+
+					Matrix3f mat2 = stack.matrices.get(rbB.getCenterOfMassTransform().basis);
+					mat2.transpose();
+
+					tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition());
+					tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition());
+
+					jac[i].init(
+							mat1,
+							mat2,
+							tmp1,
+							tmp2,
+							normal[i],
+							rbA.getInvInertiaDiagLocal(),
+							rbA.getInvMass(),
+							rbB.getInvInertiaDiagLocal(),
+							rbB.getInvMass());
+				}
+			}
+
+			Vector3f b1Axis1 = stack.vectors.get(), b1Axis2 = stack.vectors.get(), b1Axis3 = stack.vectors.get();
+			Vector3f b2Axis1 = stack.vectors.get(), b2Axis2 = stack.vectors.get();
+
+			rbAFrame.basis.getColumn(0, b1Axis1);
+			getRigidBodyA().getCenterOfMassTransform().basis.transform(b1Axis1);
+
+			rbBFrame.basis.getColumn(0, b2Axis1);
+			getRigidBodyB().getCenterOfMassTransform().basis.transform(b2Axis1);
+
+			float swing1 = 0f, swing2 = 0f;
+
+			// Get Frame into world space
+			if (swingSpan1 >= 0.05f) {
+				rbAFrame.basis.getColumn(1, b1Axis2);
+				getRigidBodyA().getCenterOfMassTransform().basis.transform(b1Axis2);
+				swing1 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis2), b2Axis1.dot(b1Axis1));
+			}
+
+			if (swingSpan2 >= 0.05f) {
+				rbAFrame.basis.getColumn(2, b1Axis3);
+				getRigidBodyA().getCenterOfMassTransform().basis.transform(b1Axis3);
+				swing2 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis3), b2Axis1.dot(b1Axis1));
+			}
+
+			float RMaxAngle1Sq = 1.0f / (swingSpan1 * swingSpan1);
+			float RMaxAngle2Sq = 1.0f / (swingSpan2 * swingSpan2);
+			float EllipseAngle = Math.abs(swing1) * RMaxAngle1Sq + Math.abs(swing2) * RMaxAngle2Sq;
+
+			if (EllipseAngle > 1.0f) {
+				swingCorrection = EllipseAngle - 1.0f;
+				solveSwingLimit = true;
+
+				// Calculate necessary axis & factors
+				tmp1.scale(b2Axis1.dot(b1Axis2), b1Axis2);
+				tmp2.scale(b2Axis1.dot(b1Axis3), b1Axis3);
+				tmp.add(tmp1, tmp2);
+				swingAxis.cross(b2Axis1, tmp);
+				swingAxis.normalize();
+
+				float swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
+				swingAxis.scale(swingAxisSign);
+
+				kSwing = 1f / (getRigidBodyA().computeAngularImpulseDenominator(swingAxis) +
+						getRigidBodyB().computeAngularImpulseDenominator(swingAxis));
+
+			}
+
+			// Twist limits
+			if (twistSpan >= 0f) {
+				//Vector3f b2Axis2 = stack.vectors.get();
+				rbBFrame.basis.getColumn(1, b2Axis2);
+				getRigidBodyB().getCenterOfMassTransform().basis.transform(b2Axis2);
+
+				Quat4f rotationArc = stack.quats.get(QuaternionUtil.shortestArcQuat(b2Axis1, b1Axis1));
+				Vector3f TwistRef = stack.vectors.get(QuaternionUtil.quatRotate(rotationArc, b2Axis2));
+				float twist = ScalarUtil.atan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2));
+
+				float lockedFreeFactor = (twistSpan > 0.05f) ? limitSoftness : 0f;
+				if (twist <= -twistSpan * lockedFreeFactor) {
+					twistCorrection = -(twist + twistSpan);
+					solveTwistLimit = true;
+
+					twistAxis.add(b2Axis1, b1Axis1);
+					twistAxis.scale(0.5f);
+					twistAxis.normalize();
+					twistAxis.scale(-1.0f);
+
+					kTwist = 1f / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis) +
+							getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
+
+				}
+				else if (twist > twistSpan * lockedFreeFactor) {
+					twistCorrection = (twist - twistSpan);
+					solveTwistLimit = true;
+
+					twistAxis.add(b2Axis1, b1Axis1);
+					twistAxis.scale(0.5f);
+					twistAxis.normalize();
+
+					kTwist = 1f / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis) +
+							getRigidBodyB().computeAngularImpulseDenominator(twistAxis));
+				}
+			}
+		}
+		finally {
+			stack.popCommonMath();
+			stack.quats.pop();
+		}
+	}
+
+	@Override
+	public void solveConstraint(float timeStep) {
+		stack.vectors.push();
+		try {
+			Vector3f tmp = stack.vectors.get();
+			Vector3f tmp2 = stack.vectors.get();
+
+			Vector3f pivotAInW = stack.vectors.get(rbAFrame.origin);
+			rbA.getCenterOfMassTransform().transform(pivotAInW);
+
+			Vector3f pivotBInW = stack.vectors.get(rbBFrame.origin);
+			rbB.getCenterOfMassTransform().transform(pivotBInW);
+
+			float tau = 0.3f;
+
+			// linear part
+			if (!angularOnly) {
+				Vector3f rel_pos1 = stack.vectors.get();
+				rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition());
+
+				Vector3f rel_pos2 = stack.vectors.get();
+				rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition());
+
+				Vector3f vel1 = stack.vectors.get(rbA.getVelocityInLocalPoint(rel_pos1));
+				Vector3f vel2 = stack.vectors.get(rbB.getVelocityInLocalPoint(rel_pos2));
+				Vector3f vel = stack.vectors.get();
+				vel.sub(vel1, vel2);
+
+				for (int i = 0; i < 3; i++) {
+					Vector3f normal = jac[i].linearJointAxis;
+					float jacDiagABInv = 1f / jac[i].getDiagonal();
+
+					float rel_vel;
+					rel_vel = normal.dot(vel);
+					// positional error (zeroth order error)
+					tmp.sub(pivotAInW, pivotBInW);
+					float depth = -(tmp).dot(normal); // this is the error projected on the normal
+					float impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
+					appliedImpulse += impulse;
+					Vector3f impulse_vector = stack.vectors.get();
+					impulse_vector.scale(impulse, normal);
+
+					tmp.sub(pivotAInW, rbA.getCenterOfMassPosition());
+					rbA.applyImpulse(impulse_vector, tmp);
+
+					tmp.negate(impulse_vector);
+					tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition());
+					rbB.applyImpulse(tmp, tmp2);
+				}
+			}
+
+			{
+				// solve angular part
+				Vector3f angVelA = getRigidBodyA().getAngularVelocity();
+				Vector3f angVelB = getRigidBodyB().getAngularVelocity();
+
+				// solve swing limit
+				if (solveSwingLimit) {
+					tmp.sub(angVelB, angVelA);
+					float amplitude = ((tmp).dot(swingAxis) * relaxationFactor * relaxationFactor + swingCorrection * (1f / timeStep) * biasFactor);
+					float impulseMag = amplitude * kSwing;
+
+					// Clamp the accumulated impulse
+					float temp = accSwingLimitImpulse;
+					accSwingLimitImpulse = Math.max(accSwingLimitImpulse + impulseMag, 0.0f);
+					impulseMag = accSwingLimitImpulse - temp;
+
+					Vector3f impulse = stack.vectors.get();
+					impulse.scale(impulseMag, swingAxis);
+
+					rbA.applyTorqueImpulse(impulse);
+
+					tmp.negate(impulse);
+					rbB.applyTorqueImpulse(tmp);
+				}
+
+				// solve twist limit
+				if (solveTwistLimit) {
+					tmp.sub(angVelB, angVelA);
+					float amplitude = ((tmp).dot(twistAxis) * relaxationFactor * relaxationFactor + twistCorrection * (1f / timeStep) * biasFactor);
+					float impulseMag = amplitude * kTwist;
+
+					// Clamp the accumulated impulse
+					float temp = accTwistLimitImpulse;
+					accTwistLimitImpulse = Math.max(accTwistLimitImpulse + impulseMag, 0.0f);
+					impulseMag = accTwistLimitImpulse - temp;
+
+					Vector3f impulse = stack.vectors.get();
+					impulse.scale(impulseMag, twistAxis);
+
+					rbA.applyTorqueImpulse(impulse);
+
+					tmp.negate(impulse);
+					rbB.applyTorqueImpulse(tmp);
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void updateRHS(float timeStep) {
+	}
+
+	public void setAngularOnly(boolean angularOnly) {
+		this.angularOnly = angularOnly;
+	}
+
+	public void setLimit(float _swingSpan1, float _swingSpan2, float _twistSpan) {
+		setLimit(_swingSpan1, _swingSpan2, _twistSpan, 0.8f, 0.3f, 1.0f);
+	}
+
+	public void setLimit(float _swingSpan1, float _swingSpan2, float _twistSpan, float _softness, float _biasFactor, float _relaxationFactor) {
+		swingSpan1 = _swingSpan1;
+		swingSpan2 = _swingSpan2;
+		twistSpan = _twistSpan;
+
+		limitSoftness = _softness;
+		biasFactor = _biasFactor;
+		relaxationFactor = _relaxationFactor;
+	}
+
+	public Transform getAFrame() {
+		return rbAFrame;
+	}
+
+	public Transform getBFrame() {
+		return rbBFrame;
+	}
+
+	public boolean getSolveTwistLimit() {
+		return solveTwistLimit;
+	}
+
+	public boolean getSolveSwingLimit() {
+		return solveTwistLimit;
+	}
+
+	public float getTwistLimitSign() {
+		return twistLimitSign;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ConstraintPersistentData.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ConstraintPersistentData.java
new file mode 100644
index 0000000..b8e7220
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ConstraintPersistentData.java
@@ -0,0 +1,90 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class ConstraintPersistentData {
+	
+	/** total applied impulse during most recent frame */
+	public float appliedImpulse = 0f;
+	public float prevAppliedImpulse = 0f;
+	public float accumulatedTangentImpulse0 = 0f;
+	public float accumulatedTangentImpulse1 = 0f;
+
+	public float jacDiagABInv = 0f;
+	public float jacDiagABInvTangent0;
+	public float jacDiagABInvTangent1;
+	public int persistentLifeTime = 0;
+	public float restitution = 0f;
+	public float friction = 0f;
+	public float penetration = 0f;
+	public final Vector3f frictionWorldTangential0 = new Vector3f();
+	public final Vector3f frictionWorldTangential1 = new Vector3f();
+
+	public final Vector3f frictionAngularComponent0A = new Vector3f();
+	public final Vector3f frictionAngularComponent0B = new Vector3f();
+	public final Vector3f frictionAngularComponent1A = new Vector3f();
+	public final Vector3f frictionAngularComponent1B = new Vector3f();
+
+	//some data doesn't need to be persistent over frames: todo: clean/reuse this
+	public final Vector3f angularComponentA = new Vector3f();
+	public final Vector3f angularComponentB = new Vector3f();
+
+	public ContactSolverFunc contactSolverFunc = null;
+	public ContactSolverFunc frictionSolverFunc = null;
+	
+	public void reset() {
+		appliedImpulse = 0f;
+		prevAppliedImpulse = 0f;
+		accumulatedTangentImpulse0 = 0f;
+		accumulatedTangentImpulse1 = 0f;
+
+		jacDiagABInv = 0f;
+		jacDiagABInvTangent0 = 0f;
+		jacDiagABInvTangent1 = 0f;
+		persistentLifeTime = 0;
+		restitution = 0f;
+		friction = 0f;
+		penetration = 0f;
+		frictionWorldTangential0.set(0f, 0f, 0f);
+		frictionWorldTangential1.set(0f, 0f, 0f);
+
+		frictionAngularComponent0A.set(0f, 0f, 0f);
+		frictionAngularComponent0B.set(0f, 0f, 0f);
+		frictionAngularComponent1A.set(0f, 0f, 0f);
+		frictionAngularComponent1B.set(0f, 0f, 0f);
+
+		angularComponentA.set(0f, 0f, 0f);
+		angularComponentB.set(0f, 0f, 0f);
+
+		contactSolverFunc = null;
+		frictionSolverFunc = null;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ConstraintSolver.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ConstraintSolver.java
new file mode 100644
index 0000000..84062c3
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ConstraintSolver.java
@@ -0,0 +1,55 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+import java.util.List;
+import javabullet.BulletStack;
+import javabullet.collision.broadphase.Dispatcher;
+import javabullet.collision.dispatch.CollisionObject;
+import javabullet.collision.narrowphase.PersistentManifold;
+import javabullet.linearmath.IDebugDraw;
+
+/**
+ *
+ * @author jezek2
+ */
+public abstract class ConstraintSolver {
+	
+	protected final BulletStack stack = BulletStack.get();
+
+	public void prepareSolve (int numBodies, int numManifolds) {}
+
+	/**
+	 * Solve a group of constraints.
+	 */
+	public abstract float solveGroup(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifold, int manifold_offset, int numManifolds, List<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo info, IDebugDraw debugDrawer/*, btStackAlloc* stackAlloc*/, Dispatcher dispatcher);
+
+	public void allSolved(ContactSolverInfo info, IDebugDraw debugDrawer/*, btStackAlloc* stackAlloc*/) {}
+
+	/**
+	 * Clear internal cached data and reset random seed.
+	 */
+	public abstract void reset();
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactConstraint.java
new file mode 100644
index 0000000..ea07897
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactConstraint.java
@@ -0,0 +1,460 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletPool;
+import javabullet.BulletStack;
+import javabullet.ObjectPool;
+import javabullet.collision.narrowphase.ManifoldPoint;
+import javabullet.dynamics.RigidBody;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class ContactConstraint {
+	
+	public static final ContactSolverFunc resolveSingleCollision = new ContactSolverFunc() {
+		public float invoke(RigidBody body1, RigidBody body2, ManifoldPoint contactPoint, ContactSolverInfo info) {
+			return resolveSingleCollision(body1, body2, contactPoint, info);
+		}
+	};
+
+	public static final ContactSolverFunc resolveSingleFriction = new ContactSolverFunc() {
+		public float invoke(RigidBody body1, RigidBody body2, ManifoldPoint contactPoint, ContactSolverInfo info) {
+			return resolveSingleFriction(body1, body2, contactPoint, info);
+		}
+	};
+
+	public static final ContactSolverFunc resolveSingleCollisionCombined = new ContactSolverFunc() {
+		public float invoke(RigidBody body1, RigidBody body2, ManifoldPoint contactPoint, ContactSolverInfo info) {
+			return resolveSingleCollisionCombined(body1, body2, contactPoint, info);
+		}
+	};
+	
+	/**
+	 * Bilateral constraint between two dynamic objects.
+	 */
+	public static void resolveSingleBilateral(RigidBody body1, Vector3f pos1,
+			RigidBody body2, Vector3f pos2,
+			float distance, Vector3f normal, float[] impulse, float timeStep) {
+		float normalLenSqr = normal.lengthSquared();
+		assert (Math.abs(normalLenSqr) < 1.1f);
+		if (normalLenSqr > 1.1f) {
+			impulse[0] = 0f;
+			return;
+		}
+
+		BulletStack stack = BulletStack.get();
+		ObjectPool<JacobianEntry> jacobiansPool = BulletPool.get(JacobianEntry.class);
+		
+		stack.pushCommonMath();
+		try {
+			Vector3f rel_pos1 = stack.vectors.get();
+			rel_pos1.sub(pos1, body1.getCenterOfMassPosition());
+
+			Vector3f rel_pos2 = stack.vectors.get();
+			rel_pos2.sub(pos2, body2.getCenterOfMassPosition());
+
+			//this jacobian entry could be re-used for all iterations
+
+			Vector3f vel1 = stack.vectors.get();
+			vel1.set(body1.getVelocityInLocalPoint(rel_pos1));
+
+			Vector3f vel2 = stack.vectors.get();
+			vel2.set(body2.getVelocityInLocalPoint(rel_pos2));
+
+			Vector3f vel = stack.vectors.get();
+			vel.sub(vel1, vel2);
+
+			Matrix3f mat1 = stack.matrices.get(body1.getCenterOfMassTransform().basis);
+			mat1.transpose();
+
+			Matrix3f mat2 = stack.matrices.get(body2.getCenterOfMassTransform().basis);
+			mat2.transpose();
+
+			JacobianEntry jac = jacobiansPool.get();
+			jac.init(mat1, mat2,
+					rel_pos1, rel_pos2, normal, body1.getInvInertiaDiagLocal(), body1.getInvMass(),
+					body2.getInvInertiaDiagLocal(), body2.getInvMass());
+
+			float jacDiagAB = jac.getDiagonal();
+			float jacDiagABInv = 1f / jacDiagAB;
+
+			Vector3f tmp1 = stack.vectors.get(body1.getAngularVelocity());
+			mat1.transform(tmp1);
+
+			Vector3f tmp2 = stack.vectors.get(body2.getAngularVelocity());
+			mat2.transform(tmp2);
+
+			float rel_vel = jac.getRelativeVelocity(
+					body1.getLinearVelocity(),
+					tmp1,
+					body2.getLinearVelocity(),
+					tmp2);
+			
+			jacobiansPool.release(jac);
+			
+			float a;
+			a = jacDiagABInv;
+
+
+			rel_vel = normal.dot(vel);
+
+			// todo: move this into proper structure
+			float contactDamping = 0.2f;
+
+			//#ifdef ONLY_USE_LINEAR_MASS
+			//	btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
+			//	impulse = - contactDamping * rel_vel * massTerm;
+			//#else	
+			float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
+			impulse[0] = velocityImpulse;
+			//#endif
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	/**
+	 * Response between two dynamic objects with friction.
+	 */
+	public static float resolveSingleCollision(
+			RigidBody body1,
+			RigidBody body2,
+			ManifoldPoint contactPoint,
+			ContactSolverInfo solverInfo) {
+
+		BulletStack stack = BulletStack.get();
+		
+		stack.vectors.push();
+		try {
+			Vector3f pos1_ = contactPoint.getPositionWorldOnA();
+			Vector3f pos2_ = contactPoint.getPositionWorldOnB();
+			Vector3f normal = contactPoint.normalWorldOnB;
+
+			// constant over all iterations
+			Vector3f rel_pos1 = stack.vectors.get();
+			rel_pos1.sub(pos1_, body1.getCenterOfMassPosition());
+
+			Vector3f rel_pos2 = stack.vectors.get();
+			rel_pos2.sub(pos2_, body2.getCenterOfMassPosition());
+
+			Vector3f vel1 = stack.vectors.get(body1.getVelocityInLocalPoint(rel_pos1));
+			Vector3f vel2 = stack.vectors.get(body2.getVelocityInLocalPoint(rel_pos2));
+			Vector3f vel = stack.vectors.get();
+			vel.sub(vel1, vel2);
+
+			float rel_vel;
+			rel_vel = normal.dot(vel);
+
+			float Kfps = 1f / solverInfo.timeStep;
+
+			// btScalar damping = solverInfo.m_damping ;
+			float Kerp = solverInfo.erp;
+			float Kcor = Kerp * Kfps;
+
+			ConstraintPersistentData cpd = (ConstraintPersistentData) contactPoint.userPersistentData;
+			assert (cpd != null);
+			float distance = cpd.penetration;
+			float positionalError = Kcor * -distance;
+			float velocityError = cpd.restitution - rel_vel; // * damping;
+
+			float penetrationImpulse = positionalError * cpd.jacDiagABInv;
+
+			float velocityImpulse = velocityError * cpd.jacDiagABInv;
+
+			float normalImpulse = penetrationImpulse + velocityImpulse;
+
+			// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+			float oldNormalImpulse = cpd.appliedImpulse;
+			float sum = oldNormalImpulse + normalImpulse;
+			cpd.appliedImpulse = 0f > sum ? 0f : sum;
+
+			normalImpulse = cpd.appliedImpulse - oldNormalImpulse;
+
+			//#ifdef USE_INTERNAL_APPLY_IMPULSE
+			Vector3f tmp = stack.vectors.get();
+			if (body1.getInvMass() != 0f) {
+				tmp.scale(body1.getInvMass(), contactPoint.normalWorldOnB);
+				body1.internalApplyImpulse(tmp, cpd.angularComponentA, normalImpulse);
+			}
+			if (body2.getInvMass() != 0f) {
+				tmp.scale(body2.getInvMass(), contactPoint.normalWorldOnB);
+				body2.internalApplyImpulse(tmp, cpd.angularComponentB, -normalImpulse);
+			}
+			//#else //USE_INTERNAL_APPLY_IMPULSE
+			//	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
+			//	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
+			//#endif //USE_INTERNAL_APPLY_IMPULSE
+
+			return normalImpulse;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public static float resolveSingleFriction(
+			RigidBody body1,
+			RigidBody body2,
+			ManifoldPoint contactPoint,
+			ContactSolverInfo solverInfo) {
+		
+		BulletStack stack = BulletStack.get();
+		
+		stack.vectors.push();
+		try {
+			Vector3f pos1 = contactPoint.getPositionWorldOnA();
+			Vector3f pos2 = contactPoint.getPositionWorldOnB();
+
+			Vector3f rel_pos1 = stack.vectors.get();
+			rel_pos1.sub(pos1, body1.getCenterOfMassPosition());
+
+			Vector3f rel_pos2 = stack.vectors.get();
+			rel_pos2.sub(pos2, body2.getCenterOfMassPosition());
+
+			ConstraintPersistentData cpd = (ConstraintPersistentData) contactPoint.userPersistentData;
+			assert (cpd != null);
+
+			float combinedFriction = cpd.friction;
+
+			float limit = cpd.appliedImpulse * combinedFriction;
+
+			if (cpd.appliedImpulse > 0f) //friction
+			{
+				//apply friction in the 2 tangential directions
+
+				// 1st tangent
+				Vector3f vel1 = stack.vectors.get();
+				vel1.set(body1.getVelocityInLocalPoint(rel_pos1));
+
+				Vector3f vel2 = stack.vectors.get();
+				vel2.set(body2.getVelocityInLocalPoint(rel_pos2));
+
+				Vector3f vel = stack.vectors.get();
+				vel.sub(vel1, vel2);
+
+				float j1, j2;
+
+				{
+					float vrel = cpd.frictionWorldTangential0.dot(vel);
+
+					// calculate j that moves us to zero relative velocity
+					j1 = -vrel * cpd.jacDiagABInvTangent0;
+					float oldTangentImpulse = cpd.accumulatedTangentImpulse0;
+					cpd.accumulatedTangentImpulse0 = oldTangentImpulse + j1;
+
+					cpd.accumulatedTangentImpulse0 = Math.min(cpd.accumulatedTangentImpulse0, limit);
+					cpd.accumulatedTangentImpulse0 = Math.max(cpd.accumulatedTangentImpulse0, -limit);
+					j1 = cpd.accumulatedTangentImpulse0 - oldTangentImpulse;
+				}
+				{
+					// 2nd tangent
+
+					float vrel = cpd.frictionWorldTangential1.dot(vel);
+
+					// calculate j that moves us to zero relative velocity
+					j2 = -vrel * cpd.jacDiagABInvTangent1;
+					float oldTangentImpulse = cpd.accumulatedTangentImpulse1;
+					cpd.accumulatedTangentImpulse1 = oldTangentImpulse + j2;
+
+					cpd.accumulatedTangentImpulse1 = Math.min(cpd.accumulatedTangentImpulse1, limit);
+					cpd.accumulatedTangentImpulse1 = Math.max(cpd.accumulatedTangentImpulse1, -limit);
+					j2 = cpd.accumulatedTangentImpulse1 - oldTangentImpulse;
+				}
+
+				//#ifdef USE_INTERNAL_APPLY_IMPULSE
+				Vector3f tmp = stack.vectors.get();
+
+				if (body1.getInvMass() != 0f) {
+					tmp.scale(body1.getInvMass(), cpd.frictionWorldTangential0);
+					body1.internalApplyImpulse(tmp, cpd.frictionAngularComponent0A, j1);
+
+					tmp.scale(body1.getInvMass(), cpd.frictionWorldTangential1);
+					body1.internalApplyImpulse(tmp, cpd.frictionAngularComponent1A, j2);
+				}
+				if (body2.getInvMass() != 0f) {
+					tmp.scale(body2.getInvMass(), cpd.frictionWorldTangential0);
+					body2.internalApplyImpulse(tmp, cpd.frictionAngularComponent0B, -j1);
+
+					tmp.scale(body2.getInvMass(), cpd.frictionWorldTangential1);
+					body2.internalApplyImpulse(tmp, cpd.frictionAngularComponent1B, -j2);
+				}
+				//#else //USE_INTERNAL_APPLY_IMPULSE
+				//	body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1);
+				//	body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2);
+				//#endif //USE_INTERNAL_APPLY_IMPULSE
+			}
+			return cpd.appliedImpulse;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	/**
+	 * velocity + friction<br>
+	 * response between two dynamic objects with friction
+	 */
+	public static float resolveSingleCollisionCombined(
+			RigidBody body1,
+			RigidBody body2,
+			ManifoldPoint contactPoint,
+			ContactSolverInfo solverInfo) {
+		
+		BulletStack stack = BulletStack.get();
+		
+		stack.vectors.push();
+		try {
+			Vector3f pos1 = contactPoint.getPositionWorldOnA();
+			Vector3f pos2 = contactPoint.getPositionWorldOnB();
+			Vector3f normal = contactPoint.normalWorldOnB;
+
+			Vector3f rel_pos1 = stack.vectors.get();
+			rel_pos1.sub(pos1, body1.getCenterOfMassPosition());
+
+			Vector3f rel_pos2 = stack.vectors.get();
+			rel_pos2.sub(pos2, body2.getCenterOfMassPosition());
+
+			Vector3f vel1 = stack.vectors.get(body1.getVelocityInLocalPoint(rel_pos1));
+			Vector3f vel2 = stack.vectors.get(body2.getVelocityInLocalPoint(rel_pos2));
+			Vector3f vel = stack.vectors.get();
+			vel.sub(vel1, vel2);
+
+			float rel_vel;
+			rel_vel = normal.dot(vel);
+
+			float Kfps = 1f / solverInfo.timeStep;
+
+			//btScalar damping = solverInfo.m_damping ;
+			float Kerp = solverInfo.erp;
+			float Kcor = Kerp * Kfps;
+
+			ConstraintPersistentData cpd = (ConstraintPersistentData) contactPoint.userPersistentData;
+			assert (cpd != null);
+			float distance = cpd.penetration;
+			float positionalError = Kcor * -distance;
+			float velocityError = cpd.restitution - rel_vel;// * damping;
+
+			float penetrationImpulse = positionalError * cpd.jacDiagABInv;
+
+			float velocityImpulse = velocityError * cpd.jacDiagABInv;
+
+			float normalImpulse = penetrationImpulse + velocityImpulse;
+
+			// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+			float oldNormalImpulse = cpd.appliedImpulse;
+			float sum = oldNormalImpulse + normalImpulse;
+			cpd.appliedImpulse = 0f > sum ? 0f : sum;
+
+			normalImpulse = cpd.appliedImpulse - oldNormalImpulse;
+
+
+			//#ifdef USE_INTERNAL_APPLY_IMPULSE
+			Vector3f tmp = stack.vectors.get();
+			if (body1.getInvMass() != 0f) {
+				tmp.scale(body1.getInvMass(), contactPoint.normalWorldOnB);
+				body1.internalApplyImpulse(tmp, cpd.angularComponentA, normalImpulse);
+			}
+			if (body2.getInvMass() != 0f) {
+				tmp.scale(body2.getInvMass(), contactPoint.normalWorldOnB);
+				body2.internalApplyImpulse(tmp, cpd.angularComponentB, -normalImpulse);
+			}
+			//#else //USE_INTERNAL_APPLY_IMPULSE
+			//	body1.applyImpulse(normal*(normalImpulse), rel_pos1);
+			//	body2.applyImpulse(-normal*(normalImpulse), rel_pos2);
+			//#endif //USE_INTERNAL_APPLY_IMPULSE
+
+			{
+				//friction
+				vel1.set(body1.getVelocityInLocalPoint(rel_pos1));
+				vel2.set(body2.getVelocityInLocalPoint(rel_pos2));
+				vel.sub(vel1, vel2);
+
+				rel_vel = normal.dot(vel);
+
+				tmp.scale(rel_vel, normal);
+				Vector3f lat_vel = stack.vectors.get();
+				lat_vel.sub(vel, tmp);
+				float lat_rel_vel = lat_vel.length();
+
+				float combinedFriction = cpd.friction;
+
+				if (cpd.appliedImpulse > 0) {
+					if (lat_rel_vel > BulletGlobals.FLT_EPSILON) {
+						lat_vel.scale(1f / lat_rel_vel);
+
+						Vector3f temp1 = stack.vectors.get();
+						temp1.cross(rel_pos1, lat_vel);
+						body1.getInvInertiaTensorWorld().transform(temp1);
+
+						Vector3f temp2 = stack.vectors.get();
+						temp2.cross(rel_pos2, lat_vel);
+						body2.getInvInertiaTensorWorld().transform(temp2);
+
+						Vector3f java_tmp1 = stack.vectors.get();
+						java_tmp1.cross(temp1, rel_pos1);
+
+						Vector3f java_tmp2 = stack.vectors.get();
+						java_tmp2.cross(temp2, rel_pos2);
+
+						tmp.add(java_tmp1, java_tmp2);
+
+						float friction_impulse = lat_rel_vel /
+								(body1.getInvMass() + body2.getInvMass() + lat_vel.dot(tmp));
+						float normal_impulse = cpd.appliedImpulse * combinedFriction;
+
+						friction_impulse = Math.min(friction_impulse, normal_impulse);
+						friction_impulse = Math.max(friction_impulse, -normal_impulse);
+
+						tmp.scale(-friction_impulse, lat_vel);
+						body1.applyImpulse(tmp, rel_pos1);
+
+						tmp.scale(friction_impulse, lat_vel);
+						body2.applyImpulse(tmp, rel_pos2);
+					}
+				}
+			}
+
+			return normalImpulse;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public static float resolveSingleFrictionEmpty(
+			RigidBody body1,
+			RigidBody body2,
+			ManifoldPoint contactPoint,
+			ContactSolverInfo solverInfo) {
+		return 0f;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactConstraintEnum.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactConstraintEnum.java
new file mode 100644
index 0000000..3382637
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactConstraintEnum.java
@@ -0,0 +1,37 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+/**
+ * TODO: name
+ * 
+ * @author jezek2
+ */
+public enum ContactConstraintEnum {
+	DEFAULT_CONTACT_SOLVER_TYPE,
+	CONTACT_SOLVER_TYPE1,
+	CONTACT_SOLVER_TYPE2,
+	USER_CONTACT_SOLVER_TYPE1,
+	MAX_CONTACT_SOLVER_TYPES
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactSolverFunc.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactSolverFunc.java
new file mode 100644
index 0000000..97799e1
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactSolverFunc.java
@@ -0,0 +1,37 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+import javabullet.collision.narrowphase.ManifoldPoint;
+import javabullet.dynamics.RigidBody;
+
+/**
+ *
+ * @author jezek2
+ */
+public interface ContactSolverFunc {
+
+	public float invoke(RigidBody body1, RigidBody body2, ManifoldPoint contactPoint, ContactSolverInfo info);
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactSolverInfo.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactSolverInfo.java
new file mode 100644
index 0000000..7915516
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactSolverInfo.java
@@ -0,0 +1,57 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+/**
+ *
+ * @author jezek2
+ */
+public class ContactSolverInfo {
+
+	public float tau = 0.6f;
+	public float damping = 1f;
+	public float friction = 0.3f;
+	public float timeStep;
+	public float restitution = 0f;
+	public int numIterations = 10;
+	public float maxErrorReduction = 20f;
+	public float sor = 1.3f;
+	public float erp = 0.4f;
+
+	public ContactSolverInfo() {
+	}
+	
+	public ContactSolverInfo(ContactSolverInfo g) {
+		tau = g.tau;
+		damping = g.damping;
+		friction = g.friction;
+		timeStep = g.timeStep;
+		restitution = g.restitution;
+		numIterations = g.numIterations;
+		maxErrorReduction = g.maxErrorReduction;
+		sor = g.sor;
+		erp = g.erp;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/Generic6DofConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/Generic6DofConstraint.java
new file mode 100644
index 0000000..46e9b93
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/Generic6DofConstraint.java
@@ -0,0 +1,505 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+/*
+2007-09-09
+btGeneric6DofConstraint Refactored by Francisco Le�n
+email: [email protected]
+http://gimpact.sf.net
+*/
+
+package javabullet.dynamics.constraintsolver;
+
+import javabullet.BulletGlobals;
+import javabullet.dynamics.RigidBody;
+import javabullet.linearmath.MatrixUtil;
+import javabullet.linearmath.Transform;
+
+
+/// 
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Vector3f;
+/*!
+
+*/
+/**
+ * Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space.<p>
+ * Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'.
+ * Currently this limit supports rotational motors.<br>
+ * <ul>
+ * <li>For Linear limits, use Generic6DofConstraint.setLinearUpperLimit, Generic6DofConstraint.setLinearLowerLimit. You can set the parameters with the TranslationalLimitMotor structure accsesible through the Generic6DofConstraint.getTranslationalLimitMotor method.
+ * At this moment translational motors are not supported. May be in the future.</li>
+ * 
+ * <li>For Angular limits, use the RotationalLimitMotor structure for configuring the limit.
+ * This is accessible through Generic6DofConstraint.getLimitMotor method,
+ * This brings support for limit parameters and motors.</li>
+ * 
+ * <li>Angulars limits have these possible ranges:
+ * <table border=1 >
+ * <tr>
+ * 	<td><b>AXIS</b></td>
+ * 	<td><b>MIN ANGLE</b></td>
+ * 	<td><b>MAX ANGLE</b></td>
+ * </tr><tr>
+ * 	<td>X</td>
+ * 		<td>-PI</td>
+ * 		<td>PI</td>
+ * </tr><tr>
+ * 	<td>Y</td>
+ * 		<td>-PI/2</td>
+ * 		<td>PI/2</td>
+ * </tr><tr>
+ * 	<td>Z</td>
+ * 		<td>-PI/2</td>
+ * 		<td>PI/2</td>
+ * </tr>
+ * </table>
+ * </li>
+ * </ul>
+ * 
+ * @author jezek2
+ */
+public class Generic6DofConstraint extends TypedConstraint {
+
+	protected final Transform frameInA = new Transform(); //!< the constraint space w.r.t body A
+    protected final Transform frameInB = new Transform(); //!< the constraint space w.r.t body B
+
+	protected final JacobianEntry[] jacLinear/*[3]*/ = new JacobianEntry[] { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() }; //!< 3 orthogonal linear constraints
+    protected final JacobianEntry[] jacAng/*[3]*/ = new JacobianEntry[] { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() }; //!< 3 orthogonal angular constraints
+
+	protected final TranslationalLimitMotor linearLimits = new TranslationalLimitMotor();
+
+	protected final RotationalLimitMotor[] angularLimits/*[3]*/ = new RotationalLimitMotor[] { new RotationalLimitMotor(), new RotationalLimitMotor(), new RotationalLimitMotor() };
+
+	protected float timeStep;
+    protected final Transform calculatedTransformA = new Transform();
+    protected final Transform calculatedTransformB = new Transform();
+    protected final Vector3f calculatedAxisAngleDiff = new Vector3f();
+    protected final Vector3f[] calculatedAxis/*[3]*/ = new Vector3f[] { new Vector3f(), new Vector3f(), new Vector3f() };
+    
+    protected boolean useLinearReferenceFrameA;
+
+	public Generic6DofConstraint() {
+		super(TypedConstraintType.D6_CONSTRAINT_TYPE);
+		useLinearReferenceFrameA = true;
+	}
+
+	public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Transform frameInA, Transform frameInB, boolean useLinearReferenceFrameA) {
+		super(TypedConstraintType.D6_CONSTRAINT_TYPE, rbA, rbB);
+		this.frameInA.set(frameInA);
+		this.frameInB.set(frameInB);
+		this.useLinearReferenceFrameA = useLinearReferenceFrameA;
+	}
+
+	private static float getMatrixElem(Matrix3f mat, int index) {
+		int i = index % 3;
+		int j = index / 3;
+		return mat.getElement(i, j);
+	}
+	
+	/**
+	 * MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
+	 */
+	private static boolean matrixToEulerXYZ(Matrix3f mat, Vector3f xyz) {
+		//	// rot =  cy*cz          -cy*sz           sy
+		//	//        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+		//	//       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+		//
+
+		if (getMatrixElem(mat, 2) < 1.0f) {
+			if (getMatrixElem(mat, 2) > -1.0f) {
+				xyz.x = (float) Math.atan2(-getMatrixElem(mat, 5), getMatrixElem(mat, 8));
+				xyz.y = (float) Math.asin(getMatrixElem(mat, 2));
+				xyz.z = (float) Math.atan2(-getMatrixElem(mat, 1), getMatrixElem(mat, 0));
+				return true;
+			}
+			else {
+				// WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+				xyz.x = -(float) Math.atan2(getMatrixElem(mat, 3), getMatrixElem(mat, 4));
+				xyz.y = -BulletGlobals.SIMD_HALF_PI;
+				xyz.z = 0.0f;
+				return false;
+			}
+		}
+		else {
+			// WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+			xyz.x = (float) Math.atan2(getMatrixElem(mat, 3), getMatrixElem(mat, 4));
+			xyz.y = BulletGlobals.SIMD_HALF_PI;
+			xyz.z = 0.0f;
+		}
+
+		return false;
+	}
+
+	/**
+	 * Calcs the euler angles between the two bodies.
+	 */
+	protected void calculateAngleInfo() {
+		stack.pushCommonMath();
+		try {
+			Matrix3f mat = stack.matrices.get();
+
+			Matrix3f relative_frame = stack.matrices.get();
+			mat.set(calculatedTransformA.basis);
+			MatrixUtil.invert(mat);
+			relative_frame.mul(mat, calculatedTransformB.basis);
+
+			matrixToEulerXYZ(relative_frame, calculatedAxisAngleDiff);
+
+			// in euler angle mode we do not actually constrain the angular velocity
+			// along the axes axis[0] and axis[2] (although we do use axis[1]) :
+			//
+			//    to get			constrain w2-w1 along		...not
+			//    ------			---------------------		------
+			//    d(angle[0])/dt = 0	ax[1] x ax[2]			ax[0]
+			//    d(angle[1])/dt = 0	ax[1]
+			//    d(angle[2])/dt = 0	ax[0] x ax[1]			ax[2]
+			//
+			// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
+			// to prove the result for angle[0], write the expression for angle[0] from
+			// GetInfo1 then take the derivative. to prove this for angle[2] it is
+			// easier to take the euler rate expression for d(angle[2])/dt with respect
+			// to the components of w and set that to 0.
+
+			Vector3f axis0 = stack.vectors.get();
+			calculatedTransformB.basis.getColumn(0, axis0);
+
+			Vector3f axis2 = stack.vectors.get();
+			calculatedTransformA.basis.getColumn(2, axis2);
+
+			calculatedAxis[1].cross(axis2, axis0);
+			calculatedAxis[0].cross(calculatedAxis[1], axis2);
+			calculatedAxis[2].cross(axis0, calculatedAxis[1]);
+
+			//    if(m_debugDrawer)
+			//    {
+			//
+			//    	char buff[300];
+			//		sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
+			//		m_calculatedAxisAngleDiff[0],
+			//		m_calculatedAxisAngleDiff[1],
+			//		m_calculatedAxisAngleDiff[2]);
+			//    	m_debugDrawer->reportErrorWarning(buff);
+			//    }
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	/**
+	 * Calcs global transform of the offsets.<p>
+	 * Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
+	 * 
+	 * See also: Generic6DofConstraint.getCalculatedTransformA, Generic6DofConstraint.getCalculatedTransformB, Generic6DofConstraint.calculateAngleInfo
+	 */
+	public void calculateTransforms() {
+		calculatedTransformA.set(rbA.getCenterOfMassTransform());
+		calculatedTransformA.mul(frameInA);
+
+		calculatedTransformB.set(rbB.getCenterOfMassTransform());
+		calculatedTransformB.mul(frameInB);
+
+		calculateAngleInfo();
+	}
+	
+	protected void buildLinearJacobian(/*JacobianEntry jacLinear*/int jacLinear_index, Vector3f normalWorld, Vector3f pivotAInW, Vector3f pivotBInW) {
+		stack.pushCommonMath();
+		try {
+			Matrix3f mat1 = stack.matrices.get(rbA.getCenterOfMassTransform().basis);
+			mat1.transpose();
+
+			Matrix3f mat2 = stack.matrices.get(rbB.getCenterOfMassTransform().basis);
+			mat2.transpose();
+
+			Vector3f tmp1 = stack.vectors.get();
+			tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition());
+
+			Vector3f tmp2 = stack.vectors.get();
+			tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition());
+
+			jacLinear[jacLinear_index].init(
+					mat1,
+					mat2,
+					tmp1,
+					tmp2,
+					normalWorld,
+					rbA.getInvInertiaDiagLocal(),
+					rbA.getInvMass(),
+					rbB.getInvInertiaDiagLocal(),
+					rbB.getInvMass());
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	protected void buildAngularJacobian(/*JacobianEntry jacAngular*/int jacAngular_index, Vector3f jointAxisW) {
+		stack.matrices.push();
+		try {
+			Matrix3f mat1 = stack.matrices.get(rbA.getCenterOfMassTransform().basis);
+			mat1.transpose();
+
+			Matrix3f mat2 = stack.matrices.get(rbB.getCenterOfMassTransform().basis);
+			mat2.transpose();
+
+			jacAng[jacAngular_index].init(jointAxisW,
+					mat1,
+					mat2,
+					rbA.getInvInertiaDiagLocal(),
+					rbB.getInvInertiaDiagLocal());
+		}
+		finally {
+			stack.matrices.pop();
+		}
+	}
+
+	/**
+	 * Test angular limit.<p>
+	 * Calculates angular correction and returns true if limit needs to be corrected.
+	 * Generic6DofConstraint.buildJacobian must be called previously.
+	 */
+	public boolean testAngularLimitMotor(int axis_index) {
+		float angle = VectorUtil.getCoord(calculatedAxisAngleDiff, axis_index);
+
+		// test limits
+		angularLimits[axis_index].testLimitValue(angle);
+		return angularLimits[axis_index].needApplyTorques();
+	}
+	
+	@Override
+	public void buildJacobian() {
+		stack.vectors.push();
+		try {
+			// calculates transform
+			calculateTransforms();
+
+			Vector3f pivotAInW = calculatedTransformA.origin;
+			Vector3f pivotBInW = calculatedTransformB.origin;
+
+			Vector3f rel_pos1 = stack.vectors.get();
+			rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition());
+
+			Vector3f rel_pos2 = stack.vectors.get();
+			rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition());
+
+			Vector3f normalWorld = stack.vectors.get();
+			int i;
+			// linear part
+			for (i = 0; i < 3; i++) {
+				if (linearLimits.isLimited(i)) {
+					if (useLinearReferenceFrameA) {
+						calculatedTransformA.basis.getColumn(i, normalWorld);
+					}
+					else {
+						calculatedTransformB.basis.getColumn(i, normalWorld);
+					}
+
+					buildLinearJacobian(
+							/*jacLinear[i]*/i, normalWorld,
+							pivotAInW, pivotBInW);
+
+				}
+			}
+
+			// angular part
+			for (i = 0; i < 3; i++) {
+				// calculates error angle
+				if (testAngularLimitMotor(i)) {
+					normalWorld.set(this.getAxis(i));
+					// Create angular atom
+					buildAngularJacobian(/*jacAng[i]*/i, normalWorld);
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void solveConstraint(float timeStep) {
+		stack.vectors.push();
+		try {
+			this.timeStep = timeStep;
+
+			//calculateTransforms();
+
+			int i;
+
+			// linear
+
+			Vector3f pointInA = stack.vectors.get(calculatedTransformA.origin);
+			Vector3f pointInB = stack.vectors.get(calculatedTransformB.origin);
+
+			float jacDiagABInv;
+			Vector3f linear_axis = stack.vectors.get();
+			for (i = 0; i < 3; i++) {
+				if (linearLimits.isLimited(i)) {
+					jacDiagABInv = 1f / jacLinear[i].getDiagonal();
+
+					if (useLinearReferenceFrameA) {
+						calculatedTransformA.basis.getColumn(i, linear_axis);
+					}
+					else {
+						calculatedTransformB.basis.getColumn(i, linear_axis);
+					}
+
+					linearLimits.solveLinearAxis(
+							this.timeStep,
+							jacDiagABInv,
+							rbA, pointInA,
+							rbB, pointInB,
+							i, linear_axis);
+
+				}
+			}
+
+			// angular
+			Vector3f angular_axis = stack.vectors.get();
+			float angularJacDiagABInv;
+			for (i = 0; i < 3; i++) {
+				if (angularLimits[i].needApplyTorques()) {
+					// get axis
+					angular_axis.set(getAxis(i));
+
+					angularJacDiagABInv = 1f / jacAng[i].getDiagonal();
+
+					angularLimits[i].solveAngularLimits(this.timeStep, angular_axis, angularJacDiagABInv, rbA, rbB);
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+
+    public void updateRHS(float timeStep) {
+	}
+
+	/**
+	 * Get the rotation axis in global coordinates.
+	 * Generic6DofConstraint.buildJacobian must be called previously.
+	 */
+	public Vector3f getAxis(int axis_index) {
+		return calculatedAxis[axis_index];
+	}
+
+	/**
+	 * Get the relative Euler angle.
+	 * Generic6DofConstraint.buildJacobian must be called previously.
+	 */
+	public float getAngle(int axis_index) {
+		return VectorUtil.getCoord(calculatedAxisAngleDiff, axis_index);
+	}
+
+	/**
+	 * Gets the global transform of the offset for body A.<p>
+	 * See also: Generic6DofConstraint.getFrameOffsetA, Generic6DofConstraint.getFrameOffsetB, Generic6DofConstraint.calculateAngleInfo.
+	 */
+	public Transform getCalculatedTransformA() {
+		return calculatedTransformA;
+	}
+
+	/**
+	 * Gets the global transform of the offset for body B.<p>
+	 * See also: Generic6DofConstraint.getFrameOffsetA, Generic6DofConstraint.getFrameOffsetB, Generic6DofConstraint.calculateAngleInfo.
+	 */
+	public Transform getCalculatedTransformB() {
+		return calculatedTransformB;
+	}
+
+	public Transform getFrameOffsetA() {
+		return frameInA;
+	}
+
+	public Transform getFrameOffsetB() {
+		return frameInB;
+	}
+	
+	public void setLinearLowerLimit(Vector3f linearLower) {
+		linearLimits.lowerLimit.set(linearLower);
+	}
+
+	public void setLinearUpperLimit(Vector3f linearUpper) {
+		linearLimits.upperLimit.set(linearUpper);
+	}
+
+	public void setAngularLowerLimit(Vector3f angularLower) {
+		angularLimits[0].loLimit = angularLower.x;
+		angularLimits[1].loLimit = angularLower.y;
+		angularLimits[2].loLimit = angularLower.z;
+	}
+
+	public void setAngularUpperLimit(Vector3f angularUpper) {
+		angularLimits[0].hiLimit = angularUpper.x;
+		angularLimits[1].hiLimit = angularUpper.y;
+		angularLimits[2].hiLimit = angularUpper.z;
+	}
+
+	/**
+	 * Retrieves the angular limit informacion.
+	 */
+	public RotationalLimitMotor getRotationalLimitMotor(int index) {
+		return angularLimits[index];
+	}
+
+	/**
+	 * Retrieves the limit informacion.
+	 */
+	public TranslationalLimitMotor getTranslationalLimitMotor() {
+		return linearLimits;
+	}
+
+	/**
+	 * first 3 are linear, next 3 are angular
+	 */
+	public void setLimit(int axis, float lo, float hi) {
+		if (axis < 3) {
+			VectorUtil.setCoord(linearLimits.lowerLimit, axis, lo);
+			VectorUtil.setCoord(linearLimits.upperLimit, axis, hi);
+		}
+		else {
+			angularLimits[axis - 3].loLimit = lo;
+			angularLimits[axis - 3].hiLimit = hi;
+		}
+	}
+	
+	/**
+	 * Test limit.<p>
+	 * - free means upper &lt; lower,<br>
+	 * - locked means upper == lower<br>
+	 * - limited means upper &gt; lower<br>
+	 * - limitIndex: first 3 are linear, next 3 are angular
+	 */
+	public boolean isLimited(int limitIndex) {
+		if (limitIndex < 3) {
+			return linearLimits.isLimited(limitIndex);
+
+		}
+		return angularLimits[limitIndex - 3].isLimited();
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/HingeConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/HingeConstraint.java
new file mode 100644
index 0000000..a514c3e
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/HingeConstraint.java
@@ -0,0 +1,619 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */
+
+package javabullet.dynamics.constraintsolver;
+
+import javabullet.BulletGlobals;
+import javabullet.dynamics.RigidBody;
+import javabullet.linearmath.QuaternionUtil;
+import javabullet.linearmath.ScalarUtil;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.TransformUtil;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Quat4f;
+import javax.vecmath.Vector3f;
+
+/**
+ * Hinge constraint between two rigidbodies each with a pivotpoint that descibes
+ * the axis location in local space. Axis defines the orientation of the hinge axis.
+ * 
+ * @author jezek2
+ */
+public class HingeConstraint extends TypedConstraint {
+
+	private JacobianEntry[] jac/*[3]*/ = new JacobianEntry[] { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() }; // 3 orthogonal linear constraints
+	private JacobianEntry[] jacAng/*[3]*/ = new JacobianEntry[] { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() }; // 2 orthogonal angular constraints+ 1 for limit/motor
+
+	private final Transform rbAFrame = new Transform(); // constraint axii. Assumes z is hinge axis.
+	private final Transform rbBFrame = new Transform();
+
+	private float motorTargetVelocity;
+	private float maxMotorImpulse;
+
+	private float limitSoftness; 
+	private float biasFactor; 
+	private float relaxationFactor; 
+
+	private float lowerLimit;	
+	private float upperLimit;	
+	
+	private float kHinge;
+
+	private float limitSign;
+	private float correction;
+
+	private float accLimitImpulse;
+
+	private boolean angularOnly;
+	private boolean enableAngularMotor;
+	private boolean solveLimit;
+
+	public HingeConstraint() {
+		super(TypedConstraintType.HINGE_CONSTRAINT_TYPE);
+		enableAngularMotor = false;
+	}
+
+	public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3f pivotInA, Vector3f pivotInB, Vector3f axisInA, Vector3f axisInB) {
+		super(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA, rbB);
+		angularOnly = false;
+		enableAngularMotor = false;
+
+		stack.vectors.push();
+		stack.quats.push();
+		try {
+			rbAFrame.origin.set(pivotInA);
+
+			// since no frame is given, assume this to be zero angle and just pick rb transform axis
+			Vector3f rbAxisA1 = stack.vectors.get();
+			rbA.getCenterOfMassTransform().basis.getColumn(0, rbAxisA1);
+
+			float projection = rbAxisA1.dot(axisInA);
+			if (projection > BulletGlobals.FLT_EPSILON) {
+				rbAxisA1.scale(projection);
+				rbAxisA1.sub(axisInA);
+			}
+			else {
+				rbA.getCenterOfMassTransform().basis.getColumn(1, rbAxisA1);
+			}
+
+			Vector3f rbAxisA2 = stack.vectors.get();
+			rbAxisA2.cross(rbAxisA1, axisInA);
+
+			rbAFrame.basis.setRow(0, rbAxisA1.x, rbAxisA2.x, axisInA.x);
+			rbAFrame.basis.setRow(1, rbAxisA1.y, rbAxisA2.y, axisInA.y);
+			rbAFrame.basis.setRow(2, rbAxisA1.z, rbAxisA2.z, axisInA.z);
+
+			Quat4f rotationArc = stack.quats.get(QuaternionUtil.shortestArcQuat(axisInA, axisInB));
+			Vector3f rbAxisB1 = stack.vectors.get(QuaternionUtil.quatRotate(rotationArc, rbAxisA1));
+			Vector3f rbAxisB2 = stack.vectors.get();
+			rbAxisB2.cross(rbAxisB1, axisInB);
+
+			rbBFrame.origin.set(pivotInB);
+			rbBFrame.basis.setRow(0, rbAxisB1.x, rbAxisB2.x, -axisInB.x);
+			rbBFrame.basis.setRow(1, rbAxisB1.y, rbAxisB2.y, -axisInB.y);
+			rbBFrame.basis.setRow(2, rbAxisB1.z, rbAxisB2.z, -axisInB.z);
+
+			// start with free
+			lowerLimit = 1e30f;
+			upperLimit = -1e30f;
+			biasFactor = 0.3f;
+			relaxationFactor = 1.0f;
+			limitSoftness = 0.9f;
+			solveLimit = false;
+		}
+		finally {
+			stack.vectors.pop();
+			stack.quats.pop();
+		}
+	}
+
+	public HingeConstraint(RigidBody rbA, Vector3f pivotInA, Vector3f axisInA) {
+		super(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA);
+		angularOnly = false;
+		enableAngularMotor = false;
+
+		stack.vectors.push();
+		stack.quats.push();
+		try {
+			// since no frame is given, assume this to be zero angle and just pick rb transform axis
+			// fixed axis in worldspace
+			Vector3f rbAxisA1 = stack.vectors.get();
+			rbA.getCenterOfMassTransform().basis.getColumn(0, rbAxisA1);
+
+			float projection = rbAxisA1.dot(axisInA);
+			if (projection > BulletGlobals.FLT_EPSILON) {
+				rbAxisA1.scale(projection);
+				rbAxisA1.sub(axisInA);
+			}
+			else {
+				rbA.getCenterOfMassTransform().basis.getColumn(1, rbAxisA1);
+			}
+
+			Vector3f rbAxisA2 = stack.vectors.get();
+			rbAxisA2.cross(axisInA, rbAxisA1);
+
+			rbAFrame.origin.set(pivotInA);
+			rbAFrame.basis.setRow(0, rbAxisA1.x, rbAxisA2.x, axisInA.x);
+			rbAFrame.basis.setRow(1, rbAxisA1.y, rbAxisA2.y, axisInA.y);
+			rbAFrame.basis.setRow(2, rbAxisA1.z, rbAxisA2.z, axisInA.z);
+
+			Vector3f axisInB = stack.vectors.get();
+			axisInB.negate(axisInA);
+			rbA.getCenterOfMassTransform().basis.transform(axisInB);
+
+			Quat4f rotationArc = stack.quats.get(QuaternionUtil.shortestArcQuat(axisInA, axisInB));
+			Vector3f rbAxisB1 = stack.vectors.get(QuaternionUtil.quatRotate(rotationArc, rbAxisA1));
+			Vector3f rbAxisB2 = stack.vectors.get();
+			rbAxisB2.cross(axisInB, rbAxisB1);
+
+			rbBFrame.origin.set(pivotInA);
+			rbA.getCenterOfMassTransform().transform(rbBFrame.origin);
+			rbBFrame.basis.setRow(0, rbAxisB1.x, rbAxisB2.x, axisInB.x);
+			rbBFrame.basis.setRow(1, rbAxisB1.y, rbAxisB2.y, axisInB.y);
+			rbBFrame.basis.setRow(2, rbAxisB1.z, rbAxisB2.z, axisInB.z);
+
+			// start with free
+			lowerLimit = 1e30f;
+			upperLimit = -1e30f;
+			biasFactor = 0.3f;
+			relaxationFactor = 1.0f;
+			limitSoftness = 0.9f;
+			solveLimit = false;
+		}
+		finally {
+			stack.vectors.pop();
+			stack.quats.pop();
+		}
+	}
+
+	public HingeConstraint(RigidBody rbA, RigidBody rbB, Transform rbAFrame, Transform rbBFrame) {
+		super(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA, rbB);
+		this.rbAFrame.set(rbAFrame);
+		this.rbBFrame.set(rbBFrame);
+		angularOnly = false;
+		enableAngularMotor = false;
+
+		// flip axis
+		this.rbBFrame.basis.m02 *= -1f;
+		this.rbBFrame.basis.m12 *= -1f;
+		this.rbBFrame.basis.m22 *= -1f;
+
+		// start with free
+		lowerLimit = 1e30f;
+		upperLimit = -1e30f;
+		biasFactor = 0.3f;
+		relaxationFactor = 1.0f;
+		limitSoftness = 0.9f;
+		solveLimit = false;
+	}
+
+	public HingeConstraint(RigidBody rbA, Transform rbAFrame) {
+		super(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA);
+		this.rbAFrame.set(rbAFrame);
+		this.rbBFrame.set(rbAFrame);
+		angularOnly = false;
+		enableAngularMotor = false;
+
+		// not providing rigidbody B means implicitly using worldspace for body B
+
+		// flip axis
+		this.rbBFrame.basis.m02 *= -1f;
+		this.rbBFrame.basis.m12 *= -1f;
+		this.rbBFrame.basis.m22 *= -1f;
+
+		this.rbBFrame.origin.set(this.rbAFrame.origin);
+		rbA.getCenterOfMassTransform().transform(this.rbBFrame.origin);
+
+		// start with free
+		lowerLimit = 1e30f;
+		upperLimit = -1e30f;
+		biasFactor = 0.3f;
+		relaxationFactor = 1.0f;
+		limitSoftness = 0.9f;
+		solveLimit = false;
+	}
+	
+	@Override
+	public void buildJacobian() {
+		stack.pushCommonMath();
+		try {
+			Vector3f tmp = stack.vectors.get();
+			Vector3f tmp1 = stack.vectors.get();
+			Vector3f tmp2 = stack.vectors.get();
+			Matrix3f mat1 = stack.matrices.get();
+			Matrix3f mat2 = stack.matrices.get();
+
+			appliedImpulse = 0f;
+
+			if (!angularOnly) {
+				Vector3f pivotAInW = stack.vectors.get(rbAFrame.origin);
+				rbA.getCenterOfMassTransform().transform(pivotAInW);
+
+				Vector3f pivotBInW = stack.vectors.get(rbBFrame.origin);
+				rbB.getCenterOfMassTransform().transform(pivotBInW);
+
+				Vector3f relPos = stack.vectors.get();
+				relPos.sub(pivotBInW, pivotAInW);
+
+				Vector3f[] normal/*[3]*/ = new Vector3f[]{stack.vectors.get(), stack.vectors.get(), stack.vectors.get()};
+				if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) {
+					normal[0].set(relPos);
+					normal[0].normalize();
+				}
+				else {
+					normal[0].set(1f, 0f, 0f);
+				}
+
+				TransformUtil.planeSpace1(normal[0], normal[1], normal[2]);
+
+				for (int i = 0; i < 3; i++) {
+					mat1.transpose(rbA.getCenterOfMassTransform().basis);
+					mat2.transpose(rbB.getCenterOfMassTransform().basis);
+
+					tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition());
+					tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition());
+
+					jac[i].init(
+							mat1,
+							mat2,
+							tmp1,
+							tmp2,
+							normal[i],
+							rbA.getInvInertiaDiagLocal(),
+							rbA.getInvMass(),
+							rbB.getInvInertiaDiagLocal(),
+							rbB.getInvMass());
+				}
+			}
+
+			// calculate two perpendicular jointAxis, orthogonal to hingeAxis
+			// these two jointAxis require equal angular velocities for both bodies
+
+			// this is unused for now, it's a todo
+			Vector3f jointAxis0local = stack.vectors.get();
+			Vector3f jointAxis1local = stack.vectors.get();
+
+			rbAFrame.basis.getColumn(2, tmp);
+			TransformUtil.planeSpace1(tmp, jointAxis0local, jointAxis1local);
+
+			// TODO: check this
+			//getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
+
+			Vector3f jointAxis0 = stack.vectors.get(jointAxis0local);
+			getRigidBodyA().getCenterOfMassTransform().basis.transform(jointAxis0);
+
+			Vector3f jointAxis1 = stack.vectors.get(jointAxis1local);
+			getRigidBodyA().getCenterOfMassTransform().basis.transform(jointAxis1);
+
+			Vector3f hingeAxisWorld = stack.vectors.get();
+			rbAFrame.basis.getColumn(2, hingeAxisWorld);
+			getRigidBodyA().getCenterOfMassTransform().basis.transform(hingeAxisWorld);
+
+			mat1.transpose(rbA.getCenterOfMassTransform().basis);
+			mat2.transpose(rbB.getCenterOfMassTransform().basis);
+			jacAng[0].init(jointAxis0,
+					mat1,
+					mat2,
+					rbA.getInvInertiaDiagLocal(),
+					rbB.getInvInertiaDiagLocal());
+
+			// JAVA NOTE: reused mat1 and mat2, as recomputation is not needed
+			jacAng[1].init(jointAxis1,
+					mat1,
+					mat2,
+					rbA.getInvInertiaDiagLocal(),
+					rbB.getInvInertiaDiagLocal());
+
+			// JAVA NOTE: reused mat1 and mat2, as recomputation is not needed
+			jacAng[2].init(hingeAxisWorld,
+					mat1,
+					mat2,
+					rbA.getInvInertiaDiagLocal(),
+					rbB.getInvInertiaDiagLocal());
+
+			// Compute limit information
+			float hingeAngle = getHingeAngle();
+
+			//set bias, sign, clear accumulator
+			correction = 0f;
+			limitSign = 0f;
+			solveLimit = false;
+			accLimitImpulse = 0f;
+
+			if (lowerLimit < upperLimit) {
+				if (hingeAngle <= lowerLimit * limitSoftness) {
+					correction = (lowerLimit - hingeAngle);
+					limitSign = 1.0f;
+					solveLimit = true;
+				}
+				else if (hingeAngle >= upperLimit * limitSoftness) {
+					correction = upperLimit - hingeAngle;
+					limitSign = -1.0f;
+					solveLimit = true;
+				}
+			}
+
+			// Compute K = J*W*J' for hinge axis
+			Vector3f axisA = stack.vectors.get();
+			rbAFrame.basis.getColumn(2, axisA);
+			getRigidBodyA().getCenterOfMassTransform().basis.transform(axisA);
+
+			kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
+					getRigidBodyB().computeAngularImpulseDenominator(axisA));
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	@Override
+	public void solveConstraint(float timeStep) {
+		stack.vectors.push();
+		try {
+			Vector3f tmp = stack.vectors.get();
+			Vector3f tmp2 = stack.vectors.get();
+
+			Vector3f pivotAInW = stack.vectors.get(rbAFrame.origin);
+			rbA.getCenterOfMassTransform().transform(pivotAInW);
+
+			Vector3f pivotBInW = stack.vectors.get(rbBFrame.origin);
+			rbB.getCenterOfMassTransform().transform(pivotBInW);
+
+			float tau = 0.3f;
+
+			// linear part
+			if (!angularOnly) {
+				Vector3f rel_pos1 = stack.vectors.get();
+				rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition());
+
+				Vector3f rel_pos2 = stack.vectors.get();
+				rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition());
+
+				Vector3f vel1 = stack.vectors.get(rbA.getVelocityInLocalPoint(rel_pos1));
+				Vector3f vel2 = stack.vectors.get(rbB.getVelocityInLocalPoint(rel_pos2));
+				Vector3f vel = stack.vectors.get();
+				vel.sub(vel1, vel2);
+
+				for (int i = 0; i < 3; i++) {
+					Vector3f normal = jac[i].linearJointAxis;
+					float jacDiagABInv = 1f / jac[i].getDiagonal();
+
+					float rel_vel;
+					rel_vel = normal.dot(vel);
+					// positional error (zeroth order error)
+					tmp.sub(pivotAInW, pivotBInW);
+					float depth = -(tmp).dot(normal); // this is the error projected on the normal
+					float impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
+					appliedImpulse += impulse;
+					Vector3f impulse_vector = stack.vectors.get();
+					impulse_vector.scale(impulse, normal);
+
+					tmp.sub(pivotAInW, rbA.getCenterOfMassPosition());
+					rbA.applyImpulse(impulse_vector, tmp);
+
+					tmp.negate(impulse_vector);
+					tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition());
+					rbB.applyImpulse(tmp, tmp2);
+				}
+			}
+
+
+			{
+				// solve angular part
+
+				// get axes in world space
+				Vector3f axisA = stack.vectors.get();
+				rbAFrame.basis.getColumn(2, axisA);
+				getRigidBodyA().getCenterOfMassTransform().basis.transform(axisA);
+
+				Vector3f axisB = stack.vectors.get();
+				rbBFrame.basis.getColumn(2, axisB);
+				getRigidBodyB().getCenterOfMassTransform().basis.transform(axisB);
+
+				Vector3f angVelA = getRigidBodyA().getAngularVelocity();
+				Vector3f angVelB = getRigidBodyB().getAngularVelocity();
+
+				Vector3f angVelAroundHingeAxisA = stack.vectors.get();
+				angVelAroundHingeAxisA.scale(axisA.dot(angVelA), axisA);
+
+				Vector3f angVelAroundHingeAxisB = stack.vectors.get();
+				angVelAroundHingeAxisB.scale(axisB.dot(angVelB), axisB);
+
+				Vector3f angAorthog = stack.vectors.get();
+				angAorthog.sub(angVelA, angVelAroundHingeAxisA);
+
+				Vector3f angBorthog = stack.vectors.get();
+				angBorthog.sub(angVelB, angVelAroundHingeAxisB);
+
+				Vector3f velrelOrthog = stack.vectors.get();
+				velrelOrthog.sub(angAorthog, angBorthog);
+
+				{
+					// solve orthogonal angular velocity correction
+					float relaxation = 1f;
+					float len = velrelOrthog.length();
+					if (len > 0.00001f) {
+						Vector3f normal = stack.vectors.get();
+						normal.normalize(velrelOrthog);
+
+						float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
+								getRigidBodyB().computeAngularImpulseDenominator(normal);
+						// scale for mass and relaxation
+						// todo:  expose this 0.9 factor to developer
+						velrelOrthog.scale((1f / denom) * relaxationFactor);
+					}
+
+					// solve angular positional correction
+					// TODO: check
+					//Vector3f angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
+					Vector3f angularError = stack.vectors.get();
+					angularError.cross(axisA, axisB);
+					angularError.negate();
+					angularError.scale(1f / timeStep);
+					float len2 = angularError.length();
+					if (len2 > 0.00001f) {
+						Vector3f normal2 = stack.vectors.get();
+						normal2.normalize(angularError);
+
+						float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
+								getRigidBodyB().computeAngularImpulseDenominator(normal2);
+						angularError.scale((1f / denom2) * relaxation);
+					}
+
+					tmp.negate(velrelOrthog);
+					tmp.add(angularError);
+					rbA.applyTorqueImpulse(tmp);
+
+					tmp.sub(velrelOrthog, angularError);
+					rbB.applyTorqueImpulse(tmp);
+
+					// solve limit
+					if (solveLimit) {
+						tmp.sub(angVelB, angVelA);
+						float amplitude = ((tmp).dot(axisA) * relaxationFactor + correction * (1f / timeStep) * biasFactor) * limitSign;
+
+						float impulseMag = amplitude * kHinge;
+
+						// Clamp the accumulated impulse
+						float temp = accLimitImpulse;
+						accLimitImpulse = Math.max(accLimitImpulse + impulseMag, 0f);
+						impulseMag = accLimitImpulse - temp;
+
+						Vector3f impulse = stack.vectors.get();
+						impulse.scale(impulseMag * limitSign, axisA);
+
+						rbA.applyTorqueImpulse(impulse);
+
+						tmp.negate(impulse);
+						rbB.applyTorqueImpulse(tmp);
+					}
+				}
+
+				// apply motor
+				if (enableAngularMotor) {
+					// todo: add limits too
+					Vector3f angularLimit = stack.vectors.get(0f, 0f, 0f);
+
+					Vector3f velrel = stack.vectors.get();
+					velrel.sub(angVelAroundHingeAxisA, angVelAroundHingeAxisB);
+					float projRelVel = velrel.dot(axisA);
+
+					float desiredMotorVel = motorTargetVelocity;
+					float motor_relvel = desiredMotorVel - projRelVel;
+
+					float unclippedMotorImpulse = kHinge * motor_relvel;
+					// todo: should clip against accumulated impulse
+					float clippedMotorImpulse = unclippedMotorImpulse > maxMotorImpulse ? maxMotorImpulse : unclippedMotorImpulse;
+					clippedMotorImpulse = clippedMotorImpulse < -maxMotorImpulse ? -maxMotorImpulse : clippedMotorImpulse;
+					Vector3f motorImp = stack.vectors.get();
+					motorImp.scale(clippedMotorImpulse, axisA);
+
+					tmp.add(motorImp, angularLimit);
+					rbA.applyTorqueImpulse(tmp);
+
+					tmp.negate(motorImp);
+					tmp.sub(angularLimit);
+					rbB.applyTorqueImpulse(tmp);
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void updateRHS(float timeStep) {
+	}
+
+	public float getHingeAngle() {
+		stack.vectors.push();
+		try {
+			Vector3f refAxis0 = stack.vectors.get();
+			rbAFrame.basis.getColumn(0, refAxis0);
+			getRigidBodyA().getCenterOfMassTransform().basis.transform(refAxis0);
+
+			Vector3f refAxis1 = stack.vectors.get();
+			rbAFrame.basis.getColumn(1, refAxis1);
+			getRigidBodyA().getCenterOfMassTransform().basis.transform(refAxis1);
+
+			Vector3f swingAxis = stack.vectors.get();
+			rbBFrame.basis.getColumn(1, swingAxis);
+			getRigidBodyB().getCenterOfMassTransform().basis.transform(swingAxis);
+
+			return ScalarUtil.atan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public void setAngularOnly(boolean angularOnly) {
+		this.angularOnly = angularOnly;
+	}
+
+	public void enableAngularMotor(boolean enableMotor, float targetVelocity, float maxMotorImpulse) {
+		this.enableAngularMotor = enableMotor;
+		this.motorTargetVelocity = targetVelocity;
+		this.maxMotorImpulse = maxMotorImpulse;
+	}
+
+	public void setLimit(float low, float high) {
+		setLimit(low, high, 0.9f, 0.3f, 1.0f);
+	}
+
+	public void setLimit(float low, float high, float _softness, float _biasFactor, float _relaxationFactor) {
+		lowerLimit = low;
+		upperLimit = high;
+
+		limitSoftness = _softness;
+		biasFactor = _biasFactor;
+		relaxationFactor = _relaxationFactor;
+	}
+
+	public float getLowerLimit() {
+		return lowerLimit;
+	}
+
+	public float getUpperLimit() {
+		return upperLimit;
+	}
+
+	public Transform getAFrame() {
+		return rbAFrame;
+	}
+
+	public Transform getBFrame() {
+		return rbBFrame;
+	}
+
+	public boolean getSolveLimit() {
+		return solveLimit;
+	}
+
+	public float getLimitSign() {
+		return limitSign;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/JacobianEntry.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/JacobianEntry.java
new file mode 100644
index 0000000..aca6ccc
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/JacobianEntry.java
@@ -0,0 +1,231 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Vector3f;
+
+//notes:
+// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
+// which makes the btJacobianEntry memory layout 16 bytes
+// if you only are interested in angular part, just feed massInvA and massInvB zero
+
+/**
+ * Jacobian entry is an abstraction that allows to describe constraints.
+ * It can be used in combination with a constraint solver.
+ * Can be used to relate the effect of an impulse to the constraint error.
+ * 
+ * @author jezek2
+ */
+public class JacobianEntry {
+	
+	protected final BulletStack stack = BulletStack.get();
+	
+	public final Vector3f linearJointAxis = new Vector3f();
+	public final Vector3f aJ = new Vector3f();
+	public final Vector3f bJ = new Vector3f();
+	public final Vector3f m_0MinvJt = new Vector3f();
+	public final Vector3f m_1MinvJt = new Vector3f();
+	// Optimization: can be stored in the w/last component of one of the vectors
+	public float Adiag;
+
+	public JacobianEntry() {
+	}
+
+	/**
+	 * Constraint between two different rigidbodies.
+	 */
+	public void init(Matrix3f world2A,
+			Matrix3f world2B,
+			Vector3f rel_pos1, Vector3f rel_pos2,
+			Vector3f jointAxis,
+			Vector3f inertiaInvA,
+			float massInvA,
+			Vector3f inertiaInvB,
+			float massInvB)
+	{
+		linearJointAxis.set(jointAxis);
+
+		aJ.cross(rel_pos1, linearJointAxis);
+		world2A.transform(aJ);
+
+		bJ.set(linearJointAxis);
+		bJ.negate();
+		bJ.cross(rel_pos2, bJ);
+		world2B.transform(bJ);
+
+		VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ);
+		VectorUtil.mul(m_1MinvJt, inertiaInvB, bJ);
+		Adiag = massInvA + m_0MinvJt.dot(aJ) + massInvB + m_1MinvJt.dot(bJ);
+
+		assert (Adiag > 0f);
+	}
+
+	/**
+	 * Angular constraint between two different rigidbodies.
+	 */
+	public void init(Vector3f jointAxis,
+		Matrix3f world2A,
+		Matrix3f world2B,
+		Vector3f inertiaInvA,
+		Vector3f inertiaInvB)
+	{
+		linearJointAxis.set(0f, 0f, 0f);
+
+		aJ.set(jointAxis);
+		world2A.transform(aJ);
+
+		bJ.set(jointAxis);
+		bJ.negate();
+		world2B.transform(bJ);
+
+		VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ);
+		VectorUtil.mul(m_1MinvJt, inertiaInvB, bJ);
+		Adiag = m_0MinvJt.dot(aJ) + m_1MinvJt.dot(bJ);
+
+		assert (Adiag > 0f);
+	}
+
+	/**
+	 * Angular constraint between two different rigidbodies.
+	 */
+	public void init(Vector3f axisInA,
+		Vector3f axisInB,
+		Vector3f inertiaInvA,
+		Vector3f inertiaInvB)
+	{
+		linearJointAxis.set(0f, 0f, 0f);
+		aJ.set(axisInA);
+
+		bJ.set(axisInB);
+		bJ.negate();
+
+		VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ);
+		VectorUtil.mul(m_1MinvJt, inertiaInvB, bJ);
+		Adiag = m_0MinvJt.dot(aJ) + m_1MinvJt.dot(bJ);
+
+		assert (Adiag > 0f);
+	}
+
+	/**
+	 * Constraint on one rigidbody.
+	 */
+	public void init(
+		Matrix3f world2A,
+		Vector3f rel_pos1, Vector3f rel_pos2,
+		Vector3f jointAxis,
+		Vector3f inertiaInvA, 
+		float massInvA)
+	{
+		linearJointAxis.set(jointAxis);
+
+		aJ.cross(rel_pos1, jointAxis);
+		world2A.transform(aJ);
+
+		bJ.set(jointAxis);
+		bJ.negate();
+		bJ.cross(rel_pos2, bJ);
+		world2A.transform(bJ);
+
+		VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ);
+		m_1MinvJt.set(0f, 0f, 0f);
+		Adiag = massInvA + m_0MinvJt.dot(aJ);
+
+		assert (Adiag > 0f);
+	}
+
+	public float getDiagonal() { return Adiag; }
+
+	/**
+	 * For two constraints on the same rigidbody (for example vehicle friction).
+	 */
+	public float getNonDiagonal(JacobianEntry jacB, float massInvA) {
+		JacobianEntry jacA = this;
+		float lin = massInvA * jacA.linearJointAxis.dot(jacB.linearJointAxis);
+		float ang = jacA.m_0MinvJt.dot(jacB.aJ);
+		return lin + ang;
+	}
+
+	/**
+	 * For two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies).
+	 */
+	public float getNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB) {
+		stack.vectors.push();
+		try {
+			JacobianEntry jacA = this;
+
+			Vector3f lin = stack.vectors.get();
+			VectorUtil.mul(lin, jacA.linearJointAxis, jacB.linearJointAxis);
+
+			Vector3f ang0 = stack.vectors.get();
+			VectorUtil.mul(ang0, jacA.m_0MinvJt, jacB.aJ);
+
+			Vector3f ang1 = stack.vectors.get();
+			VectorUtil.mul(ang1, jacA.m_1MinvJt, jacB.bJ);
+
+			Vector3f lin0 = stack.vectors.get();
+			lin0.scale(massInvA, lin);
+
+			Vector3f lin1 = stack.vectors.get();
+			lin1.scale(massInvB, lin);
+
+			Vector3f sum = stack.vectors.get();
+			VectorUtil.add(sum, ang0, ang1, lin0, lin1);
+
+			return sum.x + sum.y + sum.z;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public float getRelativeVelocity(Vector3f linvelA, Vector3f angvelA, Vector3f linvelB, Vector3f angvelB) {
+		stack.vectors.push();
+		try {
+			Vector3f linrel = stack.vectors.get();
+			linrel.sub(linvelA, linvelB);
+
+			Vector3f angvela = stack.vectors.get();
+			VectorUtil.mul(angvela, angvelA, aJ);
+
+			Vector3f angvelb = stack.vectors.get();
+			VectorUtil.mul(angvelb, angvelB, bJ);
+
+			VectorUtil.mul(linrel, linrel, linearJointAxis);
+
+			angvela.add(angvelb);
+			angvela.add(linrel);
+
+			float rel_vel2 = angvela.x + angvela.y + angvela.z;
+			return rel_vel2 + BulletGlobals.FLT_EPSILON;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/Point2PointConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/Point2PointConstraint.java
new file mode 100644
index 0000000..0d64c25
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/Point2PointConstraint.java
@@ -0,0 +1,197 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+import javabullet.dynamics.RigidBody;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Vector3f;
+
+/**
+ * Point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space.
+ * 
+ * @author jezek2
+ */
+public class Point2PointConstraint extends TypedConstraint {
+
+	private final JacobianEntry[] jac = new JacobianEntry[]/*[3]*/ { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() }; // 3 orthogonal linear constraints
+	
+	private final Vector3f pivotInA = new Vector3f();
+	private final Vector3f pivotInB = new Vector3f();
+
+	public ConstraintSetting setting = new ConstraintSetting();
+	
+	public Point2PointConstraint() {
+		super(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE);
+	}
+
+	public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3f pivotInA, Vector3f pivotInB) {
+		super(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE, rbA, rbB);
+		this.pivotInA.set(pivotInA);
+		this.pivotInB.set(pivotInB);
+	}
+
+	public Point2PointConstraint(RigidBody rbA, Vector3f pivotInA) {
+		super(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE, rbA);
+		this.pivotInA.set(pivotInA);
+		this.pivotInB.set(pivotInA);
+		rbA.getCenterOfMassTransform().transform(this.pivotInB);
+	}
+
+	@Override
+	public void buildJacobian() {
+		stack.pushCommonMath();
+		try {
+			appliedImpulse = 0f;
+
+			Vector3f normal = stack.vectors.get(0f, 0f, 0f);
+
+			Matrix3f tmpMat1 = stack.matrices.get();
+			Matrix3f tmpMat2 = stack.matrices.get();
+			Vector3f tmp1 = stack.vectors.get();
+			Vector3f tmp2 = stack.vectors.get();
+
+			for (int i = 0; i < 3; i++) {
+				VectorUtil.setCoord(normal, i, 1f);
+
+				tmpMat1.transpose(rbA.getCenterOfMassTransform().basis);
+				tmpMat2.transpose(rbB.getCenterOfMassTransform().basis);
+
+				tmp1.set(pivotInA);
+				rbA.getCenterOfMassTransform().transform(tmp1);
+				tmp1.sub(rbA.getCenterOfMassPosition());
+
+				tmp2.set(pivotInB);
+				rbB.getCenterOfMassTransform().transform(tmp2);
+				tmp2.sub(rbB.getCenterOfMassPosition());
+
+				jac[i].init(
+						tmpMat1,
+						tmpMat2,
+						tmp1,
+						tmp2,
+						normal,
+						rbA.getInvInertiaDiagLocal(),
+						rbA.getInvMass(),
+						rbB.getInvInertiaDiagLocal(),
+						rbB.getInvMass());
+				VectorUtil.setCoord(normal, i, 0f);
+			}
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	@Override
+	public void solveConstraint(float timeStep) {
+		stack.vectors.push();
+		try {
+			Vector3f tmp = stack.vectors.get();
+			Vector3f tmp2 = stack.vectors.get();
+
+			Vector3f pivotAInW = stack.vectors.get(pivotInA);
+			rbA.getCenterOfMassTransform().transform(pivotAInW);
+
+			Vector3f pivotBInW = stack.vectors.get(pivotInB);
+			rbB.getCenterOfMassTransform().transform(pivotBInW);
+
+			Vector3f normal = stack.vectors.get(0f, 0f, 0f);
+
+			//btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
+			//btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
+
+			for (int i = 0; i < 3; i++) {
+				VectorUtil.setCoord(normal, i, 1f);
+				float jacDiagABInv = 1f / jac[i].getDiagonal();
+
+				Vector3f rel_pos1 = stack.vectors.get();
+				rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition());
+				Vector3f rel_pos2 = stack.vectors.get();
+				rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition());
+				// this jacobian entry could be re-used for all iterations
+
+				Vector3f vel1 = stack.vectors.get(rbA.getVelocityInLocalPoint(rel_pos1));
+				Vector3f vel2 = stack.vectors.get(rbB.getVelocityInLocalPoint(rel_pos2));
+				Vector3f vel = stack.vectors.get();
+				vel.sub(vel1, vel2);
+
+				float rel_vel;
+				rel_vel = normal.dot(vel);
+
+				/*
+				//velocity error (first order error)
+				btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
+				m_rbB.getLinearVelocity(),angvelB);
+				 */
+
+				// positional error (zeroth order error)
+				tmp.sub(pivotAInW, pivotBInW);
+				float depth = -tmp.dot(normal); //this is the error projected on the normal
+
+				float impulse = depth * setting.tau / timeStep * jacDiagABInv - setting.damping * rel_vel * jacDiagABInv;
+				appliedImpulse += impulse;
+				Vector3f impulse_vector = stack.vectors.get();
+				impulse_vector.scale(impulse, normal);
+				tmp.sub(pivotAInW, rbA.getCenterOfMassPosition());
+				rbA.applyImpulse(impulse_vector, tmp);
+				tmp.negate(impulse_vector);
+				tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition());
+				rbB.applyImpulse(tmp, tmp2);
+
+				VectorUtil.setCoord(normal, i, 0f);
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public void updateRHS(float timeStep) {
+	}
+
+	public void setPivotA(Vector3f pivotA) {
+		pivotInA.set(pivotA);
+	}
+
+	public void setPivotB(Vector3f pivotB) {
+		pivotInB.set(pivotB);
+	}
+
+	public Vector3f getPivotInA() {
+		return pivotInA;
+	}
+
+	public Vector3f getPivotInB() {
+		return pivotInB;
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	public static class ConstraintSetting {
+		public float tau = 0.3f;
+		public float damping = 1f;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/RotationalLimitMotor.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/RotationalLimitMotor.java
new file mode 100644
index 0000000..73fa92e
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/RotationalLimitMotor.java
@@ -0,0 +1,211 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+/*
+2007-09-09
+btGeneric6DofConstraint Refactored by Francisco Le�n
+email: [email protected]
+http://gimpact.sf.net
+*/
+
+package javabullet.dynamics.constraintsolver;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javabullet.dynamics.RigidBody;
+import javax.vecmath.Vector3f;
+
+/**
+ * Rotation Limit structure for generic joints.
+ * 
+ * @author jezek2
+ */
+public class RotationalLimitMotor {
+	
+	protected final BulletStack stack = BulletStack.get();
+
+	public float loLimit; //!< joint limit
+	public float hiLimit; //!< joint limit
+	public float targetVelocity; //!< target motor velocity
+	public float maxMotorForce; //!< max force on motor
+	public float maxLimitForce; //!< max force on limit
+	public float damping; //!< Damping.
+	public float limitSoftness; //! Relaxation factor
+	public float ERP; //!< Error tolerance factor when joint is at limit
+	public float bounce; //!< restitution factor
+	public boolean enableMotor;
+	
+	public float currentLimitError;//!  How much is violated this limit
+	public int currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
+	public float accumulatedImpulse;
+
+	public RotationalLimitMotor() {
+    	accumulatedImpulse = 0.f;
+        targetVelocity = 0;
+        maxMotorForce = 0.1f;
+        maxLimitForce = 300.0f;
+        loLimit = -BulletGlobals.SIMD_INFINITY;
+        hiLimit = BulletGlobals.SIMD_INFINITY;
+        ERP = 0.5f;
+        bounce = 0.0f;
+        damping = 1.0f;
+        limitSoftness = 0.5f;
+        currentLimit = 0;
+        currentLimitError = 0;
+        enableMotor = false;
+	}
+	
+	public RotationalLimitMotor(RotationalLimitMotor limot) {
+		targetVelocity = limot.targetVelocity;
+		maxMotorForce = limot.maxMotorForce;
+		limitSoftness = limot.limitSoftness;
+		loLimit = limot.loLimit;
+		hiLimit = limot.hiLimit;
+		ERP = limot.ERP;
+		bounce = limot.bounce;
+		currentLimit = limot.currentLimit;
+		currentLimitError = limot.currentLimitError;
+		enableMotor = limot.enableMotor;
+	}
+
+	/**
+	 * Is limited?
+	 */
+    public boolean isLimited()
+    {
+    	if(loLimit>=hiLimit) return false;
+    	return true;
+    }
+
+	/**
+	 * Need apply correction?
+	 */
+    public boolean needApplyTorques()
+    {
+    	if(currentLimit == 0 && enableMotor == false) return false;
+    	return true;
+    }
+
+	/**
+	 * Calculates error. Calculates currentLimit and currentLimitError.
+	 */
+	public int testLimitValue(float test_value) {
+		if (loLimit > hiLimit) {
+			currentLimit = 0; // Free from violation
+			return 0;
+		}
+
+		if (test_value < loLimit) {
+			currentLimit = 1; // low limit violation
+			currentLimitError = test_value - loLimit;
+			return 1;
+		}
+		else if (test_value > hiLimit) {
+			currentLimit = 2; // High limit violation
+			currentLimitError = test_value - hiLimit;
+			return 2;
+		}
+		else {
+			currentLimit = 0; // Free from violation
+			return 0;
+		}
+		//return 0;
+	}
+
+	/**
+	 * Apply the correction impulses for two bodies.
+	 */
+	public float solveAngularLimits(float timeStep, Vector3f axis, float jacDiagABInv, RigidBody body0, RigidBody body1) {
+		if (needApplyTorques() == false) {
+			return 0.0f;
+		}
+
+		stack.vectors.push();
+		try {
+			float target_velocity = this.targetVelocity;
+			float maxMotorForce = this.maxMotorForce;
+
+			// current error correction
+			if (currentLimit != 0) {
+				target_velocity = -ERP * currentLimitError / (timeStep);
+				maxMotorForce = maxLimitForce;
+			}
+
+			maxMotorForce *= timeStep;
+
+			// current velocity difference
+			Vector3f vel_diff = stack.vectors.get(body0.getAngularVelocity());
+			if (body1 != null) {
+				vel_diff.sub(body1.getAngularVelocity());
+			}
+
+			float rel_vel = axis.dot(vel_diff);
+
+			// correction velocity
+			float motor_relvel = limitSoftness * (target_velocity - damping * rel_vel);
+
+			if (motor_relvel < BulletGlobals.FLT_EPSILON && motor_relvel > -BulletGlobals.FLT_EPSILON) {
+				return 0.0f; // no need for applying force
+			}
+
+			// correction impulse
+			float unclippedMotorImpulse = (1 + bounce) * motor_relvel * jacDiagABInv;
+
+			// clip correction impulse
+			float clippedMotorImpulse;
+
+			// todo: should clip against accumulated impulse
+			if (unclippedMotorImpulse > 0.0f) {
+				clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
+			}
+			else {
+				clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
+			}
+
+			// sort with accumulated impulses
+			float lo = -1e30f;
+			float hi = 1e30f;
+
+			float oldaccumImpulse = accumulatedImpulse;
+			float sum = oldaccumImpulse + clippedMotorImpulse;
+			accumulatedImpulse = sum > hi ? 0f : sum < lo ? 0f : sum;
+
+			clippedMotorImpulse = accumulatedImpulse - oldaccumImpulse;
+
+			Vector3f motorImp = stack.vectors.get();
+			motorImp.scale(clippedMotorImpulse, axis);
+
+			body0.applyTorqueImpulse(motorImp);
+			if (body1 != null) {
+				motorImp.negate();
+				body1.applyTorqueImpulse(motorImp);
+			}
+
+			return clippedMotorImpulse;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/SequentialImpulseConstraintSolver.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/SequentialImpulseConstraintSolver.java
new file mode 100644
index 0000000..ba52ff0
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/SequentialImpulseConstraintSolver.java
@@ -0,0 +1,1233 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+import java.util.ArrayList;
+import java.util.List;
+import javabullet.BulletGlobals;
+import javabullet.BulletPool;
+import javabullet.ContactDestroyedCallback;
+import javabullet.ObjectPool;
+import javabullet.collision.broadphase.Dispatcher;
+import javabullet.collision.dispatch.CollisionObject;
+import javabullet.collision.narrowphase.ManifoldPoint;
+import javabullet.collision.narrowphase.PersistentManifold;
+import javabullet.dynamics.RigidBody;
+import javabullet.linearmath.IDebugDraw;
+import javabullet.linearmath.MiscUtil;
+import javabullet.linearmath.TransformUtil;
+import javabullet.util.IntArrayList;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Vector3f;
+
+/**
+ * SequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses.
+ * The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com<p>
+ * 
+ * Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP).
+ * Applies impulses for combined restitution and penetration recovery and to simulate friction.
+ * 
+ * @author jezek2
+ */
+public class SequentialImpulseConstraintSolver extends ConstraintSolver {
+	
+	private static final int MAX_CONTACT_SOLVER_TYPES = ContactConstraintEnum.MAX_CONTACT_SOLVER_TYPES.ordinal();
+
+	private static final int SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS = 16384;
+	private static OrderIndex[] gOrder = new OrderIndex[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS];
+	
+	private static int totalCpd = 0;
+	
+	static {
+		for (int i=0; i<gOrder.length; i++) {
+			gOrder[i] = new OrderIndex();
+		}
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	private final ObjectPool<SolverBody> bodiesPool = BulletPool.get(SolverBody.class);
+	private final ObjectPool<SolverConstraint> constraintsPool = BulletPool.get(SolverConstraint.class);
+	private final ObjectPool<JacobianEntry> jacobiansPool = BulletPool.get(JacobianEntry.class);
+	
+	private final List<SolverBody> tmpSolverBodyPool = new ArrayList<SolverBody>();
+	private final List<SolverConstraint> tmpSolverConstraintPool = new ArrayList<SolverConstraint>();
+	private final List<SolverConstraint> tmpSolverFrictionConstraintPool = new ArrayList<SolverConstraint>();
+	private final IntArrayList orderTmpConstraintPool = new IntArrayList();
+	private final IntArrayList orderFrictionConstraintPool = new IntArrayList();
+	
+	protected final ContactSolverFunc[][] contactDispatch = new ContactSolverFunc[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
+	protected final ContactSolverFunc[][] frictionDispatch = new ContactSolverFunc[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES];
+	
+	// choose between several modes, different friction model etc.
+	protected int solverMode = SolverMode.SOLVER_RANDMIZE_ORDER | SolverMode.SOLVER_CACHE_FRIENDLY; // not using SOLVER_USE_WARMSTARTING,
+	// btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
+	protected long btSeed2 = 0L;
+
+	public SequentialImpulseConstraintSolver() {
+		BulletGlobals.gContactDestroyedCallback = new ContactDestroyedCallback() {
+			public boolean invoke(Object userPersistentData) {
+				assert (userPersistentData != null);
+				ConstraintPersistentData cpd = (ConstraintPersistentData) userPersistentData;
+				//btAlignedFree(cpd);
+				totalCpd--;
+				//printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData);
+				return true;
+			}
+		};
+
+		// initialize default friction/contact funcs
+		int i, j;
+		for (i = 0; i < MAX_CONTACT_SOLVER_TYPES; i++) {
+			for (j = 0; j < MAX_CONTACT_SOLVER_TYPES; j++) {
+				contactDispatch[i][j] = ContactConstraint.resolveSingleCollision;
+				frictionDispatch[i][j] = ContactConstraint.resolveSingleFriction;
+			}
+		}
+	}
+	
+	public long rand2() {
+		btSeed2 = (1664525L * btSeed2 + 1013904223L) & 0xffffffff;
+		return btSeed2;
+	}
+	
+	// See ODE: adam's all-int straightforward(?) dRandInt (0..n-1)
+	public int randInt2(int n) {
+		// seems good; xor-fold and modulus
+		long un = n;
+		long r = rand2();
+
+		// note: probably more aggressive than it needs to be -- might be
+		//       able to get away without one or two of the innermost branches.
+		if (un <= 0x00010000L) {
+			r ^= (r >>> 16);
+			if (un <= 0x00000100L) {
+				r ^= (r >>> 8);
+				if (un <= 0x00000010L) {
+					r ^= (r >>> 4);
+					if (un <= 0x00000004L) {
+						r ^= (r >>> 2);
+						if (un <= 0x00000002L) {
+							r ^= (r >>> 1);
+						}
+					}
+				}
+			}
+		}
+
+		// TODO: check modulo C vs Java mismatch
+		return (int) Math.abs(r % un);
+	}
+	
+	private void initSolverBody(SolverBody solverBody, CollisionObject collisionObject) {
+		RigidBody rb = RigidBody.upcast(collisionObject);
+		if (rb != null) {
+			solverBody.angularVelocity.set(rb.getAngularVelocity());
+			solverBody.centerOfMassPosition.set(collisionObject.getWorldTransform().origin);
+			solverBody.friction = collisionObject.getFriction();
+			solverBody.invMass = rb.getInvMass();
+			solverBody.linearVelocity.set(rb.getLinearVelocity());
+			solverBody.originalBody = rb;
+			solverBody.angularFactor = rb.getAngularFactor();
+		}
+		else {
+			solverBody.angularVelocity.set(0f, 0f, 0f);
+			solverBody.centerOfMassPosition.set(collisionObject.getWorldTransform().origin);
+			solverBody.friction = collisionObject.getFriction();
+			solverBody.invMass = 0f;
+			solverBody.linearVelocity.set(0f, 0f, 0f);
+			solverBody.originalBody = null;
+			solverBody.angularFactor = 1f;
+		}
+	}
+	
+	private float restitutionCurve(float rel_vel, float restitution) {
+		float rest = restitution * -rel_vel;
+		return rest;
+	}
+
+	/**
+	 * velocity + friction
+	 * response  between two dynamic objects with friction
+	 */
+	private float resolveSingleCollisionCombinedCacheFriendly(
+			SolverBody body1,
+			SolverBody body2,
+			SolverConstraint contactConstraint,
+			ContactSolverInfo solverInfo) {
+		stack.vectors.push();
+		try {
+			float normalImpulse;
+
+			//  Optimized version of projected relative velocity, use precomputed cross products with normal
+			//	body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1);
+			//	body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2);
+			//	btVector3 vel = vel1 - vel2;
+			//	btScalar  rel_vel = contactConstraint.m_contactNormal.dot(vel);
+
+			float rel_vel;
+			float vel1Dotn = contactConstraint.contactNormal.dot(body1.linearVelocity) + contactConstraint.relpos1CrossNormal.dot(body1.angularVelocity);
+			float vel2Dotn = contactConstraint.contactNormal.dot(body2.linearVelocity) + contactConstraint.relpos2CrossNormal.dot(body2.angularVelocity);
+
+			rel_vel = vel1Dotn - vel2Dotn;
+
+
+			float positionalError = contactConstraint.penetration;
+			float velocityError = contactConstraint.restitution - rel_vel; // * damping;
+
+			float penetrationImpulse = positionalError * contactConstraint.jacDiagABInv;
+			float velocityImpulse = velocityError * contactConstraint.jacDiagABInv;
+			normalImpulse = penetrationImpulse + velocityImpulse;
+
+			// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+			float oldNormalImpulse = contactConstraint.appliedImpulse;
+			float sum = oldNormalImpulse + normalImpulse;
+			contactConstraint.appliedImpulse = 0f > sum ? 0f : sum;
+
+			float oldVelocityImpulse = contactConstraint.appliedVelocityImpulse;
+			float velocitySum = oldVelocityImpulse + velocityImpulse;
+			contactConstraint.appliedVelocityImpulse = 0f > velocitySum ? 0f : velocitySum;
+
+			normalImpulse = contactConstraint.appliedImpulse - oldNormalImpulse;
+
+			Vector3f tmp = stack.vectors.get();
+			if (body1.invMass != 0f) {
+				tmp.scale(body1.invMass, contactConstraint.contactNormal);
+				body1.internalApplyImpulse(tmp, contactConstraint.angularComponentA, normalImpulse);
+			}
+			if (body2.invMass != 0f) {
+				tmp.scale(body2.invMass, contactConstraint.contactNormal);
+				body2.internalApplyImpulse(tmp, contactConstraint.angularComponentB, -normalImpulse);
+			}
+
+			return normalImpulse;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	private float resolveSingleFrictionCacheFriendly(
+			SolverBody body1,
+			SolverBody body2,
+			SolverConstraint contactConstraint,
+			ContactSolverInfo solverInfo,
+			float appliedNormalImpulse) {
+		stack.vectors.push();
+		try {
+			float combinedFriction = contactConstraint.friction;
+
+			float limit = appliedNormalImpulse * combinedFriction;
+
+			if (appliedNormalImpulse > 0f) //friction
+			{
+
+				float j1;
+				{
+
+					float rel_vel;
+					float vel1Dotn = contactConstraint.contactNormal.dot(body1.linearVelocity) + contactConstraint.relpos1CrossNormal.dot(body1.angularVelocity);
+					float vel2Dotn = contactConstraint.contactNormal.dot(body2.linearVelocity) + contactConstraint.relpos2CrossNormal.dot(body2.angularVelocity);
+					rel_vel = vel1Dotn - vel2Dotn;
+
+					// calculate j that moves us to zero relative velocity
+					j1 = -rel_vel * contactConstraint.jacDiagABInv;
+					//#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1
+					//#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE
+					float oldTangentImpulse = contactConstraint.appliedImpulse;
+					contactConstraint.appliedImpulse = oldTangentImpulse + j1;
+
+					if (limit < contactConstraint.appliedImpulse) {
+						contactConstraint.appliedImpulse = limit;
+					}
+					else {
+						if (contactConstraint.appliedImpulse < -limit) {
+							contactConstraint.appliedImpulse = -limit;
+						}
+					}
+					j1 = contactConstraint.appliedImpulse - oldTangentImpulse;
+					//	#else
+					//	if (limit < j1)
+					//	{
+					//		j1 = limit;
+					//	} else
+					//	{
+					//		if (j1 < -limit)
+					//			j1 = -limit;
+					//	}
+					//	#endif
+
+					//GEN_set_min(contactConstraint.m_appliedImpulse, limit);
+					//GEN_set_max(contactConstraint.m_appliedImpulse, -limit);
+				}
+
+				Vector3f tmp = stack.vectors.get();
+				if (body1.invMass != 0f) {
+					tmp.scale(body1.invMass, contactConstraint.contactNormal);
+					body1.internalApplyImpulse(tmp, contactConstraint.angularComponentA, j1);
+				}
+				if (body2.invMass != 0f) {
+					tmp.scale(body2.invMass, contactConstraint.contactNormal);
+					body2.internalApplyImpulse(tmp, contactConstraint.angularComponentB, -j1);
+				}
+
+			}
+			return 0f;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	protected void addFrictionConstraint(Vector3f normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, ManifoldPoint cp, Vector3f rel_pos1, Vector3f rel_pos2, CollisionObject colObj0, CollisionObject colObj1, float relaxation) {
+		stack.vectors.push();
+		try {
+			RigidBody body0 = RigidBody.upcast(colObj0);
+			RigidBody body1 = RigidBody.upcast(colObj1);
+
+			SolverConstraint solverConstraint = constraintsPool.get();
+			tmpSolverFrictionConstraintPool.add(solverConstraint);
+
+			solverConstraint.contactNormal.set(normalAxis);
+
+			solverConstraint.solverBodyIdA = solverBodyIdA;
+			solverConstraint.solverBodyIdB = solverBodyIdB;
+			solverConstraint.constraintType = SolverConstraintType.SOLVER_FRICTION_1D;
+			solverConstraint.frictionIndex = frictionIndex;
+
+			solverConstraint.friction = cp.combinedFriction;
+
+			solverConstraint.appliedImpulse = 0f;
+			solverConstraint.appliedVelocityImpulse = 0f;
+			solverConstraint.penetration = 0f;
+			{
+				Vector3f ftorqueAxis1 = stack.vectors.get();
+				ftorqueAxis1.cross(rel_pos1, solverConstraint.contactNormal);
+				solverConstraint.relpos1CrossNormal.set(ftorqueAxis1);
+				if (body0 != null) {
+					solverConstraint.angularComponentA.set(ftorqueAxis1);
+					body0.getInvInertiaTensorWorld().transform(solverConstraint.angularComponentA);
+				}
+				else {
+					solverConstraint.angularComponentA.set(0f, 0f, 0f);
+				}
+			}
+			{
+				Vector3f ftorqueAxis1 = stack.vectors.get();
+				ftorqueAxis1.cross(rel_pos2, solverConstraint.contactNormal);
+				solverConstraint.relpos2CrossNormal.set(ftorqueAxis1);
+				if (body1 != null) {
+					solverConstraint.angularComponentB.set(ftorqueAxis1);
+					body1.getInvInertiaTensorWorld().transform(solverConstraint.angularComponentB);
+				}
+				else {
+					solverConstraint.angularComponentB.set(0f, 0f, 0f);
+				}
+			}
+
+			//#ifdef COMPUTE_IMPULSE_DENOM
+			//	btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
+			//	btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
+			//#else
+			Vector3f vec = stack.vectors.get();
+			float denom0 = 0f;
+			float denom1 = 0f;
+			if (body0 != null) {
+				vec.cross(solverConstraint.angularComponentA, rel_pos1);
+				denom0 = body0.getInvMass() + normalAxis.dot(vec);
+			}
+			if (body1 != null) {
+				vec.cross(solverConstraint.angularComponentB, rel_pos2);
+				denom1 = body1.getInvMass() + normalAxis.dot(vec);
+			}
+			//#endif //COMPUTE_IMPULSE_DENOM
+
+			float denom = relaxation / (denom0 + denom1);
+			solverConstraint.jacDiagABInv = denom;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public float solveGroupCacheFriendlySetup(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifoldPtr, int manifold_offset, int numManifolds, List<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer/*,btStackAlloc* stackAlloc*/) {
+		BulletGlobals.pushProfile("solveGroupCacheFriendlySetup");
+		stack.vectors.push();
+		try {
+
+			if ((numConstraints + numManifolds) == 0) {
+				// printf("empty\n");
+				return 0f;
+			}
+			PersistentManifold manifold = null;
+			CollisionObject colObj0 = null, colObj1 = null;
+
+			//btRigidBody* rb0=0,*rb1=0;
+
+	//	//#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
+	//
+	//		BEGIN_PROFILE("refreshManifolds");
+	//
+	//		int i;
+	//
+	//
+	//
+	//		for (i=0;i<numManifolds;i++)
+	//		{
+	//			manifold = manifoldPtr[i];
+	//			rb1 = (btRigidBody*)manifold->getBody1();
+	//			rb0 = (btRigidBody*)manifold->getBody0();
+	//
+	//			manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform());
+	//
+	//		}
+	//
+	//		END_PROFILE("refreshManifolds");
+	//	//#endif //FORCE_REFESH_CONTACT_MANIFOLDS
+
+			Vector3f color = stack.vectors.get(0f, 1f, 0f);
+
+			//int sizeofSB = sizeof(btSolverBody);
+			//int sizeofSC = sizeof(btSolverConstraint);
+
+			//if (1)
+			{
+				//if m_stackAlloc, try to pack bodies/constraints to speed up solving
+				//		btBlock*					sablock;
+				//		sablock = stackAlloc->beginBlock();
+
+				//	int memsize = 16;
+				//		unsigned char* stackMemory = stackAlloc->allocate(memsize);
+
+
+				// todo: use stack allocator for this temp memory
+				int minReservation = numManifolds * 2;
+
+				//m_tmpSolverBodyPool.reserve(minReservation);
+
+				//don't convert all bodies, only the one we need so solver the constraints
+				/*
+				{
+				for (int i=0;i<numBodies;i++)
+				{
+				btRigidBody* rb = btRigidBody::upcast(bodies[i]);
+				if (rb && 	(rb->getIslandTag() >= 0))
+				{
+				btAssert(rb->getCompanionId() < 0);
+				int solverBodyId = m_tmpSolverBodyPool.size();
+				btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
+				initSolverBody(&solverBody,rb);
+				rb->setCompanionId(solverBodyId);
+				} 
+				}
+				}
+				*/
+
+				//m_tmpSolverConstraintPool.reserve(minReservation);
+				//m_tmpSolverFrictionConstraintPool.reserve(minReservation);
+
+				{
+					int i;
+
+					for (i = 0; i < numManifolds; i++) {
+						manifold = manifoldPtr.get(manifold_offset+i);
+						colObj0 = (CollisionObject) manifold.getBody0();
+						colObj1 = (CollisionObject) manifold.getBody1();
+
+						int solverBodyIdA = -1;
+						int solverBodyIdB = -1;
+
+						if (manifold.getNumContacts() != 0) {
+							if (colObj0.getIslandTag() >= 0) {
+								if (colObj0.getCompanionId() >= 0) {
+									// body has already been converted
+									solverBodyIdA = colObj0.getCompanionId();
+								}
+								else {
+									solverBodyIdA = tmpSolverBodyPool.size();
+									SolverBody solverBody = bodiesPool.get();
+									tmpSolverBodyPool.add(solverBody);
+									initSolverBody(solverBody, colObj0);
+									colObj0.setCompanionId(solverBodyIdA);
+								}
+							}
+							else {
+								// create a static body
+								solverBodyIdA = tmpSolverBodyPool.size();
+								SolverBody solverBody = bodiesPool.get();
+								tmpSolverBodyPool.add(solverBody);
+								initSolverBody(solverBody, colObj0);
+							}
+
+							if (colObj1.getIslandTag() >= 0) {
+								if (colObj1.getCompanionId() >= 0) {
+									solverBodyIdB = colObj1.getCompanionId();
+								}
+								else {
+									solverBodyIdB = tmpSolverBodyPool.size();
+									SolverBody solverBody = bodiesPool.get();
+									tmpSolverBodyPool.add(solverBody);
+									initSolverBody(solverBody, colObj1);
+									colObj1.setCompanionId(solverBodyIdB);
+								}
+							}
+							else {
+								// create a static body
+								solverBodyIdB = tmpSolverBodyPool.size();
+								SolverBody solverBody = bodiesPool.get();
+								tmpSolverBodyPool.add(solverBody);
+								initSolverBody(solverBody, colObj1);
+							}
+						}
+
+						Vector3f rel_pos1 = stack.vectors.get();
+						Vector3f rel_pos2 = stack.vectors.get();
+						float relaxation;
+
+						for (int j = 0; j < manifold.getNumContacts(); j++) {
+
+							ManifoldPoint cp = manifold.getContactPoint(j);
+
+							if (debugDrawer != null) {
+								debugDrawer.drawContactPoint(cp.positionWorldOnB, cp.normalWorldOnB, cp.getDistance(), cp.getLifeTime(), color);
+							}
+
+							if (cp.getDistance() <= 0f) {
+								Vector3f pos1 = cp.getPositionWorldOnA();
+								Vector3f pos2 = cp.getPositionWorldOnB();
+
+								rel_pos1.sub(pos1, colObj0.getWorldTransform().origin);
+								rel_pos2.sub(pos2, colObj1.getWorldTransform().origin);
+
+								relaxation = 1f;
+								float rel_vel;
+								Vector3f vel = stack.vectors.get();
+
+								int frictionIndex = tmpSolverConstraintPool.size();
+
+								{
+									SolverConstraint solverConstraint = constraintsPool.get();
+									tmpSolverConstraintPool.add(solverConstraint);
+									RigidBody rb0 = RigidBody.upcast(colObj0);
+									RigidBody rb1 = RigidBody.upcast(colObj1);
+
+									solverConstraint.solverBodyIdA = solverBodyIdA;
+									solverConstraint.solverBodyIdB = solverBodyIdB;
+									solverConstraint.constraintType = SolverConstraintType.SOLVER_CONTACT_1D;
+
+									Vector3f torqueAxis0 = stack.vectors.get();
+									torqueAxis0.cross(rel_pos1, cp.normalWorldOnB);
+
+									if (rb0 != null) {
+										solverConstraint.angularComponentA.set(torqueAxis0);
+										rb0.getInvInertiaTensorWorld().transform(solverConstraint.angularComponentA);
+									}
+									else {
+										solverConstraint.angularComponentA.set(0f, 0f, 0f);
+									}
+
+									Vector3f torqueAxis1 = stack.vectors.get();
+									torqueAxis1.cross(rel_pos2, cp.normalWorldOnB);
+
+									if (rb1 != null) {
+										solverConstraint.angularComponentB.set(torqueAxis1);
+										rb1.getInvInertiaTensorWorld().transform(solverConstraint.angularComponentB);
+									}
+									else {
+										solverConstraint.angularComponentB.set(0f, 0f, 0f);
+									}
+
+									{
+										//#ifdef COMPUTE_IMPULSE_DENOM
+										//btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB);
+										//btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB);
+										//#else							
+										Vector3f vec = stack.vectors.get();
+										float denom0 = 0f;
+										float denom1 = 0f;
+										if (rb0 != null) {
+											vec.cross(solverConstraint.angularComponentA, rel_pos1);
+											denom0 = rb0.getInvMass() + cp.normalWorldOnB.dot(vec);
+										}
+										if (rb1 != null) {
+											vec.cross(solverConstraint.angularComponentB, rel_pos2);
+											denom1 = rb1.getInvMass() + cp.normalWorldOnB.dot(vec);
+										}
+										//#endif //COMPUTE_IMPULSE_DENOM		
+
+										float denom = relaxation / (denom0 + denom1);
+										solverConstraint.jacDiagABInv = denom;
+									}
+
+									solverConstraint.contactNormal.set(cp.normalWorldOnB);
+									solverConstraint.relpos1CrossNormal.cross(rel_pos1, cp.normalWorldOnB);
+									solverConstraint.relpos2CrossNormal.cross(rel_pos2, cp.normalWorldOnB);
+
+									Vector3f vel1 = rb0 != null ? stack.vectors.get(rb0.getVelocityInLocalPoint(rel_pos1)) : stack.vectors.get(0f, 0f, 0f);
+									Vector3f vel2 = rb1 != null ? stack.vectors.get(rb1.getVelocityInLocalPoint(rel_pos2)) : stack.vectors.get(0f, 0f, 0f);
+
+									vel.sub(vel1, vel2);
+
+									rel_vel = cp.normalWorldOnB.dot(vel);
+
+									solverConstraint.penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations);
+									solverConstraint.friction = cp.combinedFriction;
+									solverConstraint.restitution = restitutionCurve(rel_vel, cp.combinedRestitution);
+									if (solverConstraint.restitution <= 0f) {
+										solverConstraint.restitution = 0f;
+									}
+
+									float penVel = -solverConstraint.penetration / infoGlobal.timeStep;
+									solverConstraint.penetration *= -(infoGlobal.erp / infoGlobal.timeStep);
+
+									if (solverConstraint.restitution > penVel) {
+										solverConstraint.penetration = 0f;
+									}
+
+									solverConstraint.appliedImpulse = 0f;
+									solverConstraint.appliedVelocityImpulse = 0f;
+								}
+
+								{
+									Vector3f frictionDir1 = stack.vectors.get();
+									frictionDir1.scale(rel_vel, cp.normalWorldOnB);
+									frictionDir1.sub(vel, frictionDir1);
+
+									float lat_rel_vel = frictionDir1.lengthSquared();
+									if (lat_rel_vel > BulletGlobals.FLT_EPSILON)//0.0f)
+									{
+										frictionDir1.scale(1f / (float) Math.sqrt(lat_rel_vel));
+										addFrictionConstraint(frictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+										Vector3f frictionDir2 = stack.vectors.get();
+										frictionDir2.cross(frictionDir1, cp.normalWorldOnB);
+										frictionDir2.normalize();//??
+										addFrictionConstraint(frictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+									}
+									else {
+										// re-calculate friction direction every frame, todo: check if this is really needed
+										Vector3f /*frictionDir1 = stack.vectors.get(),*/ frictionDir2 = stack.vectors.get();
+										TransformUtil.planeSpace1(cp.normalWorldOnB, frictionDir1, frictionDir2);
+										addFrictionConstraint(frictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+										addFrictionConstraint(frictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation);
+									}
+								}
+							}
+						}
+					}
+				}
+			}
+
+			// TODO: btContactSolverInfo info = infoGlobal;
+
+			{
+				int j;
+				for (j = 0; j < numConstraints; j++) {
+					TypedConstraint constraint = constraints.get(constraints_offset+j);
+					constraint.buildJacobian();
+				}
+			}
+
+
+
+			int numConstraintPool = tmpSolverConstraintPool.size();
+			int numFrictionPool = tmpSolverFrictionConstraintPool.size();
+
+			// todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
+			MiscUtil.resize(orderTmpConstraintPool, numConstraintPool, 0);
+			MiscUtil.resize(orderFrictionConstraintPool, numFrictionPool, 0);
+			{
+				int i;
+				for (i = 0; i < numConstraintPool; i++) {
+					orderTmpConstraintPool.set(i, i);
+				}
+				for (i = 0; i < numFrictionPool; i++) {
+					orderFrictionConstraintPool.set(i, i);
+				}
+			}
+
+			return 0f;
+		}
+		finally {
+			stack.vectors.pop();
+			BulletGlobals.popProfile();
+		}
+	}
+	
+	public float solveGroupCacheFriendlyIterations(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifoldPtr, int manifold_offset, int numManifolds, List<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer/*,btStackAlloc* stackAlloc*/) {
+		BulletGlobals.pushProfile("solveGroupCacheFriendlyIterations");
+		try {
+			int numConstraintPool = tmpSolverConstraintPool.size();
+			int numFrictionPool = tmpSolverFrictionConstraintPool.size();
+
+			// should traverse the contacts random order...
+			int iteration;
+			{
+				for (iteration = 0; iteration < infoGlobal.numIterations; iteration++) {
+
+					int j;
+					if ((solverMode & SolverMode.SOLVER_RANDMIZE_ORDER) != 0) {
+						if ((iteration & 7) == 0) {
+							for (j = 0; j < numConstraintPool; ++j) {
+								int tmp = orderTmpConstraintPool.get(j);
+								int swapi = randInt2(j + 1);
+								orderTmpConstraintPool.set(j, orderTmpConstraintPool.get(swapi));
+								orderTmpConstraintPool.set(swapi, tmp);
+							}
+
+							for (j = 0; j < numFrictionPool; ++j) {
+								int tmp = orderFrictionConstraintPool.get(j);
+								int swapi = randInt2(j + 1);
+								orderFrictionConstraintPool.set(j, orderFrictionConstraintPool.get(swapi));
+								orderFrictionConstraintPool.set(swapi, tmp);
+							}
+						}
+					}
+
+					for (j = 0; j < numConstraints; j++) {
+						BulletGlobals.pushProfile("solveConstraint");
+						try {
+							TypedConstraint constraint = constraints.get(constraints_offset+j);
+							// todo: use solver bodies, so we don't need to copy from/to btRigidBody
+
+							if ((constraint.getRigidBodyA().getIslandTag() >= 0) && (constraint.getRigidBodyA().getCompanionId() >= 0)) {
+								tmpSolverBodyPool.get(constraint.getRigidBodyA().getCompanionId()).writebackVelocity();
+							}
+							if ((constraint.getRigidBodyB().getIslandTag() >= 0) && (constraint.getRigidBodyB().getCompanionId() >= 0)) {
+								tmpSolverBodyPool.get(constraint.getRigidBodyB().getCompanionId()).writebackVelocity();
+							}
+
+							constraint.solveConstraint(infoGlobal.timeStep);
+
+							if ((constraint.getRigidBodyA().getIslandTag() >= 0) && (constraint.getRigidBodyA().getCompanionId() >= 0)) {
+								tmpSolverBodyPool.get(constraint.getRigidBodyA().getCompanionId()).readVelocity();
+							}
+							if ((constraint.getRigidBodyB().getIslandTag() >= 0) && (constraint.getRigidBodyB().getCompanionId() >= 0)) {
+								tmpSolverBodyPool.get(constraint.getRigidBodyB().getCompanionId()).readVelocity();
+							}
+						}
+						finally {
+							BulletGlobals.popProfile();
+						}
+					}
+
+					{
+						BulletGlobals.pushProfile("resolveSingleCollisionCombinedCacheFriendly");
+						try {
+							int numPoolConstraints = tmpSolverConstraintPool.size();
+							for (j = 0; j < numPoolConstraints; j++) {
+								SolverConstraint solveManifold = tmpSolverConstraintPool.get(orderTmpConstraintPool.get(j));
+								resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool.get(solveManifold.solverBodyIdA),
+										tmpSolverBodyPool.get(solveManifold.solverBodyIdB), solveManifold, infoGlobal);
+							}
+						}
+						finally {
+							BulletGlobals.popProfile();
+						}
+					}
+
+					{
+						BulletGlobals.pushProfile("resolveSingleFrictionCacheFriendly");
+						try {
+							int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size();
+
+							for (j = 0; j < numFrictionPoolConstraints; j++) {
+								SolverConstraint solveManifold = tmpSolverFrictionConstraintPool.get(orderFrictionConstraintPool.get(j));
+								resolveSingleFrictionCacheFriendly(tmpSolverBodyPool.get(solveManifold.solverBodyIdA),
+										tmpSolverBodyPool.get(solveManifold.solverBodyIdB), solveManifold, infoGlobal,
+										tmpSolverConstraintPool.get(solveManifold.frictionIndex).appliedImpulse);
+							}
+						}
+						finally {
+							BulletGlobals.popProfile();
+						}
+					}
+				}
+			}
+
+			return 0f;
+		}
+		finally {
+			BulletGlobals.popProfile();		
+		}
+	}
+
+	public float solveGroupCacheFriendly(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifoldPtr, int manifold_offset, int numManifolds, List<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer/*,btStackAlloc* stackAlloc*/) {
+		int i;
+
+		solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, manifold_offset, numManifolds, constraints, constraints_offset, numConstraints, infoGlobal, debugDrawer/*, stackAlloc*/);
+		solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, manifold_offset, numManifolds, constraints, constraints_offset, numConstraints, infoGlobal, debugDrawer/*, stackAlloc*/);
+
+		for (i = 0; i < tmpSolverBodyPool.size(); i++) {
+			SolverBody body = tmpSolverBodyPool.get(i);
+			body.writebackVelocity();
+			bodiesPool.release(body);
+		}
+
+		//	printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
+
+		/*
+		printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size());
+		printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size());
+		printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size());
+		printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity());
+		printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity());
+		printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity());
+		*/
+
+		tmpSolverBodyPool.clear();
+		
+		for (i=0; i<tmpSolverConstraintPool.size(); i++) {
+			constraintsPool.release(tmpSolverConstraintPool.get(i));
+		}
+		tmpSolverConstraintPool.clear();
+		
+		for (i=0; i<tmpSolverFrictionConstraintPool.size(); i++) {
+			constraintsPool.release(tmpSolverFrictionConstraintPool.get(i));
+		}
+		tmpSolverFrictionConstraintPool.clear();
+
+		return 0f;
+	}
+	
+	/**
+	 * Sequentially applies impulses.
+	 */
+	@Override
+	public float solveGroup(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifoldPtr, int manifold_offset, int numManifolds, List<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, Dispatcher dispatcher) {
+		BulletGlobals.pushProfile("solveGroup");
+		try {
+			// TODO: solver cache friendly
+			if ((getSolverMode() & SolverMode.SOLVER_CACHE_FRIENDLY) != 0) {
+				// you need to provide at least some bodies
+				// SimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY
+				assert (bodies != null);
+				assert (numBodies != 0);
+				float value = solveGroupCacheFriendly(bodies, numBodies, manifoldPtr, manifold_offset, numManifolds, constraints, constraints_offset, numConstraints, infoGlobal, debugDrawer/*,stackAlloc*/);
+				return value;
+			}
+
+			ContactSolverInfo info = new ContactSolverInfo(infoGlobal);
+
+			int numiter = infoGlobal.numIterations;
+
+			int totalPoints = 0;
+			{
+				short j;
+				for (j = 0; j < numManifolds; j++) {
+					PersistentManifold manifold = manifoldPtr.get(manifold_offset+j);
+					prepareConstraints(manifold, info, debugDrawer);
+
+					for (short p = 0; p < manifoldPtr.get(manifold_offset+j).getNumContacts(); p++) {
+						gOrder[totalPoints].manifoldIndex = j;
+						gOrder[totalPoints].pointIndex = p;
+						totalPoints++;
+					}
+				}
+			}
+
+			{
+				int j;
+				for (j = 0; j < numConstraints; j++) {
+					TypedConstraint constraint = constraints.get(constraints_offset+j);
+					constraint.buildJacobian();
+				}
+			}
+
+			// should traverse the contacts random order...
+			int iteration;
+			{
+				for (iteration = 0; iteration < numiter; iteration++) {
+					int j;
+					if ((solverMode & SolverMode.SOLVER_RANDMIZE_ORDER) != 0) {
+						if ((iteration & 7) == 0) {
+							for (j = 0; j < totalPoints; ++j) {
+								// JAVA NOTE: swaps references instead of copying values (but that's fine in this context)
+								OrderIndex tmp = gOrder[j];
+								int swapi = randInt2(j + 1);
+								gOrder[j] = gOrder[swapi];
+								gOrder[swapi] = tmp;
+							}
+						}
+					}
+
+					for (j = 0; j < numConstraints; j++) {
+						TypedConstraint constraint = constraints.get(constraints_offset+j);
+						constraint.solveConstraint(info.timeStep);
+					}
+
+					for (j = 0; j < totalPoints; j++) {
+						PersistentManifold manifold = manifoldPtr.get(manifold_offset+gOrder[j].manifoldIndex);
+						solve((RigidBody) manifold.getBody0(),
+								(RigidBody) manifold.getBody1(), manifold.getContactPoint(gOrder[j].pointIndex), info, iteration, debugDrawer);
+					}
+
+					for (j = 0; j < totalPoints; j++) {
+						PersistentManifold manifold = manifoldPtr.get(manifold_offset+gOrder[j].manifoldIndex);
+						solveFriction((RigidBody) manifold.getBody0(),
+								(RigidBody) manifold.getBody1(), manifold.getContactPoint(gOrder[j].pointIndex), info, iteration, debugDrawer);
+					}
+
+				}
+			}
+
+			return 0f;
+		}
+		finally {
+			BulletGlobals.popProfile();
+		}
+	}
+	
+	protected void prepareConstraints(PersistentManifold manifoldPtr, ContactSolverInfo info, IDebugDraw debugDrawer) {
+		stack.pushCommonMath();
+		try {
+			RigidBody body0 = (RigidBody) manifoldPtr.getBody0();
+			RigidBody body1 = (RigidBody) manifoldPtr.getBody1();
+
+			// only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
+			{
+				//#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
+				//manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform());
+				//#endif //FORCE_REFESH_CONTACT_MANIFOLDS		
+				int numpoints = manifoldPtr.getNumContacts();
+
+				BulletGlobals.gTotalContactPoints += numpoints;
+
+				Vector3f color = stack.vectors.get(0f, 1f, 0f);
+				for (int i = 0; i < numpoints; i++) {
+					ManifoldPoint cp = manifoldPtr.getContactPoint(i);
+					if (cp.getDistance() <= 0f) {
+						Vector3f pos1 = cp.getPositionWorldOnA();
+						Vector3f pos2 = cp.getPositionWorldOnB();
+
+						Vector3f rel_pos1 = stack.vectors.get();
+						rel_pos1.sub(pos1, body0.getCenterOfMassPosition());
+
+						Vector3f rel_pos2 = stack.vectors.get();
+						rel_pos2.sub(pos2, body1.getCenterOfMassPosition());
+
+						// this jacobian entry is re-used for all iterations
+						Matrix3f mat1 = stack.matrices.get(body0.getCenterOfMassTransform().basis);
+						mat1.transpose();
+
+						Matrix3f mat2 = stack.matrices.get(body1.getCenterOfMassTransform().basis);
+						mat2.transpose();
+
+						JacobianEntry jac = jacobiansPool.get();
+						jac.init(mat1, mat2,
+								rel_pos1, rel_pos2, cp.normalWorldOnB, body0.getInvInertiaDiagLocal(), body0.getInvMass(),
+								body1.getInvInertiaDiagLocal(), body1.getInvMass());
+
+						float jacDiagAB = jac.getDiagonal();
+						jacobiansPool.release(jac);
+
+						ConstraintPersistentData cpd = (ConstraintPersistentData) cp.userPersistentData;
+						if (cpd != null) {
+							// might be invalid
+							cpd.persistentLifeTime++;
+							if (cpd.persistentLifeTime != cp.getLifeTime()) {
+								//printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
+								//new (cpd) btConstraintPersistentData;
+								cpd.reset();
+								cpd.persistentLifeTime = cp.getLifeTime();
+
+							}
+							else {
+							//printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
+							}
+						}
+						else {
+							// todo: should this be in a pool?
+							//void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16);
+							//cpd = new (mem)btConstraintPersistentData;
+							cpd = new ConstraintPersistentData();
+							//assert(cpd != null);
+
+							totalCpd++;
+							//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
+							cp.userPersistentData = cpd;
+							cpd.persistentLifeTime = cp.getLifeTime();
+						//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
+						}
+						assert (cpd != null);
+
+						cpd.jacDiagABInv = 1f / jacDiagAB;
+
+						// Dependent on Rigidbody A and B types, fetch the contact/friction response func
+						// perhaps do a similar thing for friction/restutution combiner funcs...
+
+						cpd.frictionSolverFunc = frictionDispatch[body0.frictionSolverType][body1.frictionSolverType];
+						cpd.contactSolverFunc = contactDispatch[body0.contactSolverType][body1.contactSolverType];
+
+						Vector3f vel1 = stack.vectors.get(body0.getVelocityInLocalPoint(rel_pos1));
+						Vector3f vel2 = stack.vectors.get(body1.getVelocityInLocalPoint(rel_pos2));
+						Vector3f vel = stack.vectors.get();
+						vel.sub(vel1, vel2);
+
+						float rel_vel;
+						rel_vel = cp.normalWorldOnB.dot(vel);
+
+						float combinedRestitution = cp.combinedRestitution;
+
+						cpd.penetration = cp.getDistance(); ///btScalar(info.m_numIterations);
+						cpd.friction = cp.combinedFriction;
+						cpd.restitution = restitutionCurve(rel_vel, combinedRestitution);
+						if (cpd.restitution <= 0f) {
+							cpd.restitution = 0f;
+						}
+
+						// restitution and penetration work in same direction so
+						// rel_vel 
+
+						float penVel = -cpd.penetration / info.timeStep;
+
+						if (cpd.restitution > penVel) {
+							cpd.penetration = 0f;
+						}
+
+						float relaxation = info.damping;
+						if ((solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) {
+							cpd.appliedImpulse *= relaxation;
+						}
+						else {
+							cpd.appliedImpulse = 0f;
+						}
+
+						// for friction
+						cpd.prevAppliedImpulse = cpd.appliedImpulse;
+
+						// re-calculate friction direction every frame, todo: check if this is really needed
+						TransformUtil.planeSpace1(cp.normalWorldOnB, cpd.frictionWorldTangential0, cpd.frictionWorldTangential1);
+
+						//#define NO_FRICTION_WARMSTART 1
+						//#ifdef NO_FRICTION_WARMSTART
+						cpd.accumulatedTangentImpulse0 = 0f;
+						cpd.accumulatedTangentImpulse1 = 0f;
+						//#endif //NO_FRICTION_WARMSTART
+						float denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential0);
+						float denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential0);
+						float denom = relaxation / (denom0 + denom1);
+						cpd.jacDiagABInvTangent0 = denom;
+
+						denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential1);
+						denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential1);
+						denom = relaxation / (denom0 + denom1);
+						cpd.jacDiagABInvTangent1 = denom;
+
+						Vector3f totalImpulse = stack.vectors.get();
+						//btVector3 totalImpulse = 
+						//	//#ifndef NO_FRICTION_WARMSTART
+						//	//cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+
+						//	//cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+
+						//	//#endif //NO_FRICTION_WARMSTART
+						//	cp.normalWorldOnB*cpd.appliedImpulse;
+						totalImpulse.scale(cpd.appliedImpulse, cp.normalWorldOnB);
+
+						///
+						{
+							Vector3f torqueAxis0 = stack.vectors.get();
+							torqueAxis0.cross(rel_pos1, cp.normalWorldOnB);
+
+							cpd.angularComponentA.set(torqueAxis0);
+							body0.getInvInertiaTensorWorld().transform(cpd.angularComponentA);
+
+							Vector3f torqueAxis1 = stack.vectors.get();
+							torqueAxis1.cross(rel_pos2, cp.normalWorldOnB);
+
+							cpd.angularComponentB.set(torqueAxis1);
+							body1.getInvInertiaTensorWorld().transform(cpd.angularComponentB);
+						}
+						{
+							Vector3f ftorqueAxis0 = stack.vectors.get();
+							ftorqueAxis0.cross(rel_pos1, cpd.frictionWorldTangential0);
+
+							cpd.frictionAngularComponent0A.set(ftorqueAxis0);
+							body0.getInvInertiaTensorWorld().transform(cpd.frictionAngularComponent0A);
+						}
+						{
+							Vector3f ftorqueAxis1 = stack.vectors.get();
+							ftorqueAxis1.cross(rel_pos1, cpd.frictionWorldTangential1);
+
+							cpd.frictionAngularComponent1A.set(ftorqueAxis1);
+							body0.getInvInertiaTensorWorld().transform(cpd.frictionAngularComponent1A);
+						}
+						{
+							Vector3f ftorqueAxis0 = stack.vectors.get();
+							ftorqueAxis0.cross(rel_pos2, cpd.frictionWorldTangential0);
+
+							cpd.frictionAngularComponent0B.set(ftorqueAxis0);
+							body1.getInvInertiaTensorWorld().transform(cpd.frictionAngularComponent0B);
+						}
+						{
+							Vector3f ftorqueAxis1 = stack.vectors.get();
+							ftorqueAxis1.cross(rel_pos2, cpd.frictionWorldTangential1);
+
+							cpd.frictionAngularComponent1B.set(ftorqueAxis1);
+							body1.getInvInertiaTensorWorld().transform(cpd.frictionAngularComponent1B);
+						}
+
+						///
+
+						// apply previous frames impulse on both bodies
+						body0.applyImpulse(totalImpulse, rel_pos1);
+
+						Vector3f tmp = stack.vectors.get(totalImpulse);
+						tmp.negate();
+						body1.applyImpulse(tmp, rel_pos2);
+					}
+
+				}
+			}
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	public float solveCombinedContactFriction(RigidBody body0, RigidBody body1, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDrawer) {
+		stack.vectors.push();
+		try {
+			float maxImpulse = 0f;
+			
+			Vector3f color = stack.vectors.get(0f, 1f, 0f);
+			{
+				if (cp.getDistance() <= 0f) {
+
+					if (iter == 0) {
+						if (debugDrawer != null) {
+							debugDrawer.drawContactPoint(cp.positionWorldOnB, cp.normalWorldOnB, cp.getDistance(), cp.getLifeTime(), color);
+						}
+					}
+
+					{
+						//btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData;
+						float impulse = ContactConstraint.resolveSingleCollisionCombined(body0, body1, cp, info);
+
+						if (maxImpulse < impulse) {
+							maxImpulse = impulse;
+						}
+					}
+				}
+			}
+			return maxImpulse;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	protected float solve(RigidBody body0, RigidBody body1, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDrawer) {
+		stack.vectors.push();
+		try {
+			float maxImpulse = 0f;
+			
+			Vector3f color = stack.vectors.get(0f, 1f, 0f);
+			{
+				if (cp.getDistance() <= 0f) {
+
+					if (iter == 0) {
+						if (debugDrawer != null) {
+							debugDrawer.drawContactPoint(cp.positionWorldOnB, cp.normalWorldOnB, cp.getDistance(), cp.getLifeTime(), color);
+						}
+					}
+
+					{
+						ConstraintPersistentData cpd = (ConstraintPersistentData) cp.userPersistentData;
+						float impulse = cpd.contactSolverFunc.invoke(body0, body1, cp, info);
+
+						if (maxImpulse < impulse) {
+							maxImpulse = impulse;
+						}
+					}
+				}
+			}
+			
+			return maxImpulse;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	protected float solveFriction(RigidBody body0, RigidBody body1, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDrawer) {
+		stack.vectors.push();
+		try {
+			Vector3f color = stack.vectors.get(0f, 1f, 0f);
+			{
+				if (cp.getDistance() <= 0f) {
+					ConstraintPersistentData cpd = (ConstraintPersistentData) cp.userPersistentData;
+					cpd.frictionSolverFunc.invoke(body0, body1, cp, info);
+				}
+			}
+			return 0f;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	@Override
+	public void reset() {
+		btSeed2 = 0;
+	}
+	
+	/**
+	 * Advanced: Override the default contact solving function for contacts, for certain types of rigidbody<br>
+	 * See RigidBody.contactSolverType and RigidBody.frictionSolverType
+	 */
+	public void setContactSolverFunc(ContactSolverFunc func, int type0, int type1) {
+		contactDispatch[type0][type1] = func;
+	}
+	
+	/**
+	 * Advanced: Override the default friction solving function for contacts, for certain types of rigidbody<br>
+	 * See RigidBody.contactSolverType and RigidBody.frictionSolverType
+	 */
+	public void setFrictionSolverFunc(ContactSolverFunc func, int type0, int type1) {
+		frictionDispatch[type0][type1] = func;
+	}
+
+	public int getSolverMode() {
+		return solverMode;
+	}
+
+	public void setSolverMode(int solverMode) {
+		this.solverMode = solverMode;
+	}
+
+	public void setRandSeed(long seed) {
+		btSeed2 = seed;
+	}
+
+	public long getRandSeed() {
+		return btSeed2;
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	private static class OrderIndex {
+		public int manifoldIndex;
+		public int pointIndex;
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverBody.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverBody.java
new file mode 100644
index 0000000..92f59f7
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverBody.java
@@ -0,0 +1,82 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+import javabullet.BulletStack;
+import javabullet.dynamics.RigidBody;
+import javax.vecmath.Vector3f;
+
+/**
+ * SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
+ * 
+ * @author jezek2
+ */
+public class SolverBody {
+	
+	protected final BulletStack stack = BulletStack.get();
+
+	public final Vector3f angularVelocity = new Vector3f();
+	public float angularFactor;
+	public float invMass;
+	public float friction;
+	public RigidBody originalBody;
+	public final Vector3f linearVelocity = new Vector3f();
+	public final Vector3f centerOfMassPosition = new Vector3f();
+
+	public void getVelocityInLocalPoint(Vector3f rel_pos, Vector3f velocity) {
+		stack.vectors.push();
+		try {
+			Vector3f tmp = stack.vectors.get();
+			tmp.cross(angularVelocity, rel_pos);
+			velocity.add(linearVelocity, tmp);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	/**
+	 * Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position.
+	 */
+	public void internalApplyImpulse(Vector3f linearComponent, Vector3f angularComponent, float impulseMagnitude) {
+		linearVelocity.scaleAdd(impulseMagnitude, linearComponent, linearVelocity);
+		angularVelocity.scaleAdd(impulseMagnitude * angularFactor, angularComponent, angularVelocity);
+	}
+
+	public void writebackVelocity() {
+		if (invMass != 0f) {
+			originalBody.setLinearVelocity(linearVelocity);
+			originalBody.setAngularVelocity(angularVelocity);
+			//m_originalBody->setCompanionId(-1);
+		}
+	}
+
+	public void readVelocity() {
+		if (invMass != 0f) {
+			linearVelocity.set(originalBody.getLinearVelocity());
+			angularVelocity.set(originalBody.getAngularVelocity());
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverConstraint.java
new file mode 100644
index 0000000..70c37af
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverConstraint.java
@@ -0,0 +1,55 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+import javax.vecmath.Vector3f;
+
+/**
+ * 1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
+ * 
+ * @author jezek2
+ */
+public class SolverConstraint {
+
+	public final Vector3f relpos1CrossNormal = new Vector3f();
+	public final Vector3f contactNormal = new Vector3f();
+
+	public final Vector3f relpos2CrossNormal = new Vector3f();
+	public final Vector3f angularComponentA = new Vector3f();
+
+	public final Vector3f angularComponentB = new Vector3f();
+	public float appliedVelocityImpulse;
+	public float appliedImpulse;
+	public int solverBodyIdA;
+	public int solverBodyIdB;
+	
+	public float friction;
+	public float restitution;
+	public float jacDiagABInv;
+	public float penetration;
+	
+	public SolverConstraintType constraintType;
+	public int frictionIndex;
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverConstraintType.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverConstraintType.java
new file mode 100644
index 0000000..eaf6286
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverConstraintType.java
@@ -0,0 +1,33 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+/**
+ *
+ * @author jezek2
+ */
+public enum SolverConstraintType {
+	SOLVER_CONTACT_1D,
+	SOLVER_FRICTION_1D
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverMode.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverMode.java
new file mode 100644
index 0000000..bd90d9b
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverMode.java
@@ -0,0 +1,37 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+/**
+ *
+ * @author jezek2
+ */
+public class SolverMode {
+	
+	public static final int SOLVER_RANDMIZE_ORDER    = 1;
+	public static final int SOLVER_FRICTION_SEPARATE = 2;
+	public static final int SOLVER_USE_WARMSTARTING  = 4;
+	public static final int SOLVER_CACHE_FRIENDLY    = 8;
+
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/TranslationalLimitMotor.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/TranslationalLimitMotor.java
new file mode 100644
index 0000000..d8dafbc
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/TranslationalLimitMotor.java
@@ -0,0 +1,155 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+/*
+2007-09-09
+btGeneric6DofConstraint Refactored by Francisco Le�n
+email: [email protected]
+http://gimpact.sf.net
+*/
+
+package javabullet.dynamics.constraintsolver;
+
+import javabullet.BulletStack;
+import javabullet.dynamics.RigidBody;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class TranslationalLimitMotor {
+	
+	protected final BulletStack stack = BulletStack.get();
+	
+	public final Vector3f lowerLimit = new Vector3f(); //!< the constraint lower limits
+	public final Vector3f upperLimit = new Vector3f(); //!< the constraint upper limits
+	public final Vector3f accumulatedImpulse = new Vector3f();
+	
+	public float limitSoftness; //!< Softness for linear limit
+	public float damping; //!< Damping for linear limit
+	public float restitution; //! Bounce parameter for linear limit
+
+	public TranslationalLimitMotor() {
+		lowerLimit.set(0f, 0f, 0f);
+		upperLimit.set(0f, 0f, 0f);
+		accumulatedImpulse.set(0f, 0f, 0f);
+
+		limitSoftness = 0.7f;
+		damping = 1.0f;
+		restitution = 0.5f;
+	}
+
+	public TranslationalLimitMotor(TranslationalLimitMotor other) {
+		lowerLimit.set(other.lowerLimit);
+		upperLimit.set(other.upperLimit);
+		accumulatedImpulse.set(other.accumulatedImpulse);
+
+		limitSoftness = other.limitSoftness;
+		damping = other.damping;
+		restitution = other.restitution;
+	}
+
+	/**
+	 * Test limit.<p>
+	 * - free means upper &lt; lower,<br>
+	 * - locked means upper == lower<br>
+	 * - limited means upper &gt; lower<br>
+	 * - limitIndex: first 3 are linear, next 3 are angular
+	 */
+	public boolean isLimited(int limitIndex) {
+		return (VectorUtil.getCoord(upperLimit, limitIndex) >= VectorUtil.getCoord(lowerLimit, limitIndex));
+	}
+
+	public float solveLinearAxis(float timeStep, float jacDiagABInv, RigidBody body1, Vector3f pointInA, RigidBody body2, Vector3f pointInB, int limit_index, Vector3f axis_normal_on_a) {
+		stack.vectors.push();
+		try {
+			Vector3f tmp = stack.vectors.get();
+
+			// find relative velocity
+			Vector3f rel_pos1 = stack.vectors.get();
+			rel_pos1.sub(pointInA, body1.getCenterOfMassPosition());
+
+			Vector3f rel_pos2 = stack.vectors.get();
+			rel_pos2.sub(pointInB, body2.getCenterOfMassPosition());
+
+			Vector3f vel1 = stack.vectors.get(body1.getVelocityInLocalPoint(rel_pos1));
+			Vector3f vel2 = stack.vectors.get(body2.getVelocityInLocalPoint(rel_pos2));
+			Vector3f vel = stack.vectors.get();
+			vel.sub(vel1, vel2);
+
+			float rel_vel = axis_normal_on_a.dot(vel);
+
+			// apply displacement correction
+
+			// positional error (zeroth order error)
+			tmp.sub(pointInA, pointInB);
+			float depth = -(tmp).dot(axis_normal_on_a);
+			float lo = -1e30f;
+			float hi = 1e30f;
+
+			float minLimit = VectorUtil.getCoord(lowerLimit, limit_index);
+			float maxLimit = VectorUtil.getCoord(upperLimit, limit_index);
+
+			// handle the limits
+			if (minLimit < maxLimit) {
+				{
+					if (depth > maxLimit) {
+						depth -= maxLimit;
+						lo = 0f;
+
+					}
+					else {
+						if (depth < minLimit) {
+							depth -= minLimit;
+							hi = 0f;
+						}
+						else {
+							return 0.0f;
+						}
+					}
+				}
+			}
+
+			float normalImpulse = limitSoftness * (restitution * depth / timeStep - damping * rel_vel) * jacDiagABInv;
+
+			float oldNormalImpulse = VectorUtil.getCoord(accumulatedImpulse, limit_index);
+			float sum = oldNormalImpulse + normalImpulse;
+			VectorUtil.setCoord(accumulatedImpulse, limit_index, sum > hi ? 0f : sum < lo ? 0f : sum);
+			normalImpulse = VectorUtil.getCoord(accumulatedImpulse, limit_index) - oldNormalImpulse;
+
+			Vector3f impulse_vector = stack.vectors.get();
+			impulse_vector.scale(normalImpulse, axis_normal_on_a);
+			body1.applyImpulse(impulse_vector, rel_pos1);
+
+			tmp.negate(impulse_vector);
+			body2.applyImpulse(tmp, rel_pos2);
+			return normalImpulse;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/TypedConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/TypedConstraint.java
new file mode 100644
index 0000000..e68d0a0
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/TypedConstraint.java
@@ -0,0 +1,101 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javabullet.dynamics.RigidBody;
+
+/**
+ * TypedConstraint is the baseclass for Bullet constraints and vehicles.
+ * 
+ * @author jezek2
+ */
+public abstract class TypedConstraint {
+	
+	protected final BulletStack stack = BulletStack.get();
+	
+	private static final RigidBody s_fixed = new RigidBody(0, null, null);
+
+	private int userConstraintType = -1;
+	private int userConstraintId = -1;
+
+	private TypedConstraintType constraintType;
+	
+	protected RigidBody rbA;
+	protected RigidBody rbB;
+	protected float appliedImpulse = 0f;
+
+	public TypedConstraint(TypedConstraintType type) {
+		this(type, s_fixed, s_fixed);
+	}
+	
+	public TypedConstraint(TypedConstraintType type, RigidBody rbA) {
+		this(type, rbA, s_fixed);
+	}
+	
+	public TypedConstraint(TypedConstraintType type, RigidBody rbA, RigidBody rbB) {
+		this.constraintType = type;
+		this.rbA = rbA;
+		this.rbB = rbB;
+		s_fixed.setMassProps(0f, BulletGlobals.ZERO_VECTOR3);
+	}
+	
+	public abstract void buildJacobian();
+
+	public abstract void solveConstraint(float timeStep);
+	
+	public RigidBody getRigidBodyA() {
+		return rbA;
+	}
+
+	public RigidBody getRigidBodyB() {
+		return rbB;
+	}
+
+	public int getUserConstraintType() {
+		return userConstraintType;
+	}
+
+	public void setUserConstraintType(int userConstraintType) {
+		this.userConstraintType = userConstraintType;
+	}
+
+	public int getUserConstraintId() {
+		return userConstraintId;
+	}
+
+	public void setUserConstraintId(int userConstraintId) {
+		this.userConstraintId = userConstraintId;
+	}
+
+	public float getAppliedImpulse() {
+		return appliedImpulse;
+	}
+
+	public TypedConstraintType getConstraintType() {
+		return constraintType;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/TypedConstraintType.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/TypedConstraintType.java
new file mode 100644
index 0000000..c2f08c0
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/TypedConstraintType.java
@@ -0,0 +1,36 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.constraintsolver;
+
+/**
+ *
+ * @author jezek2
+ */
+public enum TypedConstraintType {
+	POINT2POINT_CONSTRAINT_TYPE,
+	HINGE_CONSTRAINT_TYPE,
+	CONETWIST_CONSTRAINT_TYPE,
+	D6_CONSTRAINT_TYPE,
+	VEHICLE_CONSTRAINT_TYPE
+}
diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/package-info.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/package-info.java
new file mode 100644
index 0000000..08aa9fd
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/package-info.java
@@ -0,0 +1,28 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+/**
+ * Constraints.
+ */
+package javabullet.dynamics.constraintsolver;
+
diff --git a/src/jbullet/src/javabullet/dynamics/package-info.java b/src/jbullet/src/javabullet/dynamics/package-info.java
new file mode 100644
index 0000000..394cf74
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/package-info.java
@@ -0,0 +1,28 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+/**
+ * DynamicsWorld and RigidBody.
+ */
+package javabullet.dynamics;
+
diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/DefaultVehicleRaycaster.java b/src/jbullet/src/javabullet/dynamics/vehicle/DefaultVehicleRaycaster.java
new file mode 100644
index 0000000..c14949e
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/vehicle/DefaultVehicleRaycaster.java
@@ -0,0 +1,63 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.vehicle;
+
+import javabullet.collision.dispatch.CollisionWorld.ClosestRayResultCallback;
+import javabullet.dynamics.DynamicsWorld;
+import javabullet.dynamics.RigidBody;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class DefaultVehicleRaycaster implements VehicleRaycaster {
+
+	protected DynamicsWorld dynamicsWorld;
+
+	public DefaultVehicleRaycaster(DynamicsWorld world) {
+		this.dynamicsWorld = world;
+	}
+
+	public Object castRay(Vector3f from, Vector3f to, VehicleRaycasterResult result) {
+		//RayResultCallback& resultCallback;
+
+		ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(from, to);
+
+		dynamicsWorld.rayTest(from, to, rayCallback);
+
+		if (rayCallback.hasHit()) {
+			RigidBody body = RigidBody.upcast(rayCallback.collisionObject);
+			if (body != null) {
+				result.hitPointInWorld.set(rayCallback.hitPointWorld);
+				result.hitNormalInWorld.set(rayCallback.hitNormalWorld);
+				result.hitNormalInWorld.normalize();
+				result.distFraction = rayCallback.closestHitFraction;
+				return body;
+			}
+		}
+		return null;
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/RaycastVehicle.java b/src/jbullet/src/javabullet/dynamics/vehicle/RaycastVehicle.java
new file mode 100644
index 0000000..c13817f
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/vehicle/RaycastVehicle.java
@@ -0,0 +1,765 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.vehicle;
+
+import java.util.ArrayList;
+import java.util.List;
+import javabullet.dynamics.RigidBody;
+import javabullet.dynamics.constraintsolver.ContactConstraint;
+import javabullet.dynamics.constraintsolver.TypedConstraint;
+import javabullet.dynamics.constraintsolver.TypedConstraintType;
+import javabullet.linearmath.MatrixUtil;
+import javabullet.linearmath.MiscUtil;
+import javabullet.linearmath.QuaternionUtil;
+import javabullet.linearmath.Transform;
+import javabullet.util.FloatArrayList;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Quat4f;
+import javax.vecmath.Vector3f;
+
+/**
+ * Raycast vehicle, very special constraint that turn a rigidbody into a vehicle.
+ * 
+ * @author jezek2
+ */
+public class RaycastVehicle extends TypedConstraint {
+
+	private static RigidBody s_fixedObject = new RigidBody(0, null, null);
+	private static final float sideFrictionStiffness2 = 1.0f;
+	
+	protected List<Vector3f> forwardWS = new ArrayList<Vector3f>();
+	protected List<Vector3f> axle = new ArrayList<Vector3f>();
+	protected FloatArrayList forwardImpulse = new FloatArrayList();
+	protected FloatArrayList sideImpulse = new FloatArrayList();
+
+	private float tau;
+	private float damping;
+	private VehicleRaycaster vehicleRaycaster;
+	private float pitchControl = 0f;
+	private float steeringValue; 
+	private float currentVehicleSpeedKmHour;
+
+	private RigidBody chassisBody;
+
+	private int indexRightAxis = 0;
+	private int indexUpAxis = 2;
+	private int indexForwardAxis = 1;
+	
+	public List<WheelInfo> wheelInfo = new ArrayList<WheelInfo>();
+
+	// constructor to create a car from an existing rigidbody
+	public RaycastVehicle(VehicleTuning tuning, RigidBody chassis, VehicleRaycaster raycaster) {
+		super(TypedConstraintType.VEHICLE_CONSTRAINT_TYPE);
+		this.vehicleRaycaster = raycaster;
+		this.chassisBody = chassis;
+		defaultInit(tuning);
+	}
+	
+	private void defaultInit(VehicleTuning tuning) {
+		currentVehicleSpeedKmHour = 0f;
+		steeringValue = 0f;
+	}
+
+	/**
+	 * Basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed.
+	 */
+	public WheelInfo addWheel(Vector3f connectionPointCS, Vector3f wheelDirectionCS0, Vector3f wheelAxleCS, float suspensionRestLength, float wheelRadius, VehicleTuning tuning, boolean isFrontWheel) {
+		WheelInfoConstructionInfo ci = new WheelInfoConstructionInfo();
+
+		ci.chassisConnectionCS.set(connectionPointCS);
+		ci.wheelDirectionCS.set(wheelDirectionCS0);
+		ci.wheelAxleCS.set(wheelAxleCS);
+		ci.suspensionRestLength = suspensionRestLength;
+		ci.wheelRadius = wheelRadius;
+		ci.suspensionStiffness = tuning.suspensionStiffness;
+		ci.wheelsDampingCompression = tuning.suspensionCompression;
+		ci.wheelsDampingRelaxation = tuning.suspensionDamping;
+		ci.frictionSlip = tuning.frictionSlip;
+		ci.bIsFrontWheel = isFrontWheel;
+		ci.maxSuspensionTravelCm = tuning.maxSuspensionTravelCm;
+
+		wheelInfo.add(new WheelInfo(ci));
+
+		WheelInfo wheel = wheelInfo.get(getNumWheels() - 1);
+
+		updateWheelTransformsWS(wheel, false);
+		updateWheelTransform(getNumWheels() - 1, false);
+		return wheel;
+	}
+
+	public Transform getWheelTransformWS(int wheelIndex) {
+		assert (wheelIndex < getNumWheels());
+		WheelInfo wheel = wheelInfo.get(wheelIndex);
+		return wheel.worldTransform;
+	}
+
+	public void updateWheelTransform(int wheelIndex) {
+		updateWheelTransform(wheelIndex, true);
+	}
+	
+	public void updateWheelTransform(int wheelIndex, boolean interpolatedTransform) {
+		stack.vectors.push();
+		stack.quats.push();
+		stack.matrices.push();
+		try {
+			WheelInfo wheel = wheelInfo.get(wheelIndex);
+			updateWheelTransformsWS(wheel, interpolatedTransform);
+			Vector3f up = stack.vectors.get();
+			up.negate(wheel.raycastInfo.wheelDirectionWS);
+			Vector3f right = wheel.raycastInfo.wheelAxleWS;
+			Vector3f fwd = stack.vectors.get();
+			fwd.cross(up, right);
+			fwd.normalize();
+			// up = right.cross(fwd);
+			// up.normalize();
+
+			// rotate around steering over de wheelAxleWS
+			float steering = wheel.steering;
+
+			Quat4f steeringOrn = stack.quats.get();
+			QuaternionUtil.setRotation(steeringOrn, up, steering); //wheel.m_steering);
+			Matrix3f steeringMat = stack.matrices.get();
+			MatrixUtil.setRotation(steeringMat, steeringOrn);
+
+			Quat4f rotatingOrn = stack.quats.get();
+			QuaternionUtil.setRotation(rotatingOrn, right, -wheel.rotation);
+			Matrix3f rotatingMat = stack.matrices.get();
+			MatrixUtil.setRotation(rotatingMat, rotatingOrn);
+
+			Matrix3f basis2 = stack.matrices.get();
+			basis2.setRow(0, right.x, fwd.x, up.x);
+			basis2.setRow(1, right.y, fwd.y, up.y);
+			basis2.setRow(2, right.z, fwd.z, up.z);
+
+			Matrix3f wheelBasis = wheel.worldTransform.basis;
+			wheelBasis.mul(steeringMat, rotatingMat);
+			wheelBasis.mul(basis2);
+
+			wheel.worldTransform.origin.scaleAdd(wheel.raycastInfo.suspensionLength, wheel.raycastInfo.wheelDirectionWS, wheel.raycastInfo.hardPointWS);
+		}
+		finally {
+			stack.vectors.pop();
+			stack.quats.pop();
+			stack.matrices.pop();
+		}
+	}
+	
+	public void resetSuspension() {
+		int i;
+		for (i = 0; i < wheelInfo.size(); i++) {
+			WheelInfo wheel = wheelInfo.get(i);
+			wheel.raycastInfo.suspensionLength = wheel.getSuspensionRestLength();
+			wheel.suspensionRelativeVelocity = 0f;
+
+			wheel.raycastInfo.contactNormalWS.negate(wheel.raycastInfo.wheelDirectionWS);
+			//wheel_info.setContactFriction(btScalar(0.0));
+			wheel.clippedInvContactDotSuspension = 1f;
+		}
+	}
+
+	public void updateWheelTransformsWS(WheelInfo wheel) {
+		updateWheelTransformsWS(wheel, true);
+	}
+	
+	public void updateWheelTransformsWS(WheelInfo wheel, boolean interpolatedTransform) {
+		stack.transforms.push();
+		try {
+			wheel.raycastInfo.isInContact = false;
+
+			Transform chassisTrans = stack.transforms.get(getChassisWorldTransform());
+			if (interpolatedTransform && (getRigidBody().getMotionState() != null)) {
+				getRigidBody().getMotionState().getWorldTransform(chassisTrans);
+			}
+
+			wheel.raycastInfo.hardPointWS.set(wheel.chassisConnectionPointCS);
+			chassisTrans.transform(wheel.raycastInfo.hardPointWS);
+
+			wheel.raycastInfo.wheelDirectionWS.set(wheel.wheelDirectionCS);
+			chassisTrans.basis.transform(wheel.raycastInfo.wheelDirectionWS);
+
+			wheel.raycastInfo.wheelAxleWS.set(wheel.wheelAxleCS);
+			chassisTrans.basis.transform(wheel.raycastInfo.wheelAxleWS);
+		}
+		finally {
+			stack.transforms.pop();
+		}
+	}
+
+	public float rayCast(WheelInfo wheel) {
+		stack.vectors.push();
+		try {
+			updateWheelTransformsWS(wheel, false);
+
+			float depth = -1f;
+
+			float raylen = wheel.getSuspensionRestLength() + wheel.wheelsRadius;
+
+			Vector3f rayvector = stack.vectors.get();
+			rayvector.scale(raylen, wheel.raycastInfo.wheelDirectionWS);
+			Vector3f source = wheel.raycastInfo.hardPointWS;
+			wheel.raycastInfo.contactPointWS.add(source, rayvector);
+			Vector3f target = wheel.raycastInfo.contactPointWS;
+
+			float param = 0f;
+
+			VehicleRaycasterResult rayResults = new VehicleRaycasterResult();
+
+			assert (vehicleRaycaster != null);
+
+			Object object = vehicleRaycaster.castRay(source, target, rayResults);
+
+			wheel.raycastInfo.groundObject = null;
+
+			if (object != null) {
+				param = rayResults.distFraction;
+				depth = raylen * rayResults.distFraction;
+				wheel.raycastInfo.contactNormalWS.set(rayResults.hitNormalInWorld);
+				wheel.raycastInfo.isInContact = true;
+
+				wheel.raycastInfo.groundObject = s_fixedObject; // todo for driving on dynamic/movable objects!;
+				//wheel.m_raycastInfo.m_groundObject = object;
+
+				float hitDistance = param * raylen;
+				wheel.raycastInfo.suspensionLength = hitDistance - wheel.wheelsRadius;
+				// clamp on max suspension travel
+
+				float minSuspensionLength = wheel.getSuspensionRestLength() - wheel.maxSuspensionTravelCm * 0.01f;
+				float maxSuspensionLength = wheel.getSuspensionRestLength() + wheel.maxSuspensionTravelCm * 0.01f;
+				if (wheel.raycastInfo.suspensionLength < minSuspensionLength) {
+					wheel.raycastInfo.suspensionLength = minSuspensionLength;
+				}
+				if (wheel.raycastInfo.suspensionLength > maxSuspensionLength) {
+					wheel.raycastInfo.suspensionLength = maxSuspensionLength;
+				}
+
+				wheel.raycastInfo.contactPointWS.set(rayResults.hitPointInWorld);
+
+				float denominator = wheel.raycastInfo.contactNormalWS.dot(wheel.raycastInfo.wheelDirectionWS);
+
+				Vector3f chassis_velocity_at_contactPoint = stack.vectors.get();
+				Vector3f relpos = stack.vectors.get();
+				relpos.sub(wheel.raycastInfo.contactPointWS, getRigidBody().getCenterOfMassPosition());
+
+				chassis_velocity_at_contactPoint.set(getRigidBody().getVelocityInLocalPoint(relpos));
+
+				float projVel = wheel.raycastInfo.contactNormalWS.dot(chassis_velocity_at_contactPoint);
+
+				if (denominator >= -0.1f) {
+					wheel.suspensionRelativeVelocity = 0f;
+					wheel.clippedInvContactDotSuspension = 1f / 0.1f;
+				}
+				else {
+					float inv = -1f / denominator;
+					wheel.suspensionRelativeVelocity = projVel * inv;
+					wheel.clippedInvContactDotSuspension = inv;
+				}
+
+			}
+			else {
+				// put wheel info as in rest position
+				wheel.raycastInfo.suspensionLength = wheel.getSuspensionRestLength();
+				wheel.suspensionRelativeVelocity = 0f;
+				wheel.raycastInfo.contactNormalWS.negate(wheel.raycastInfo.wheelDirectionWS);
+				wheel.clippedInvContactDotSuspension = 1f;
+			}
+
+			return depth;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public Transform getChassisWorldTransform() {
+		/*
+		if (getRigidBody()->getMotionState())
+		{
+			btTransform chassisWorldTrans;
+			getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans);
+			return chassisWorldTrans;
+		}
+		*/
+
+		return getRigidBody().getCenterOfMassTransform();
+	}
+	
+	public void updateVehicle(float step) {
+		stack.vectors.push();
+		stack.transforms.push();
+		try {
+			for (int i = 0; i < getNumWheels(); i++) {
+				updateWheelTransform(i, false);
+			}
+
+			currentVehicleSpeedKmHour = 3.6f * getRigidBody().getLinearVelocity().length();
+
+			Transform chassisTrans = stack.transforms.get(getChassisWorldTransform());
+
+			Vector3f forwardW = stack.vectors.get(
+					chassisTrans.basis.getElement(0, indexForwardAxis),
+					chassisTrans.basis.getElement(1, indexForwardAxis),
+					chassisTrans.basis.getElement(2, indexForwardAxis));
+
+			if (forwardW.dot(getRigidBody().getLinearVelocity()) < 0f) {
+				currentVehicleSpeedKmHour *= -1f;
+			}
+
+			//
+			// simulate suspension
+			//
+
+			int i = 0;
+			for (i = 0; i < wheelInfo.size(); i++) {
+				float depth;
+				depth = rayCast(wheelInfo.get(i));
+			}
+
+			updateSuspension(step);
+
+			for (i = 0; i < wheelInfo.size(); i++) {
+				// apply suspension force
+				WheelInfo wheel = wheelInfo.get(i);
+
+				float suspensionForce = wheel.wheelsSuspensionForce;
+
+				float gMaxSuspensionForce = 6000f;
+				if (suspensionForce > gMaxSuspensionForce) {
+					suspensionForce = gMaxSuspensionForce;
+				}
+				Vector3f impulse = stack.vectors.get();
+				impulse.scale(suspensionForce * step, wheel.raycastInfo.contactNormalWS);
+				Vector3f relpos = stack.vectors.get();
+				relpos.sub(wheel.raycastInfo.contactPointWS, getRigidBody().getCenterOfMassPosition());
+
+				getRigidBody().applyImpulse(impulse, relpos);
+			}
+
+			updateFriction(step);
+
+			for (i = 0; i < wheelInfo.size(); i++) {
+				WheelInfo wheel = wheelInfo.get(i);
+				Vector3f relpos = stack.vectors.get();
+				relpos.sub(wheel.raycastInfo.hardPointWS, getRigidBody().getCenterOfMassPosition());
+				Vector3f vel = stack.vectors.get(getRigidBody().getVelocityInLocalPoint(relpos));
+
+				if (wheel.raycastInfo.isInContact) {
+					Transform chassisWorldTransform = stack.transforms.get(getChassisWorldTransform());
+
+					Vector3f fwd = stack.vectors.get(
+							chassisWorldTransform.basis.getElement(0, indexForwardAxis),
+							chassisWorldTransform.basis.getElement(1, indexForwardAxis),
+							chassisWorldTransform.basis.getElement(2, indexForwardAxis));
+
+					float proj = fwd.dot(wheel.raycastInfo.contactNormalWS);
+					Vector3f tmp = stack.vectors.get();
+					tmp.scale(proj, wheel.raycastInfo.contactNormalWS);
+					fwd.sub(tmp);
+
+					float proj2 = fwd.dot(vel);
+
+					wheel.deltaRotation = (proj2 * step) / (wheel.wheelsRadius);
+					wheel.rotation += wheel.deltaRotation;
+
+				}
+				else {
+					wheel.rotation += wheel.deltaRotation;
+				}
+
+				wheel.deltaRotation *= 0.99f; // damping of rotation when not in contact
+			}
+		}
+		finally {
+			stack.vectors.pop();
+			stack.transforms.pop();
+		}
+	}
+
+	public void setSteeringValue(float steering, int wheel) {
+		assert (wheel >= 0 && wheel < getNumWheels());
+
+		WheelInfo wheel_info = getWheelInfo(wheel);
+		wheel_info.steering = steering;
+	}
+	
+	public float getSteeringValue(int wheel) {
+		return getWheelInfo(wheel).steering;
+	}
+
+	public void applyEngineForce(float force, int wheel) {
+		assert (wheel >= 0 && wheel < getNumWheels());
+		WheelInfo wheel_info = getWheelInfo(wheel);
+		wheel_info.engineForce = force;
+	}
+
+	public WheelInfo getWheelInfo(int index) {
+		assert ((index >= 0) && (index < getNumWheels()));
+
+		return wheelInfo.get(index);
+	}
+
+	public void setBrake(float brake, int wheelIndex) {
+		assert ((wheelIndex >= 0) && (wheelIndex < getNumWheels()));
+		getWheelInfo(wheelIndex).brake = brake;
+	}
+
+	public void updateSuspension(float deltaTime) {
+		float chassisMass = 1f / chassisBody.getInvMass();
+
+		for (int w_it = 0; w_it < getNumWheels(); w_it++) {
+			WheelInfo wheel_info = wheelInfo.get(w_it);
+
+			if (wheel_info.raycastInfo.isInContact) {
+				float force;
+				//	Spring
+				{
+					float susp_length = wheel_info.getSuspensionRestLength();
+					float current_length = wheel_info.raycastInfo.suspensionLength;
+
+					float length_diff = (susp_length - current_length);
+
+					force = wheel_info.suspensionStiffness * length_diff * wheel_info.clippedInvContactDotSuspension;
+				}
+
+				// Damper
+				{
+					float projected_rel_vel = wheel_info.suspensionRelativeVelocity;
+					{
+						float susp_damping;
+						if (projected_rel_vel < 0f) {
+							susp_damping = wheel_info.wheelsDampingCompression;
+						}
+						else {
+							susp_damping = wheel_info.wheelsDampingRelaxation;
+						}
+						force -= susp_damping * projected_rel_vel;
+					}
+				}
+
+				// RESULT
+				wheel_info.wheelsSuspensionForce = force * chassisMass;
+				if (wheel_info.wheelsSuspensionForce < 0f) {
+					wheel_info.wheelsSuspensionForce = 0f;
+				}
+			}
+			else {
+				wheel_info.wheelsSuspensionForce = 0f;
+			}
+		}
+	}
+	
+	private float calcRollingFriction(WheelContactPoint contactPoint) {
+		stack.vectors.push();
+		try {
+			float j1 = 0f;
+
+			Vector3f contactPosWorld = contactPoint.frictionPositionWorld;
+
+			Vector3f rel_pos1 = stack.vectors.get();
+			rel_pos1.sub(contactPosWorld, contactPoint.body0.getCenterOfMassPosition());
+			Vector3f rel_pos2 = stack.vectors.get();
+			rel_pos2.sub(contactPosWorld, contactPoint.body1.getCenterOfMassPosition());
+
+			float maxImpulse = contactPoint.maxImpulse;
+
+			Vector3f vel1 = stack.vectors.get(contactPoint.body0.getVelocityInLocalPoint(rel_pos1));
+			Vector3f vel2 = stack.vectors.get(contactPoint.body1.getVelocityInLocalPoint(rel_pos2));
+			Vector3f vel = stack.vectors.get();
+			vel.sub(vel1, vel2);
+
+			float vrel = contactPoint.frictionDirectionWorld.dot(vel);
+
+			// calculate j that moves us to zero relative velocity
+			j1 = -vrel * contactPoint.jacDiagABInv;
+			j1 = Math.min(j1, maxImpulse);
+			j1 = Math.max(j1, -maxImpulse);
+
+			return j1;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public void updateFriction(float timeStep) {
+		stack.vectors.push();
+		stack.matrices.push();
+		try {
+			// calculate the impulse, so that the wheels don't move sidewards
+			int numWheel = getNumWheels();
+			if (numWheel == 0) {
+				return;
+			}
+
+			MiscUtil.resize(forwardWS, numWheel, Vector3f.class);
+			MiscUtil.resize(axle, numWheel, Vector3f.class);
+			MiscUtil.resize(forwardImpulse, numWheel, 0f);
+			MiscUtil.resize(sideImpulse, numWheel, 0f);
+
+			Vector3f tmp = stack.vectors.get();
+
+			int numWheelsOnGround = 0;
+
+			// collapse all those loops into one!
+			for (int i = 0; i < getNumWheels(); i++) {
+				WheelInfo wheel_info = wheelInfo.get(i);
+				RigidBody groundObject = (RigidBody) wheel_info.raycastInfo.groundObject;
+				if (groundObject != null) {
+					numWheelsOnGround++;
+				}
+				sideImpulse.set(i, 0f);
+				forwardImpulse.set(i, 0f);
+			}
+
+			{
+				for (int i = 0; i < getNumWheels(); i++) {
+
+					WheelInfo wheel_info = wheelInfo.get(i);
+
+					RigidBody groundObject = (RigidBody) wheel_info.raycastInfo.groundObject;
+
+					if (groundObject != null) {
+						Transform wheelTrans = getWheelTransformWS(i);
+
+						Matrix3f wheelBasis0 = stack.matrices.get(wheelTrans.basis);
+						axle.get(i).set(
+								wheelBasis0.getElement(0, indexRightAxis),
+								wheelBasis0.getElement(1, indexRightAxis),
+								wheelBasis0.getElement(2, indexRightAxis));
+
+						Vector3f surfNormalWS = wheel_info.raycastInfo.contactNormalWS;
+						float proj = axle.get(i).dot(surfNormalWS);
+						tmp.scale(proj, surfNormalWS);
+						axle.get(i).sub(tmp);
+						axle.get(i).normalize();
+
+						forwardWS.get(i).cross(surfNormalWS, axle.get(i));
+						forwardWS.get(i).normalize();
+
+						float[] floatPtr = stack.floatArrays.getFixed(1);
+						ContactConstraint.resolveSingleBilateral(chassisBody, wheel_info.raycastInfo.contactPointWS,
+								groundObject, wheel_info.raycastInfo.contactPointWS,
+								0f, axle.get(i), floatPtr, timeStep);
+						sideImpulse.set(i, floatPtr[0]);
+						stack.floatArrays.release(floatPtr);
+
+						sideImpulse.set(i, sideImpulse.get(i) * sideFrictionStiffness2);
+					}
+				}
+			}
+
+			float sideFactor = 1f;
+			float fwdFactor = 0.5f;
+
+			boolean sliding = false;
+			{
+				for (int wheel = 0; wheel < getNumWheels(); wheel++) {
+					WheelInfo wheel_info = wheelInfo.get(wheel);
+					RigidBody groundObject = (RigidBody) wheel_info.raycastInfo.groundObject;
+
+					float rollingFriction = 0f;
+
+					if (groundObject != null) {
+						if (wheel_info.engineForce != 0f) {
+							rollingFriction = wheel_info.engineForce * timeStep;
+						}
+						else {
+							float defaultRollingFrictionImpulse = 0f;
+							float maxImpulse = wheel_info.brake != 0f ? wheel_info.brake : defaultRollingFrictionImpulse;
+							WheelContactPoint contactPt = new WheelContactPoint(chassisBody, groundObject, wheel_info.raycastInfo.contactPointWS, forwardWS.get(wheel), maxImpulse);
+							rollingFriction = calcRollingFriction(contactPt);
+						}
+					}
+
+					// switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
+
+					forwardImpulse.set(wheel, 0f);
+					wheelInfo.get(wheel).skidInfo = 1f;
+
+					if (groundObject != null) {
+						wheelInfo.get(wheel).skidInfo = 1f;
+
+						float maximp = wheel_info.wheelsSuspensionForce * timeStep * wheel_info.frictionSlip;
+						float maximpSide = maximp;
+
+						float maximpSquared = maximp * maximpSide;
+
+						forwardImpulse.set(wheel, rollingFriction); //wheelInfo.m_engineForce* timeStep;
+
+						float x = (forwardImpulse.get(wheel)) * fwdFactor;
+						float y = (sideImpulse.get(wheel)) * sideFactor;
+
+						float impulseSquared = (x * x + y * y);
+
+						if (impulseSquared > maximpSquared) {
+							sliding = true;
+
+							float factor = maximp / (float) Math.sqrt(impulseSquared);
+
+							wheelInfo.get(wheel).skidInfo *= factor;
+						}
+					}
+
+				}
+			}
+
+			if (sliding) {
+				for (int wheel = 0; wheel < getNumWheels(); wheel++) {
+					if (sideImpulse.get(wheel) != 0f) {
+						if (wheelInfo.get(wheel).skidInfo < 1f) {
+							forwardImpulse.set(wheel, forwardImpulse.get(wheel) * wheelInfo.get(wheel).skidInfo);
+							sideImpulse.set(wheel, sideImpulse.get(wheel) * wheelInfo.get(wheel).skidInfo);
+						}
+					}
+				}
+			}
+
+			// apply the impulses
+			{
+				for (int wheel = 0; wheel < getNumWheels(); wheel++) {
+					WheelInfo wheel_info = wheelInfo.get(wheel);
+
+					Vector3f rel_pos = stack.vectors.get();
+					rel_pos.sub(wheel_info.raycastInfo.contactPointWS, chassisBody.getCenterOfMassPosition());
+
+					if (forwardImpulse.get(wheel) != 0f) {
+						tmp.scale(forwardImpulse.get(wheel), forwardWS.get(wheel));
+						chassisBody.applyImpulse(tmp, rel_pos);
+					}
+					if (sideImpulse.get(wheel) != 0f) {
+						RigidBody groundObject = (RigidBody) wheelInfo.get(wheel).raycastInfo.groundObject;
+
+						Vector3f rel_pos2 = stack.vectors.get();
+						rel_pos2.sub(wheel_info.raycastInfo.contactPointWS, groundObject.getCenterOfMassPosition());
+
+						Vector3f sideImp = stack.vectors.get();
+						sideImp.scale(sideImpulse.get(wheel), axle.get(wheel));
+
+						rel_pos.z *= wheel_info.rollInfluence;
+						chassisBody.applyImpulse(sideImp, rel_pos);
+
+						// apply friction impulse on the ground
+						tmp.negate(sideImp);
+						groundObject.applyImpulse(tmp, rel_pos2);
+					}
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+			stack.matrices.pop();
+		}
+	}
+	
+	@Override
+	public void buildJacobian() {
+		// not yet
+	}
+
+	@Override
+	public void solveConstraint(float timeStep) {
+		// not yet
+	}
+	
+	public int getNumWheels() {
+		return wheelInfo.size();
+	}
+
+	public void setPitchControl(float pitch) {
+		this.pitchControl = pitch;
+	}
+
+	public RigidBody getRigidBody() {
+		return chassisBody;
+	}
+
+	public int getRightAxis() {
+		return indexRightAxis;
+	}
+
+	public int getUpAxis() {
+		return indexUpAxis;
+	}
+
+	public int getForwardAxis() {
+		return indexForwardAxis;
+	}
+
+	/**
+	 * Worldspace forward vector.
+	 */
+	public Vector3f getForwardVector() {
+		stack.vectors.push();
+		try {
+			Transform chassisTrans = getChassisWorldTransform();
+
+			Vector3f forwardW = stack.vectors.get(
+					chassisTrans.basis.getElement(0, indexForwardAxis),
+					chassisTrans.basis.getElement(1, indexForwardAxis),
+					chassisTrans.basis.getElement(2, indexForwardAxis));
+
+			return stack.vectors.returning(forwardW);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	/**
+	 * Velocity of vehicle (positive if velocity vector has same direction as foward vector).
+	 */
+	public float getCurrentSpeedKmHour() {
+		return currentVehicleSpeedKmHour;
+	}
+
+	public void setCoordinateSystem(int rightIndex, int upIndex, int forwardIndex) {
+		this.indexRightAxis = rightIndex;
+		this.indexUpAxis = upIndex;
+		this.indexForwardAxis = forwardIndex;
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	private static class WheelContactPoint {
+		public RigidBody body0;
+		public RigidBody body1;
+		public final Vector3f frictionPositionWorld = new Vector3f();
+		public final Vector3f frictionDirectionWorld = new Vector3f();
+		public float jacDiagABInv;
+		public float maxImpulse;
+
+		public WheelContactPoint(RigidBody body0, RigidBody body1, Vector3f frictionPosWorld, Vector3f frictionDirectionWorld, float maxImpulse) {
+			this.body0 = body0;
+			this.body1 = body1;
+			this.frictionPositionWorld.set(frictionPosWorld);
+			this.frictionDirectionWorld.set(frictionDirectionWorld);
+			this.maxImpulse = maxImpulse;
+
+			float denom0 = body0.computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
+			float denom1 = body1.computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld);
+			float relaxation = 1f;
+			jacDiagABInv = relaxation / (denom0 + denom1);
+		}
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/VehicleRaycaster.java b/src/jbullet/src/javabullet/dynamics/vehicle/VehicleRaycaster.java
new file mode 100644
index 0000000..5fff289
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/vehicle/VehicleRaycaster.java
@@ -0,0 +1,37 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.vehicle;
+
+import javax.vecmath.Vector3f;
+
+/**
+ * VehicleRaycaster is provides interface for between vehicle simulation and raycasting.
+ * 
+ * @author jezek2
+ */
+public interface VehicleRaycaster {
+
+	public Object castRay(Vector3f from, Vector3f to, VehicleRaycasterResult result);
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/VehicleRaycasterResult.java b/src/jbullet/src/javabullet/dynamics/vehicle/VehicleRaycasterResult.java
new file mode 100644
index 0000000..b948341
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/vehicle/VehicleRaycasterResult.java
@@ -0,0 +1,38 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.vehicle;
+
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class VehicleRaycasterResult {
+	
+	public final Vector3f hitPointInWorld  = new Vector3f();
+	public final Vector3f hitNormalInWorld  = new Vector3f();
+	public float distFraction = -1f;
+
+}
diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/VehicleTuning.java b/src/jbullet/src/javabullet/dynamics/vehicle/VehicleTuning.java
new file mode 100644
index 0000000..47a6f29
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/vehicle/VehicleTuning.java
@@ -0,0 +1,38 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.vehicle;
+
+/**
+ *
+ * @author jezek2
+ */
+public class VehicleTuning {
+
+	public float suspensionStiffness = 5.88f;
+	public float suspensionCompression = 0.83f;
+	public float suspensionDamping = 0.88f;
+	public float maxSuspensionTravelCm = 500f;
+	public float frictionSlip = 10.5f;
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/WheelInfo.java b/src/jbullet/src/javabullet/dynamics/vehicle/WheelInfo.java
new file mode 100644
index 0000000..d8f9643
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/vehicle/WheelInfo.java
@@ -0,0 +1,145 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.vehicle;
+
+import javabullet.BulletStack;
+import javabullet.dynamics.RigidBody;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * WheelInfo contains information per wheel about friction and suspension.
+ * 
+ * @author jezek2
+ */
+public class WheelInfo {
+
+	protected final BulletStack stack = BulletStack.get();
+	
+	public final RaycastInfo raycastInfo = new RaycastInfo();
+
+	public final Transform worldTransform = new Transform();
+	
+	public final Vector3f chassisConnectionPointCS = new Vector3f(); // const
+	public final Vector3f wheelDirectionCS = new Vector3f(); // const
+	public final Vector3f wheelAxleCS = new Vector3f(); // const or modified by steering
+	public float suspensionRestLength1; // const
+	public float maxSuspensionTravelCm;
+	public float wheelsRadius; // const
+	public float suspensionStiffness; // const
+	public float wheelsDampingCompression; // const
+	public float wheelsDampingRelaxation; // const
+	public float frictionSlip;
+	public float steering;
+	public float rotation;
+	public float deltaRotation;
+	public float rollInfluence;
+
+	public float engineForce;
+
+	public float brake;
+	
+	public boolean bIsFrontWheel;
+	
+	public Object clientInfo; // can be used to store pointer to sync transforms...
+
+	public float clippedInvContactDotSuspension;
+	public float suspensionRelativeVelocity;
+	// calculated by suspension
+	public float wheelsSuspensionForce;
+	public float skidInfo;
+	
+	public WheelInfo(WheelInfoConstructionInfo ci) {
+		suspensionRestLength1 = ci.suspensionRestLength;
+		maxSuspensionTravelCm = ci.maxSuspensionTravelCm;
+
+		wheelsRadius = ci.wheelRadius;
+		suspensionStiffness = ci.suspensionStiffness;
+		wheelsDampingCompression = ci.wheelsDampingCompression;
+		wheelsDampingRelaxation = ci.wheelsDampingRelaxation;
+		chassisConnectionPointCS.set(ci.chassisConnectionCS);
+		wheelDirectionCS.set(ci.wheelDirectionCS);
+		wheelAxleCS.set(ci.wheelAxleCS);
+		frictionSlip = ci.frictionSlip;
+		steering = 0f;
+		engineForce = 0f;
+		rotation = 0f;
+		deltaRotation = 0f;
+		brake = 0f;
+		rollInfluence = 0.1f;
+		bIsFrontWheel = ci.bIsFrontWheel;
+	}
+	
+	public float getSuspensionRestLength() {
+		return suspensionRestLength1;
+	}
+
+	public void updateWheel(RigidBody chassis, RaycastInfo raycastInfo) {
+		stack.vectors.push();
+		try {
+			if (raycastInfo.isInContact) {
+				float project = raycastInfo.contactNormalWS.dot(raycastInfo.wheelDirectionWS);
+				Vector3f chassis_velocity_at_contactPoint = stack.vectors.get();
+				Vector3f relpos = stack.vectors.get();
+				relpos.sub(raycastInfo.contactPointWS, chassis.getCenterOfMassPosition());
+				chassis_velocity_at_contactPoint.set(chassis.getVelocityInLocalPoint(relpos));
+				float projVel = raycastInfo.contactNormalWS.dot(chassis_velocity_at_contactPoint);
+				if (project >= -0.1f) {
+					suspensionRelativeVelocity = 0f;
+					clippedInvContactDotSuspension = 1f / 0.1f;
+				}
+				else {
+					float inv = -1f / project;
+					suspensionRelativeVelocity = projVel * inv;
+					clippedInvContactDotSuspension = inv;
+				}
+			}
+			else {
+				// Not in contact : position wheel in a nice (rest length) position
+				raycastInfo.suspensionLength = getSuspensionRestLength();
+				suspensionRelativeVelocity = 0f;
+				raycastInfo.contactNormalWS.negate(raycastInfo.wheelDirectionWS);
+				clippedInvContactDotSuspension = 1f;
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	public static class RaycastInfo {
+		// set by raycaster
+		public final Vector3f contactNormalWS = new Vector3f(); // contactnormal
+		public final Vector3f contactPointWS = new Vector3f(); // raycast hitpoint
+		public float suspensionLength;
+		public final Vector3f hardPointWS = new Vector3f(); // raycast starting point
+		public final Vector3f wheelDirectionWS = new Vector3f(); // direction in worldspace
+		public final Vector3f wheelAxleWS = new Vector3f(); // axle in worldspace
+		public boolean isInContact;
+		public Object groundObject; // could be general void* ptr
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/WheelInfoConstructionInfo.java b/src/jbullet/src/javabullet/dynamics/vehicle/WheelInfoConstructionInfo.java
new file mode 100644
index 0000000..2824bdb
--- /dev/null
+++ b/src/jbullet/src/javabullet/dynamics/vehicle/WheelInfoConstructionInfo.java
@@ -0,0 +1,47 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.dynamics.vehicle;
+
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class WheelInfoConstructionInfo {
+
+	public final Vector3f chassisConnectionCS = new Vector3f();
+	public final Vector3f wheelDirectionCS = new Vector3f();
+	public final Vector3f wheelAxleCS = new Vector3f();
+	public float suspensionRestLength;
+	public float maxSuspensionTravelCm;
+	public float wheelRadius;
+	
+	public float suspensionStiffness;
+	public float wheelsDampingCompression;
+	public float wheelsDampingRelaxation;
+	public float frictionSlip;
+	public boolean bIsFrontWheel;
+	
+}
diff --git a/src/jbullet/src/javabullet/linearmath/AabbUtil2.java b/src/jbullet/src/javabullet/linearmath/AabbUtil2.java
new file mode 100644
index 0000000..978dcdf
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/AabbUtil2.java
@@ -0,0 +1,139 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+import javabullet.BulletStack;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class AabbUtil2 {
+
+	public static int outcode(Vector3f p, Vector3f halfExtent) {
+		return (p.x < -halfExtent.x ? 0x01 : 0x0) |
+				(p.x > halfExtent.x ? 0x08 : 0x0) |
+				(p.y < -halfExtent.y ? 0x02 : 0x0) |
+				(p.y > halfExtent.y ? 0x10 : 0x0) |
+				(p.z < -halfExtent.z ? 0x4 : 0x0) |
+				(p.z > halfExtent.z ? 0x20 : 0x0);
+	}
+	
+	public static boolean rayAabb(Vector3f rayFrom, Vector3f rayTo, Vector3f aabbMin, Vector3f aabbMax, float[] param, Vector3f normal) {
+		BulletStack stack = BulletStack.get();
+		
+		stack.vectors.push();
+		try {
+			Vector3f aabbHalfExtent = stack.vectors.get();
+			Vector3f aabbCenter = stack.vectors.get();
+			Vector3f source = stack.vectors.get();
+			Vector3f target = stack.vectors.get();
+			Vector3f r = stack.vectors.get();
+			Vector3f hitNormal = stack.vectors.get();
+
+			aabbHalfExtent.sub(aabbMax, aabbMin);
+			aabbHalfExtent.scale(0.5f);
+
+			aabbCenter.add(aabbMax, aabbMin);
+			aabbCenter.scale(0.5f);
+
+			source.sub(rayFrom, aabbCenter);
+			target.sub(rayTo, aabbCenter);
+
+			int sourceOutcode = outcode(source, aabbHalfExtent);
+			int targetOutcode = outcode(target, aabbHalfExtent);
+			if ((sourceOutcode & targetOutcode) == 0x0) {
+				float lambda_enter = 0f;
+				float lambda_exit = param[0];
+				r.sub(target, source);
+
+				float normSign = 1f;
+				hitNormal.set(0f, 0f, 0f);
+				int bit = 1;
+
+				for (int j = 0; j < 2; j++) {
+					for (int i = 0; i != 3; ++i) {
+						if ((sourceOutcode & bit) != 0) {
+							float lambda = (-VectorUtil.getCoord(source, i) - VectorUtil.getCoord(aabbHalfExtent, i) * normSign) / VectorUtil.getCoord(r, i);
+							if (lambda_enter <= lambda) {
+								lambda_enter = lambda;
+								hitNormal.set(0f, 0f, 0f);
+								VectorUtil.setCoord(hitNormal, i, normSign);
+							}
+						}
+						else if ((targetOutcode & bit) != 0) {
+							float lambda = (-VectorUtil.getCoord(source, i) - VectorUtil.getCoord(aabbHalfExtent, i) * normSign) / VectorUtil.getCoord(r, i);
+							//btSetMin(lambda_exit, lambda);
+							lambda_exit = Math.min(lambda_exit, lambda);
+						}
+						bit <<= 1;
+					}
+					normSign = -1f;
+				}
+				if (lambda_enter <= lambda_exit) {
+					param[0] = lambda_enter;
+					normal.set(hitNormal);
+					return true;
+				}
+			}
+			return false;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	/**
+	 * Conservative test for overlap between two aabbs.
+	 */
+	public static boolean testAabbAgainstAabb2(Vector3f aabbMin1, Vector3f aabbMax1, Vector3f aabbMin2, Vector3f aabbMax2) {
+		boolean overlap = true;
+		overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap;
+		overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap;
+		overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap;
+		return overlap;
+	}
+	
+	/**
+	 * Conservative test for overlap between triangle and aabb.
+	 */
+	public static boolean testTriangleAgainstAabb2(Vector3f[] vertices, Vector3f aabbMin, Vector3f aabbMax) {
+		Vector3f p1 = vertices[0];
+		Vector3f p2 = vertices[1];
+		Vector3f p3 = vertices[2];
+
+		if (Math.min(Math.min(p1.x, p2.x), p3.x) > aabbMax.x) return false;
+		if (Math.max(Math.max(p1.x, p2.x), p3.x) < aabbMin.x) return false;
+
+		if (Math.min(Math.min(p1.z, p2.z), p3.z) > aabbMax.z) return false;
+		if (Math.max(Math.max(p1.z, p2.z), p3.z) < aabbMin.z) return false;
+
+		if (Math.min(Math.min(p1.y, p2.y), p3.y) > aabbMax.y) return false;
+		if (Math.max(Math.max(p1.y, p2.y), p3.y) < aabbMin.y) return false;
+		
+		return true;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/linearmath/Clock.java b/src/jbullet/src/javabullet/linearmath/Clock.java
new file mode 100644
index 0000000..14207bb
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/Clock.java
@@ -0,0 +1,56 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+/**
+ *
+ * @author jezek2
+ */
+public class Clock {
+	
+	private long mStartTime;
+
+	public Clock() {
+		reset();
+	}
+
+	public void reset() {
+		mStartTime = System.currentTimeMillis();
+	}
+	
+	/**
+	 * Returns the time in ms since the last call to reset or since the Clock was created.
+	 */
+	public long getTimeMilliseconds() {
+		return System.currentTimeMillis() - mStartTime;
+	}
+	
+	/**
+	 * Returns the time in us since the last call to reset or since the Clock was created.
+	 */
+	public long getTimeMicroseconds() {
+		return getTimeMilliseconds() * 1000;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/linearmath/DebugDrawModes.java b/src/jbullet/src/javabullet/linearmath/DebugDrawModes.java
new file mode 100644
index 0000000..e159a3d
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/DebugDrawModes.java
@@ -0,0 +1,46 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+/**
+ *
+ * @author jezek2
+ */
+public class DebugDrawModes {
+	
+	public static final int NO_DEBUG              = 0;
+	public static final int DRAW_WIREFRAME        = 1;
+	public static final int DRAW_AABB             = 2;
+	public static final int DRAW_FEATURES_TEXT    = 4;
+	public static final int DRAW_CONTACT_POINTS   = 8;
+	public static final int NO_DEACTIVATION       = 16;
+	public static final int NO_HELP_TEXT          = 32;
+	public static final int DRAW_TEXT             = 64;
+	public static final int PROFILE_TIMINGS       = 128;
+	public static final int ENABLE_SAT_COMPARISON = 256;
+	public static final int DISABLE_BULLET_LCP    = 512;
+	public static final int ENABLE_CCD            = 1024;
+	public static final int MAX_DEBUG_DRAW_MODE   = 1025;
+	
+}
diff --git a/src/jbullet/src/javabullet/linearmath/DefaultMotionState.java b/src/jbullet/src/javabullet/linearmath/DefaultMotionState.java
new file mode 100644
index 0000000..e3bbe7e
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/DefaultMotionState.java
@@ -0,0 +1,65 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+/**
+ * DefaultMotionState provides a common implementation to synchronize world transforms with offsets.
+ * 
+ * @author jezek2
+ */
+public class DefaultMotionState implements MotionState {
+
+	public final Transform graphicsWorldTrans = new Transform();
+	public final Transform centerOfMassOffset = new Transform();
+	public final Transform startWorldTrans = new Transform();
+	
+	public DefaultMotionState() {
+		graphicsWorldTrans.setIdentity();
+		centerOfMassOffset.setIdentity();
+		startWorldTrans.setIdentity();
+	}
+
+	public DefaultMotionState(Transform startTrans) {
+		this.graphicsWorldTrans.set(startTrans);
+		centerOfMassOffset.setIdentity();
+		this.startWorldTrans.set(startTrans);
+	}
+	
+	public DefaultMotionState(Transform startTrans, Transform centerOfMassOffset) {
+		this.graphicsWorldTrans.set(startTrans);
+		this.centerOfMassOffset.set(centerOfMassOffset);
+		this.startWorldTrans.set(startTrans);
+	}
+	
+	public void getWorldTransform(Transform centerOfMassWorldTrans) {
+		centerOfMassWorldTrans.inverse(centerOfMassOffset);
+		centerOfMassWorldTrans.mul(graphicsWorldTrans);
+	}
+
+	public void setWorldTransform(Transform centerOfMassWorldTrans) {
+		graphicsWorldTrans.set(centerOfMassWorldTrans);
+		graphicsWorldTrans.mul(centerOfMassOffset);
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/linearmath/GeometryUtil.java b/src/jbullet/src/javabullet/linearmath/GeometryUtil.java
new file mode 100644
index 0000000..354203c
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/GeometryUtil.java
@@ -0,0 +1,185 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+import java.util.List;
+import javabullet.BulletStack;
+import javax.vecmath.Vector3f;
+import javax.vecmath.Vector4f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class GeometryUtil {
+
+	public static boolean isPointInsidePlanes(List<Vector4f> planeEquations, Vector3f point, float margin) {
+		int numbrushes = planeEquations.size();
+		for (int i = 0; i < numbrushes; i++) {
+			Vector4f N1 = planeEquations.get(i);
+			float dist = VectorUtil.dot3(N1, point) + N1.w - margin;
+			if (dist > 0f) {
+				return false;
+			}
+		}
+		return true;
+	}
+	
+	public static boolean areVerticesBehindPlane(Vector4f planeNormal, List<Vector3f> vertices, float margin) {
+		int numvertices = vertices.size();
+		for (int i = 0; i < numvertices; i++) {
+			Vector3f N1 = vertices.get(i);
+			float dist = VectorUtil.dot3(planeNormal, N1) + planeNormal.w - margin;
+			if (dist > 0f) {
+				return false;
+			}
+		}
+		return true;
+	}
+	
+	private static boolean notExist(Vector4f planeEquation, List<Vector4f> planeEquations) {
+		int numbrushes = planeEquations.size();
+		for (int i = 0; i < numbrushes; i++) {
+			Vector4f N1 = planeEquations.get(i);
+			if (VectorUtil.dot3(planeEquation, N1) > 0.999f) {
+				return false;
+			}
+		}
+		return true;
+	}
+
+	public static void getPlaneEquationsFromVertices(List<Vector3f> vertices, List<Vector4f> planeEquationsOut) {
+		BulletStack stack = BulletStack.get();
+		
+		stack.vectors.push();
+		stack.vectors4.push();
+		try {
+			Vector4f planeEquation = stack.vectors4.get();
+			Vector3f edge0 = stack.vectors.get(), edge1 = stack.vectors.get();
+			Vector3f tmp = stack.vectors.get();
+
+			int numvertices = vertices.size();
+			// brute force:
+			for (int i = 0; i < numvertices; i++) {
+				Vector3f N1 = vertices.get(i);
+
+				for (int j = i + 1; j < numvertices; j++) {
+					Vector3f N2 = vertices.get(j);
+
+					for (int k = j + 1; k < numvertices; k++) {
+						Vector3f N3 = vertices.get(k);
+
+						edge0.sub(N2, N1);
+						edge1.sub(N3, N1);
+						float normalSign = 1f;
+						for (int ww = 0; ww < 2; ww++) {
+							tmp.cross(edge0, edge1);
+							planeEquation.x = normalSign * tmp.x;
+							planeEquation.y = normalSign * tmp.y;
+							planeEquation.z = normalSign * tmp.z;
+
+							if (VectorUtil.lengthSquared3(planeEquation) > 0.0001f) {
+								VectorUtil.normalize3(planeEquation);
+								if (notExist(planeEquation, planeEquationsOut)) {
+									planeEquation.w = -VectorUtil.dot3(planeEquation, N1);
+
+									// check if inside, and replace supportingVertexOut if needed
+									if (areVerticesBehindPlane(planeEquation, vertices, 0.01f)) {
+										planeEquationsOut.add(planeEquation);
+									}
+								}
+							}
+							normalSign = -1f;
+						}
+					}
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+			stack.vectors4.pop();
+		}
+	}
+	
+	public static void getVerticesFromPlaneEquations(List<Vector4f> planeEquations, List<Vector3f> verticesOut) {
+		BulletStack stack = BulletStack.get();
+		
+		stack.vectors.push();
+		try {
+			Vector3f n2n3 = stack.vectors.get();
+			Vector3f n3n1 = stack.vectors.get();
+			Vector3f n1n2 = stack.vectors.get();
+			Vector3f potentialVertex = stack.vectors.get();
+
+			int numbrushes = planeEquations.size();
+			// brute force:
+			for (int i = 0; i < numbrushes; i++) {
+				Vector4f N1 = planeEquations.get(i);
+
+				for (int j = i + 1; j < numbrushes; j++) {
+					Vector4f N2 = planeEquations.get(j);
+
+					for (int k = j + 1; k < numbrushes; k++) {
+						Vector4f N3 = planeEquations.get(k);
+
+						VectorUtil.cross3(n2n3, N2, N3);
+						VectorUtil.cross3(n3n1, N3, N1);
+						VectorUtil.cross3(n1n2, N1, N2);
+
+						if ((n2n3.lengthSquared() > 0.0001f) &&
+								(n3n1.lengthSquared() > 0.0001f) &&
+								(n1n2.lengthSquared() > 0.0001f)) {
+							// point P out of 3 plane equations:
+
+							// 	     d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )  
+							// P =  -------------------------------------------------------------------------  
+							//    N1 . ( N2 * N3 )  
+
+							float quotient = VectorUtil.dot3(N1, n2n3);
+							if (Math.abs(quotient) > 0.000001f) {
+								quotient = -1f / quotient;
+								n2n3.scale(N1.w);
+								n3n1.scale(N2.w);
+								n1n2.scale(N3.w);
+								potentialVertex.set(n2n3);
+								potentialVertex.add(n3n1);
+								potentialVertex.add(n1n2);
+								potentialVertex.scale(quotient);
+
+								// check if inside, and replace supportingVertexOut if needed
+								if (isPointInsidePlanes(planeEquations, potentialVertex, 0.01f)) {
+									verticesOut.add(potentialVertex);
+								}
+							}
+						}
+					}
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/linearmath/IDebugDraw.java b/src/jbullet/src/javabullet/linearmath/IDebugDraw.java
new file mode 100644
index 0000000..407e4c6
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/IDebugDraw.java
@@ -0,0 +1,86 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+import javabullet.BulletStack;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public abstract class IDebugDraw {
+	
+	protected final BulletStack stack = BulletStack.get();
+
+	public abstract void drawLine(Vector3f from, Vector3f to, Vector3f color);
+
+	public abstract void drawContactPoint(Vector3f PointOnB, Vector3f normalOnB, float distance, int lifeTime, Vector3f color);
+
+	public abstract void reportErrorWarning(String warningString);
+
+	public abstract void draw3dText(Vector3f location, String textString);
+
+	public abstract void setDebugMode(int debugMode);
+
+	public abstract int getDebugMode();
+
+	public void drawAabb(Vector3f from, Vector3f to, Vector3f color) {
+		stack.vectors.push();
+		try {
+			Vector3f halfExtents = stack.vectors.get(to);
+			halfExtents.sub(from);
+			halfExtents.scale(0.5f);
+
+			Vector3f center = stack.vectors.get(to);
+			center.add(from);
+			center.scale(0.5f);
+
+			int i, j;
+
+			Vector3f edgecoord = stack.vectors.get(1f, 1f, 1f), pa = stack.vectors.get(), pb = stack.vectors.get();
+			for (i = 0; i < 4; i++) {
+				for (j = 0; j < 3; j++) {
+					pa.set(edgecoord.x * halfExtents.x, edgecoord.y * halfExtents.y, edgecoord.z * halfExtents.z);
+					pa.add(center);
+
+					int othercoord = j % 3;
+
+					VectorUtil.mulCoord(edgecoord, othercoord, -1f);
+					pb.set(edgecoord.x * halfExtents.x, edgecoord.y * halfExtents.y, edgecoord.z * halfExtents.z);
+					pb.add(center);
+
+					drawLine(pa, pb, color);
+				}
+				edgecoord.set(-1f, -1f, -1f);
+				if (i < 3) {
+					VectorUtil.mulCoord(edgecoord, i, -1f);
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+}
diff --git a/src/jbullet/src/javabullet/linearmath/MatrixUtil.java b/src/jbullet/src/javabullet/linearmath/MatrixUtil.java
new file mode 100644
index 0000000..f5c9774
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/MatrixUtil.java
@@ -0,0 +1,208 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+import javabullet.BulletStack;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Quat4f;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class MatrixUtil {
+	
+	public static void scale(Matrix3f dest, Matrix3f mat, Vector3f s) {
+		dest.m00 = mat.m00 * s.x;   dest.m01 = mat.m01 * s.y;   dest.m02 = mat.m02 * s.z;
+		dest.m10 = mat.m10 * s.x;   dest.m11 = mat.m11 * s.y;   dest.m12 = mat.m12 * s.z;
+		dest.m20 = mat.m20 * s.x;   dest.m21 = mat.m21 * s.y;   dest.m22 = mat.m22 * s.z;
+	}
+	
+	public static void absolute(Matrix3f mat) {
+		mat.m00 = Math.abs(mat.m00);
+		mat.m01 = Math.abs(mat.m01);
+		mat.m02 = Math.abs(mat.m02);
+		mat.m10 = Math.abs(mat.m10);
+		mat.m11 = Math.abs(mat.m11);
+		mat.m12 = Math.abs(mat.m12);
+		mat.m20 = Math.abs(mat.m20);
+		mat.m21 = Math.abs(mat.m21);
+		mat.m22 = Math.abs(mat.m22);
+	}
+	
+	public static void setFromOpenGLSubMatrix(Matrix3f mat, float[] m) {
+		mat.m00 = m[0]; mat.m01 = m[4]; mat.m02 = m[8];
+		mat.m10 = m[1]; mat.m11 = m[5]; mat.m12 = m[9];
+		mat.m20 = m[2]; mat.m21 = m[6]; mat.m22 = m[10];
+	}
+
+	public static void getOpenGLSubMatrix(Matrix3f mat, float[] m) {
+		m[0] = mat.m00;
+		m[1] = mat.m10;
+		m[2] = mat.m20;
+		m[3] = 0f;
+		m[4] = mat.m01;
+		m[5] = mat.m11;
+		m[6] = mat.m21;
+		m[7] = 0f;
+		m[8] = mat.m02;
+		m[9] = mat.m12;
+		m[10] = mat.m22;
+		m[11] = 0f;
+	}
+	
+	/**
+	 * setEulerZYX
+	 * 
+	 * @param euler a const reference to a btVector3 of euler angles
+	 * These angles are used to produce a rotation matrix. The euler
+	 * angles are applied in ZYX order. I.e a vector is first rotated 
+	 * about X then Y and then Z
+	 */
+	public static void setEulerZYX(Matrix3f mat, float eulerX, float eulerY, float eulerZ) {
+		float ci = (float) Math.cos(eulerX);
+		float cj = (float) Math.cos(eulerY);
+		float ch = (float) Math.cos(eulerZ);
+		float si = (float) Math.sin(eulerX);
+		float sj = (float) Math.sin(eulerY);
+		float sh = (float) Math.sin(eulerZ);
+		float cc = ci * ch;
+		float cs = ci * sh;
+		float sc = si * ch;
+		float ss = si * sh;
+
+		mat.setRow(0, cj * ch, sj * sc - cs, sj * cc + ss);
+		mat.setRow(1, cj * sh, sj * ss + cc, sj * cs - sc);
+		mat.setRow(2, -sj, cj * si, cj * ci);
+	}
+	
+	private static float tdotx(Matrix3f mat, Vector3f vec) {
+		return mat.m00 * vec.x + mat.m10 * vec.y + mat.m20 * vec.z;
+	}
+
+	private static float tdoty(Matrix3f mat, Vector3f vec) {
+		return mat.m01 * vec.x + mat.m11 * vec.y + mat.m21 * vec.z;
+	}
+
+	private static float tdotz(Matrix3f mat, Vector3f vec) {
+		return mat.m02 * vec.x + mat.m12 * vec.y + mat.m22 * vec.z;
+	}
+	
+	public static void transposeTransform(Vector3f dest, Vector3f vec, Matrix3f mat) {
+		float x = tdotx(mat, vec);
+		float y = tdoty(mat, vec);
+		float z = tdotz(mat, vec);
+		dest.x = x;
+		dest.y = y;
+		dest.z = z;
+	}
+	
+	public static void setRotation(Matrix3f dest, Quat4f q) {
+		float d = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
+		assert (d != 0f);
+		float s = 2f / d;
+		float xs = q.x * s, ys = q.y * s, zs = q.z * s;
+		float wx = q.w * xs, wy = q.w * ys, wz = q.w * zs;
+		float xx = q.x * xs, xy = q.x * ys, xz = q.x * zs;
+		float yy = q.y * ys, yz = q.y * zs, zz = q.z * zs;
+		dest.m00 = 1f - (yy + zz);
+		dest.m01 = xy - wz;
+		dest.m02 = xz + wy;
+		dest.m10 = xy + wz;
+		dest.m11 = 1f - (xx + zz);
+		dest.m12 = yz - wx;
+		dest.m20 = xz - wy;
+		dest.m21 = yz + wx;
+		dest.m22 = 1f - (xx + yy);
+	}
+	
+	public static void getRotation(Matrix3f mat, Quat4f dest) {
+		BulletStack stack = BulletStack.get();
+		
+		float trace = mat.m00 + mat.m11 + mat.m22;
+		float[] temp = stack.floatArrays.getFixed(4);
+
+		if (trace > 0f) {
+			float s = (float) Math.sqrt(trace + 1f);
+			temp[3] = (s * 0.5f);
+			s = 0.5f / s;
+
+			temp[0] = ((mat.m21 - mat.m12) * s);
+			temp[1] = ((mat.m02 - mat.m20) * s);
+			temp[2] = ((mat.m10 - mat.m01) * s);
+		}
+		else {
+			int i = mat.m00 < mat.m11 ? (mat.m11 < mat.m22 ? 2 : 1) : (mat.m00 < mat.m22 ? 2 : 0);
+			int j = (i + 1) % 3;
+			int k = (i + 2) % 3;
+
+			float s = (float) Math.sqrt(mat.getElement(i, i) - mat.getElement(j, j) - mat.getElement(k, k) + 1f);
+			temp[i] = s * 0.5f;
+			s = 0.5f / s;
+
+			temp[3] = (mat.getElement(k, j) - mat.getElement(j, k)) * s;
+			temp[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;
+			temp[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s;
+		}
+		dest.set(temp[0], temp[1], temp[2], temp[3]);
+		
+		stack.floatArrays.release(temp);
+	}
+
+	private static float cofac(Matrix3f mat, int r1, int c1, int r2, int c2) {
+		return mat.getElement(r1, c1) * mat.getElement(r2, c2) - mat.getElement(r1, c2) * mat.getElement(r2, c1);
+	}
+	
+	public static void invert(Matrix3f mat) {
+		float co_x = cofac(mat, 1, 1, 2, 2);
+		float co_y = cofac(mat, 1, 2, 2, 0);
+		float co_z = cofac(mat, 1, 0, 2, 1);
+		
+		float det = mat.m00*co_x + mat.m01*co_y + mat.m02*co_z;
+		assert (det != 0f);
+		
+		float s = 1f / det;
+		float m00 = co_x * s;
+		float m01 = cofac(mat, 0, 2, 2, 1) * s;
+		float m02 = cofac(mat, 0, 1, 1, 2) * s;
+		float m10 = co_y * s;
+		float m11 = cofac(mat, 0, 0, 2, 2) * s;
+		float m12 = cofac(mat, 0, 2, 1, 0) * s;
+		float m20 = co_z * s;
+		float m21 = cofac(mat, 0, 1, 2, 0) * s;
+		float m22 = cofac(mat, 0, 0, 1, 1) * s;
+		
+		mat.m00 = m00;
+		mat.m01 = m01;
+		mat.m02 = m02;
+		mat.m10 = m10;
+		mat.m11 = m11;
+		mat.m12 = m12;
+		mat.m20 = m20;
+		mat.m21 = m21;
+		mat.m22 = m22;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/linearmath/MiscUtil.java b/src/jbullet/src/javabullet/linearmath/MiscUtil.java
new file mode 100644
index 0000000..e522fb2
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/MiscUtil.java
@@ -0,0 +1,156 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+import java.util.Comparator;
+import java.util.List;
+import javabullet.util.FloatArrayList;
+import javabullet.util.IntArrayList;
+
+/**
+ *
+ * @author jezek2
+ */
+public class MiscUtil {
+
+	public static int getListCapacityForHash(List<?> list) {
+		return getListCapacityForHash(list.size());
+	}
+	
+	public static int getListCapacityForHash(int size) {
+		int n = 2;
+		while (n < size) {
+			n <<= 1;
+		}
+		return n;
+	}
+
+	public static <T> void ensureIndex(List<T> list, int index, T value) {
+		while (list.size() <= index) {
+			list.add(value);
+		}
+	}
+	
+	public static void resize(IntArrayList list, int size, int value) {
+		while (list.size() < size) {
+			list.add(value);
+		}
+		
+		while (list.size() > size) {
+			list.remove(list.size() - 1);
+		}
+	}
+	
+	public static void resize(FloatArrayList list, int size, float value) {
+		while (list.size() < size) {
+			list.add(value);
+		}
+		
+		while (list.size() > size) {
+			list.remove(list.size() - 1);
+		}
+	}
+
+	public static <T> void resize(List<T> list, int size, Class<T> valueCls) {
+		try {
+			while (list.size() < size) {
+				list.add(valueCls != null? valueCls.newInstance() : null);
+			}
+
+			while (list.size() > size) {
+				list.remove(list.size() - 1);
+			}
+		}
+		catch (IllegalAccessException e) {
+			throw new IllegalStateException(e);
+		}
+		catch (InstantiationException e) {
+			throw new IllegalStateException(e);
+		}
+	}
+	
+	public static <T> int indexOf(T[] array, T obj) {
+		for (int i=0; i<array.length; i++) {
+			if (array[i] == obj) return i;
+		}
+		return -1;
+	}
+	
+	public static float GEN_clamped(float a, float lb, float ub) {
+		return a < lb ? lb : (ub < a ? ub : a);
+	}
+
+	/**
+	 * Heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+	 */
+	private static <T> void downHeap(List<T> pArr, int k, int n, Comparator<T> comparator) {
+		/*  PRE: a[k+1..N] is a heap */
+		/* POST:  a[k..N]  is a heap */
+
+		T temp = pArr.get(k - 1);
+		/* k has child(s) */
+		while (k <= n / 2) {
+			int child = 2 * k;
+
+			if ((child < n) && comparator.compare(pArr.get(child - 1), pArr.get(child)) < 0) {
+				child++;
+			}
+			/* pick larger child */
+			if (comparator.compare(temp, pArr.get(child - 1)) < 0) {
+				/* move child up */
+				pArr.set(k - 1, pArr.get(child - 1));
+				k = child;
+			}
+			else {
+				break;
+			}
+		}
+		pArr.set(k - 1, temp);
+	}
+
+	public static <T> void heapSort(List<T> list, Comparator<T> comparator) {
+		/* sort a[0..N-1],  N.B. 0 to N-1 */
+		int k;
+		int n = list.size();
+		for (k = n / 2; k > 0; k--) {
+			downHeap(list, k, n, comparator);
+		}
+
+		/* a[1..N] is now a heap */
+		while (n >= 1) {
+			swap(list, 0, n - 1); /* largest of a[0..n-1] */
+
+			n = n - 1;
+			/* restore a[1..i-1] heap */
+			downHeap(list, 1, n, comparator);
+		}
+	}
+
+	private static <T> void swap(List<T> list, int index0, int index1) {
+		T temp = list.get(index0);
+		list.set(index0, list.get(index1));
+		list.set(index1, temp);
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/linearmath/MotionState.java b/src/jbullet/src/javabullet/linearmath/MotionState.java
new file mode 100644
index 0000000..964df4a
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/MotionState.java
@@ -0,0 +1,41 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+/**
+ * MotionState allows the dynamics world to synchronize the updated world transforms with graphics.
+ * For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation).
+ * 
+ * @author jezek2
+ */
+public interface MotionState {
+
+	public void	getWorldTransform(Transform worldTrans);
+
+	/**
+	 * Note: Bullet only calls the update of worldtransform for active objects.
+	 */
+	public void	setWorldTransform(Transform worldTrans);
+	
+}
diff --git a/src/jbullet/src/javabullet/linearmath/QuaternionUtil.java b/src/jbullet/src/javabullet/linearmath/QuaternionUtil.java
new file mode 100644
index 0000000..65fece4
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/QuaternionUtil.java
@@ -0,0 +1,104 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javax.vecmath.Quat4f;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class QuaternionUtil {
+
+	public static float getAngle(Quat4f q) {
+		float s = 2f * (float) Math.acos(q.w);
+		return s;
+	}
+	
+	public static void setRotation(Quat4f q, Vector3f axis, float angle) {
+		float d = axis.length();
+		assert (d != 0f);
+		float s = (float)Math.sin(angle * 0.5f) / d;
+		q.set(axis.x * s, axis.y * s, axis.z * s, (float) Math.cos(angle * 0.5f));
+	}
+	
+	// Game Programming Gems 2.10. make sure v0,v1 are normalized
+	public static Quat4f shortestArcQuat(Vector3f v0, Vector3f v1) {
+		BulletStack stack = BulletStack.get();
+
+		stack.vectors.push();
+		stack.quats.push();
+		try {
+			Vector3f c = stack.vectors.get();
+			c.cross(v0, v1);
+			float d = v0.dot(v1);
+
+			if (d < -1.0 + BulletGlobals.FLT_EPSILON) {
+				// just pick any vector
+				return stack.quats.returning(stack.quats.get(0.0f, 1.0f, 0.0f, 0.0f));
+			}
+
+			float s = (float) Math.sqrt((1.0f + d) * 2.0f);
+			float rs = 1.0f / s;
+
+			return stack.quats.returning(stack.quats.get(c.x * rs, c.y * rs, c.z * rs, s * 0.5f));
+		}
+		finally {
+			stack.vectors.pop();
+			stack.quats.pop();
+		}
+	}
+	
+	public static void mul(Quat4f q, Vector3f w) {
+		float rx = q.w * w.x + q.y * w.z - q.z * w.y;
+		float ry = q.w * w.y + q.z * w.x - q.x * w.z;
+		float rz = q.w * w.z + q.x * w.y - q.y * w.x;
+		float rw = -q.x * w.x - q.y * w.y - q.z * w.z;
+		q.set(rx, ry, rz, rw);
+	}
+	
+	public static Vector3f quatRotate(Quat4f rotation, Vector3f v) {
+		BulletStack stack = BulletStack.get();
+
+		stack.vectors.push();
+		stack.quats.push();
+		try {
+			Quat4f q = stack.quats.get(rotation);
+			QuaternionUtil.mul(q, v);
+
+			Quat4f tmp = stack.quats.get();
+			tmp.inverse(rotation);
+			q.mul(tmp);
+			return stack.vectors.returning(stack.vectors.get(q.x, q.y, q.z));
+		}
+		finally {
+			stack.vectors.pop();
+			stack.quats.pop();
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/linearmath/ScalarUtil.java b/src/jbullet/src/javabullet/linearmath/ScalarUtil.java
new file mode 100644
index 0000000..c898ebb
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/ScalarUtil.java
@@ -0,0 +1,58 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+import javabullet.BulletGlobals;
+
+/**
+ *
+ * @author jezek2
+ */
+public class ScalarUtil {
+
+	public static float fsel(float a, float b, float c) {
+		return a >= 0 ? b : c;
+	}
+	
+	public static boolean fuzzyZero(float x) {
+		return Math.abs(x) < BulletGlobals.FLT_EPSILON;
+	}
+
+	public static float atan2Fast(float y, float x) {
+		float coeff_1 = BulletGlobals.SIMD_PI / 4.0f;
+		float coeff_2 = 3.0f * coeff_1;
+		float abs_y = Math.abs(y);
+		float angle;
+		if (x >= 0.0f) {
+			float r = (x - abs_y) / (x + abs_y);
+			angle = coeff_1 - coeff_1 * r;
+		}
+		else {
+			float r = (x + abs_y) / (abs_y - x);
+			angle = coeff_2 - coeff_1 * r;
+		}
+		return (y < 0.0f) ? -angle : angle;
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/linearmath/Transform.java b/src/jbullet/src/javabullet/linearmath/Transform.java
new file mode 100644
index 0000000..1b44958
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/Transform.java
@@ -0,0 +1,148 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+import javabullet.BulletStack;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Quat4f;
+import javax.vecmath.Vector3f;
+
+/**
+ * Transform supports rigid transforms (only translation and rotation, no scaling/shear).
+ * 
+ * @author jezek2
+ */
+public class Transform {
+	
+	protected BulletStack stack;
+
+	public final Matrix3f basis = new Matrix3f();
+	public final Vector3f origin = new Vector3f();
+
+	public Transform() {
+	}
+
+	public Transform(Matrix3f mat) {
+		basis.set(mat);
+	}
+
+	public Transform(Transform tr) {
+		set(tr);
+	}
+	
+	public void set(Transform tr) {
+		basis.set(tr.basis);
+		origin.set(tr.origin);
+	}
+	
+	public void transform(Vector3f v) {
+		basis.transform(v);
+		v.add(origin);
+	}
+
+	public void setIdentity() {
+		basis.setIdentity();
+		origin.set(0f, 0f, 0f);
+	}
+	
+	public void inverse() {
+		basis.transpose();
+		origin.scale(-1f);
+		basis.transform(origin);
+	}
+
+	public void inverse(Transform tr) {
+		set(tr);
+		inverse();
+	}
+	
+	public void mul(Transform tr) {
+		if (stack == null) stack = BulletStack.get();
+		
+		stack.vectors.push();
+		try {
+			Vector3f vec = stack.vectors.get(tr.origin);
+			transform(vec);
+
+			basis.mul(tr.basis);
+			origin.set(vec);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void mul(Transform tr1, Transform tr2) {
+		set(tr1);
+		mul(tr2);
+	}
+	
+	public void invXform(Vector3f inVec, Vector3f out) {
+		if (stack == null) stack = BulletStack.get();
+
+		stack.matrices.push();
+		try {
+			out.sub(inVec, origin);
+
+			Matrix3f mat = stack.matrices.get(basis);
+			mat.transpose();
+			mat.transform(out);
+		}
+		finally {
+			stack.matrices.pop();
+		}
+	}
+	
+	public Quat4f getRotation() {
+		if (stack == null) stack = BulletStack.get();
+
+		stack.quats.push();
+		try {
+			Quat4f q = stack.quats.get();
+			MatrixUtil.getRotation(basis, q);
+			return stack.quats.returning(q);
+		}
+		finally {
+			stack.quats.pop();
+		}
+	}
+	
+	public void setRotation(Quat4f q) {
+		MatrixUtil.setRotation(basis, q);
+	}
+	
+	public void setFromOpenGLMatrix(float[] m) {
+		MatrixUtil.setFromOpenGLSubMatrix(basis, m);
+		origin.set(m[12], m[13], m[14]);
+	}
+
+	public void getOpenGLMatrix(float[] m) {
+		MatrixUtil.getOpenGLSubMatrix(basis, m);
+		m[12] = origin.x;
+		m[13] = origin.y;
+		m[14] = origin.z;
+		m[15] = 1f;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/linearmath/TransformUtil.java b/src/jbullet/src/javabullet/linearmath/TransformUtil.java
new file mode 100644
index 0000000..25acf80
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/TransformUtil.java
@@ -0,0 +1,174 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Quat4f;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class TransformUtil {
+	
+	public static final float SIMDSQRT12 = 0.7071067811865475244008443621048490f;
+	public static final float ANGULAR_MOTION_THRESHOLD = 0.5f*BulletGlobals.SIMD_HALF_PI;
+	
+	public static float recipSqrt(float x) {
+		return 1f / (float)Math.sqrt(x);  /* reciprocal square root */
+	}
+
+	public static void planeSpace1(Vector3f n, Vector3f p, Vector3f q) {
+		if (Math.abs(n.z) > SIMDSQRT12) {
+			// choose p in y-z plane
+			float a = n.y * n.y + n.z * n.z;
+			float k = recipSqrt(a);
+			p.set(0, -n.z * k, n.y * k);
+			// set q = n x p
+			q.set(a * k, -n.x * p.z, n.x * p.y);
+		}
+		else {
+			// choose p in x-y plane
+			float a = n.x * n.x + n.y * n.y;
+			float k = recipSqrt(a);
+			p.set(-n.y * k, n.x * k, 0);
+			// set q = n x p
+			q.set(-n.z * p.y, n.z * p.x, a * k);
+		}
+	}
+	
+	public static void integrateTransform(Transform curTrans, Vector3f linvel, Vector3f angvel, float timeStep, Transform predictedTransform) {
+		BulletStack stack = BulletStack.get();
+		
+		stack.vectors.push();
+		stack.quats.push();
+		try {
+			predictedTransform.origin.scaleAdd(timeStep, linvel, curTrans.origin);
+	//	//#define QUATERNION_DERIVATIVE
+	//	#ifdef QUATERNION_DERIVATIVE
+	//		btQuaternion predictedOrn = curTrans.getRotation();
+	//		predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
+	//		predictedOrn.normalize();
+	//	#else
+			// exponential map
+			Vector3f axis = stack.vectors.get();
+			float fAngle = angvel.length();
+
+			// limit the angular motion
+			if (fAngle * timeStep > ANGULAR_MOTION_THRESHOLD) {
+				fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
+			}
+
+			if (fAngle < 0.001f) {
+				// use Taylor's expansions of sync function
+				axis.scale(0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * fAngle * fAngle, angvel);
+			}
+			else {
+				// sync(fAngle) = sin(c*fAngle)/t
+				axis.scale((float) Math.sin(0.5f * fAngle * timeStep) / fAngle, angvel);
+			}
+			Quat4f dorn = stack.quats.get(axis.x, axis.y, axis.z, (float) Math.cos(fAngle * timeStep * 0.5f));
+			Quat4f orn0 = stack.quats.get(curTrans.getRotation());
+
+			Quat4f predictedOrn = stack.quats.get();
+			predictedOrn.mul(dorn, orn0);
+			predictedOrn.normalize();
+	//  #endif
+			predictedTransform.setRotation(predictedOrn);
+		}
+		finally {
+			stack.vectors.pop();
+			stack.quats.pop();
+		}
+	}
+
+	public static void calculateVelocity(Transform transform0, Transform transform1, float timeStep, Vector3f linVel, Vector3f angVel) {
+		BulletStack stack = BulletStack.get();
+		
+		stack.vectors.push();
+		try {
+			linVel.sub(transform1.origin, transform0.origin);
+			linVel.scale(1f / timeStep);
+
+			Vector3f axis = stack.vectors.get();
+			float[] angle = new float[1];
+			calculateDiffAxisAngle(transform0, transform1, axis, angle);
+			angVel.scale(angle[0] / timeStep, axis);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public static void calculateDiffAxisAngle(Transform transform0, Transform transform1, Vector3f axis, float[] angle) {
+		BulletStack stack = BulletStack.get();
+		
+		stack.matrices.push();
+		stack.quats.push();
+		try {
+	// #ifdef USE_QUATERNION_DIFF
+	//		btQuaternion orn0 = transform0.getRotation();
+	//		btQuaternion orn1a = transform1.getRotation();
+	//		btQuaternion orn1 = orn0.farthest(orn1a);
+	//		btQuaternion dorn = orn1 * orn0.inverse();
+	// #else
+			Matrix3f tmp = stack.matrices.get();
+			tmp.set(transform0.basis);
+			MatrixUtil.invert(tmp);
+
+			Matrix3f dmat = stack.matrices.get();
+			dmat.mul(transform1.basis, tmp);
+
+			Quat4f dorn = stack.quats.get();
+			MatrixUtil.getRotation(dmat, dorn);
+	// #endif
+
+			// floating point inaccuracy can lead to w component > 1..., which breaks 
+
+			dorn.normalize();
+
+			angle[0] = QuaternionUtil.getAngle(dorn);
+			axis.set(dorn.x, dorn.y, dorn.z);
+			// TODO: probably not needed
+			//axis[3] = btScalar(0.);
+
+			// check for axis length
+			float len = axis.lengthSquared();
+			if (len < BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) {
+				axis.set(1f, 0f, 0f);
+			}
+			else {
+				axis.scale(1f / (float) Math.sqrt(len));
+			}
+		}
+		finally {
+			stack.matrices.pop();
+			stack.quats.pop();
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/linearmath/VectorUtil.java b/src/jbullet/src/javabullet/linearmath/VectorUtil.java
new file mode 100644
index 0000000..b0ba6ca
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/VectorUtil.java
@@ -0,0 +1,189 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.linearmath;
+
+import javax.vecmath.Vector3f;
+import javax.vecmath.Vector4f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class VectorUtil {
+
+	public static int maxAxis(Vector3f v) {
+		int maxIndex = -1;
+		float maxVal = -1e30f;
+		if (v.x > maxVal) {
+			maxIndex = 0;
+			maxVal = v.x;
+		}
+		if (v.y > maxVal) {
+			maxIndex = 1;
+			maxVal = v.y;
+		}
+		if (v.z > maxVal) {
+			maxIndex = 2;
+			maxVal = v.z;
+		}
+
+		return maxIndex;
+	}
+	
+	public static int maxAxis4(Vector4f v) {
+		int maxIndex = -1;
+		float maxVal = -1e30f;
+		if (v.x > maxVal) {
+			maxIndex = 0;
+			maxVal = v.x;
+		}
+		if (v.y > maxVal) {
+			maxIndex = 1;
+			maxVal = v.y;
+		}
+		if (v.z > maxVal) {
+			maxIndex = 2;
+			maxVal = v.z;
+		}
+		if (v.w > maxVal) {
+			maxIndex = 3;
+			maxVal = v.w;
+		}
+
+		return maxIndex;
+	}
+
+	public static int closestAxis4(Vector4f vec) {
+		Vector4f tmp = new Vector4f(vec);
+		tmp.absolute();
+		return maxAxis4(tmp);
+	}
+	
+	public static float getCoord(Vector3f vec, int num) {
+		switch (num) {
+			case 0: return vec.x;
+			case 1: return vec.y;
+			case 2: return vec.z;
+			default: throw new InternalError();
+		}
+	}
+	
+	public static void setCoord(Vector3f vec, int num, float value) {
+		switch (num) {
+			case 0: vec.x = value; break;
+			case 1: vec.y = value; break;
+			case 2: vec.z = value; break;
+			default: throw new InternalError();
+		}
+	}
+
+	public static void mulCoord(Vector3f vec, int num, float value) {
+		switch (num) {
+			case 0: vec.x *= value; break;
+			case 1: vec.y *= value; break;
+			case 2: vec.z *= value; break;
+			default: throw new InternalError();
+		}
+	}
+
+	public static void setInterpolate3(Vector3f dest, Vector3f v0, Vector3f v1, float rt) {
+		float s = 1f - rt;
+		dest.x = s * v0.x + rt * v1.x;
+		dest.y = s * v0.y + rt * v1.y;
+		dest.z = s * v0.z + rt * v1.z;
+		// don't do the unused w component
+		//		m_co[3] = s * v0[3] + rt * v1[3];
+	}
+
+	public static void add(Vector3f dest, Vector3f v1, Vector3f v2) {
+		dest.x = v1.x + v2.x;
+		dest.y = v1.y + v2.y;
+		dest.z = v1.z + v2.z;
+	}
+	
+	public static void add(Vector3f dest, Vector3f v1, Vector3f v2, Vector3f v3) {
+		dest.x = v1.x + v2.x + v3.x;
+		dest.y = v1.y + v2.y + v3.y;
+		dest.z = v1.z + v2.z + v3.z;
+	}
+	
+	public static void add(Vector3f dest, Vector3f v1, Vector3f v2, Vector3f v3, Vector3f v4) {
+		dest.x = v1.x + v2.x + v3.x + v4.x;
+		dest.y = v1.y + v2.y + v3.y + v4.y;
+		dest.z = v1.z + v2.z + v3.z + v4.z;
+	}
+	
+	public static void mul(Vector3f dest, Vector3f v1, Vector3f v2) {
+		dest.x = v1.x * v2.x;
+		dest.y = v1.y * v2.y;
+		dest.z = v1.z * v2.z;
+	}
+	
+	public static void div(Vector3f dest, Vector3f v1, Vector3f v2) {
+		dest.x = v1.x / v2.x;
+		dest.y = v1.y / v2.y;
+		dest.z = v1.z / v2.z;
+	}
+	
+	public static void setMin(Vector3f a, Vector3f b) {
+		a.x = Math.min(a.x, b.x);
+		a.y = Math.min(a.y, b.y);
+		a.z = Math.min(a.z, b.z);
+	}
+	
+	public static void setMax(Vector3f a, Vector3f b) {
+		a.x = Math.max(a.x, b.x);
+		a.y = Math.max(a.y, b.y);
+		a.z = Math.max(a.z, b.z);
+	}
+	
+	public static float dot3(Vector4f v0, Vector3f v1) {
+		return (v0.x*v1.x + v0.y*v1.y + v0.z*v1.z);
+	}
+
+	public static float dot3(Vector4f v0, Vector4f v1) {
+		return (v0.x*v1.x + v0.y*v1.y + v0.z*v1.z);
+	}
+
+	public static float lengthSquared3(Vector4f v) {
+		return (v.x*v.x + v.y*v.y + v.z*v.z);
+	}
+
+	public static void normalize3(Vector4f v) {
+		float norm = (float)(1.0/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z));
+		v.x *= norm;
+		v.y *= norm;
+		v.z *= norm;
+	}
+
+	public static void cross3(Vector3f dest, Vector4f v1, Vector4f v2) {
+        float x,y;
+        x = v1.y*v2.z - v1.z*v2.y;
+        y = v2.x*v1.z - v2.z*v1.x;
+        dest.z = v1.x*v2.y - v1.y*v2.x;
+        dest.x = x;
+        dest.y = y;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/linearmath/package-info.java b/src/jbullet/src/javabullet/linearmath/package-info.java
new file mode 100644
index 0000000..3c33279
--- /dev/null
+++ b/src/jbullet/src/javabullet/linearmath/package-info.java
@@ -0,0 +1,28 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+/**
+ * Vector math library.
+ */
+package javabullet.linearmath;
+
diff --git a/src/jbullet/src/javabullet/util/FloatArrayList.java b/src/jbullet/src/javabullet/util/FloatArrayList.java
new file mode 100644
index 0000000..ab8fe41
--- /dev/null
+++ b/src/jbullet/src/javabullet/util/FloatArrayList.java
@@ -0,0 +1,71 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.util;
+
+/**
+ *
+ * @author jezek2
+ */
+public class FloatArrayList {
+
+	private float[] array = new float[16];
+	private int size;
+	
+	public void add(float value) {
+		if (size == array.length) {
+			expand();
+		}
+		
+		array[size++] = value;
+	}
+	
+	private void expand() {
+		float[] newArray = new float[array.length << 1];
+		System.arraycopy(array, 0, newArray, 0, array.length);
+		array = newArray;
+	}
+
+	public float remove(int index) {
+		if (index >= size) throw new IndexOutOfBoundsException();
+		float old = array[index];
+		System.arraycopy(array, index+1, array, index, size - index - 1);
+		size--;
+		return old;
+	}
+
+	public float get(int index) {
+		if (index >= size) throw new IndexOutOfBoundsException();
+		return array[index];
+	}
+
+	public void set(int index, float value) {
+		if (index >= size) throw new IndexOutOfBoundsException();
+		array[index] = value;
+	}
+
+	public int size() {
+		return size;
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/util/HashUtil.java b/src/jbullet/src/javabullet/util/HashUtil.java
new file mode 100644
index 0000000..10e0008
--- /dev/null
+++ b/src/jbullet/src/javabullet/util/HashUtil.java
@@ -0,0 +1,126 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.util;
+
+import gnu.trove.THashMap;
+import gnu.trove.TObjectObjectProcedure;
+import java.util.HashMap;
+import java.util.Iterator;
+
+/**
+ * Wrapper for THashMap (from GNU Trove), with fallback to less effective original HashMap.
+ * 
+ * @author jezek2
+ */
+public class HashUtil {
+	
+	private HashUtil() {}
+	
+	public interface IMap<K,V> {
+		public V get(K key);
+		public V put(K key, V value);
+		public V remove(K key);
+		public int size();
+		public boolean forEachValue(IObjectProcedure<V> proc);
+		public boolean retainEntries(IObjectProcedure<V> proc);
+	}
+	
+	public interface IObjectProcedure<T> {
+		public boolean execute(T value);
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	private static Class mapCls;
+	
+	static {
+		try {
+			mapCls = TroveHashMapImpl.class;
+		}
+		catch (Throwable t) {
+			mapCls = JavaHashMapImpl.class;
+		}
+	}
+	
+	@SuppressWarnings("unchecked")
+	public static <K,V> IMap<K,V> createMap() {
+		try {
+			return (IMap<K,V>)mapCls.newInstance();
+		}
+		catch (IllegalAccessException e) {
+			throw new IllegalStateException(e);
+		}
+		catch (InstantiationException e) {
+			throw new IllegalStateException(e);
+		}
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	protected static class JavaHashMapImpl<K,V> extends HashMap<K,V> implements IMap<K,V> {
+		public boolean forEachValue(IObjectProcedure<V> proc) {
+			for (V value : values()) {
+				if (!proc.execute(value)) return false;
+			}
+			return true;
+		}
+
+		public boolean retainEntries(IObjectProcedure<V> proc) {
+			boolean mod = false;
+			for (Iterator<V> it = values().iterator(); it.hasNext(); ) {
+				V value = it.next();
+				if (!proc.execute(value)) {
+					it.remove();
+					mod = true;
+				}
+			}
+			return mod;
+		}
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	protected static class TroveHashMapImpl<K,V> extends THashMap<K,V> implements IMap<K,V> {
+		private TroveObjectObjectProcedureWrapper<K,V> valueWrapper = new TroveObjectObjectProcedureWrapper<K,V>();
+		
+		public boolean forEachValue(IObjectProcedure<V> proc) {
+			valueWrapper.proc = proc;
+			return forEachEntry(valueWrapper);
+		}
+
+		public boolean retainEntries(IObjectProcedure<V> proc) {
+			valueWrapper.proc = proc;
+			return retainEntries(valueWrapper);
+		}
+	}
+	
+	protected static class TroveObjectObjectProcedureWrapper<K,V> implements TObjectObjectProcedure<K,V> {
+		public IObjectProcedure<V> proc;
+		
+		public boolean execute(K key, V value) {
+			return proc.execute(value);
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/util/HashUtil.java-orig b/src/jbullet/src/javabullet/util/HashUtil.java-orig
new file mode 100644
index 0000000..10e0008
--- /dev/null
+++ b/src/jbullet/src/javabullet/util/HashUtil.java-orig
@@ -0,0 +1,126 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.util;
+
+import gnu.trove.THashMap;
+import gnu.trove.TObjectObjectProcedure;
+import java.util.HashMap;
+import java.util.Iterator;
+
+/**
+ * Wrapper for THashMap (from GNU Trove), with fallback to less effective original HashMap.
+ * 
+ * @author jezek2
+ */
+public class HashUtil {
+	
+	private HashUtil() {}
+	
+	public interface IMap<K,V> {
+		public V get(K key);
+		public V put(K key, V value);
+		public V remove(K key);
+		public int size();
+		public boolean forEachValue(IObjectProcedure<V> proc);
+		public boolean retainEntries(IObjectProcedure<V> proc);
+	}
+	
+	public interface IObjectProcedure<T> {
+		public boolean execute(T value);
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	private static Class mapCls;
+	
+	static {
+		try {
+			mapCls = TroveHashMapImpl.class;
+		}
+		catch (Throwable t) {
+			mapCls = JavaHashMapImpl.class;
+		}
+	}
+	
+	@SuppressWarnings("unchecked")
+	public static <K,V> IMap<K,V> createMap() {
+		try {
+			return (IMap<K,V>)mapCls.newInstance();
+		}
+		catch (IllegalAccessException e) {
+			throw new IllegalStateException(e);
+		}
+		catch (InstantiationException e) {
+			throw new IllegalStateException(e);
+		}
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	protected static class JavaHashMapImpl<K,V> extends HashMap<K,V> implements IMap<K,V> {
+		public boolean forEachValue(IObjectProcedure<V> proc) {
+			for (V value : values()) {
+				if (!proc.execute(value)) return false;
+			}
+			return true;
+		}
+
+		public boolean retainEntries(IObjectProcedure<V> proc) {
+			boolean mod = false;
+			for (Iterator<V> it = values().iterator(); it.hasNext(); ) {
+				V value = it.next();
+				if (!proc.execute(value)) {
+					it.remove();
+					mod = true;
+				}
+			}
+			return mod;
+		}
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	protected static class TroveHashMapImpl<K,V> extends THashMap<K,V> implements IMap<K,V> {
+		private TroveObjectObjectProcedureWrapper<K,V> valueWrapper = new TroveObjectObjectProcedureWrapper<K,V>();
+		
+		public boolean forEachValue(IObjectProcedure<V> proc) {
+			valueWrapper.proc = proc;
+			return forEachEntry(valueWrapper);
+		}
+
+		public boolean retainEntries(IObjectProcedure<V> proc) {
+			valueWrapper.proc = proc;
+			return retainEntries(valueWrapper);
+		}
+	}
+	
+	protected static class TroveObjectObjectProcedureWrapper<K,V> implements TObjectObjectProcedure<K,V> {
+		public IObjectProcedure<V> proc;
+		
+		public boolean execute(K key, V value) {
+			return proc.execute(value);
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/util/IntArrayList.java b/src/jbullet/src/javabullet/util/IntArrayList.java
new file mode 100644
index 0000000..aeab7ba
--- /dev/null
+++ b/src/jbullet/src/javabullet/util/IntArrayList.java
@@ -0,0 +1,71 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+package javabullet.util;
+
+/**
+ *
+ * @author jezek2
+ */
+public class IntArrayList {
+
+	private int[] array = new int[16];
+	private int size;
+	
+	public void add(int value) {
+		if (size == array.length) {
+			expand();
+		}
+		
+		array[size++] = value;
+	}
+	
+	private void expand() {
+		int[] newArray = new int[array.length << 1];
+		System.arraycopy(array, 0, newArray, 0, array.length);
+		array = newArray;
+	}
+
+	public int remove(int index) {
+		if (index >= size) throw new IndexOutOfBoundsException();
+		int old = array[index];
+		System.arraycopy(array, index+1, array, index, size - index - 1);
+		size--;
+		return old;
+	}
+
+	public int get(int index) {
+		if (index >= size) throw new IndexOutOfBoundsException();
+		return array[index];
+	}
+
+	public void set(int index, int value) {
+		if (index >= size) throw new IndexOutOfBoundsException();
+		array[index] = value;
+	}
+
+	public int size() {
+		return size;
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/util/package-info.java b/src/jbullet/src/javabullet/util/package-info.java
new file mode 100644
index 0000000..1e9db35
--- /dev/null
+++ b/src/jbullet/src/javabullet/util/package-info.java
@@ -0,0 +1,28 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+ *
+ * This software is provided 'as-is', without any express or implied warranty.
+ * In no event will the authors be held liable for any damages arising from
+ * the use of this software.
+ * 
+ * Permission is granted to anyone to use this software for any purpose, 
+ * including commercial applications, and to alter it and redistribute it
+ * freely, subject to the following restrictions:
+ * 
+ * 1. The origin of this software must not be misrepresented; you must not
+ *    claim that you wrote the original software. If you use this software
+ *    in a product, an acknowledgment in the product documentation would be
+ *    appreciated but is not required.
+ * 2. Altered source versions must be plainly marked as such, and must not be
+ *    misrepresented as being the original software.
+ * 3. This notice may not be removed or altered from any source distribution.
+ */
+
+/**
+ * Java-specific utility classes.
+ */
+package javabullet.util;
+