Copied JOGL_2_SANDBOX r350 on to trunk; JOGL_2_SANDBOX branch is now closed

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

182 files changed, 28851 insertions, 0 deletions

diff --git a/src/jbullet/build.xml b/src/jbullet/build.xml
new file mode 100644
index 0000000..5cac11e
--- /dev/null
+++ b/src/jbullet/build.xml
@@ -0,0 +1,108 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!-- You may freely edit this file. See commented blocks below for -->
+<!-- some examples of how to customize the build. -->
+<!-- (If you delete it and reopen the project it will be recreated.) -->
+<project name="javabullet" default="default" basedir=".">
+    <description>Builds, tests, and runs the project javabullet.</description>
+    <import file="nbproject/build-impl.xml"/>
+
+    <import file="nbproject/profiler-build-impl.xml"/>    <!--
+
+    There exist several targets which are by default empty and which can be 
+    used for execution of your tasks. These targets are usually executed 
+    before and after some main targets. They are: 
+
+      -pre-init:                 called before initialization of project properties
+      -post-init:                called after initialization of project properties
+      -pre-compile:              called before javac compilation
+      -post-compile:             called after javac compilation
+      -pre-compile-single:       called before javac compilation of single file
+      -post-compile-single:      called after javac compilation of single file
+      -pre-compile-test:         called before javac compilation of JUnit tests
+      -post-compile-test:        called after javac compilation of JUnit tests
+      -pre-compile-test-single:  called before javac compilation of single JUnit test
+      -post-compile-test-single: called after javac compilation of single JUunit test
+      -pre-jar:                  called before JAR building
+      -post-jar:                 called after JAR building
+      -post-clean:               called after cleaning build products
+
+    (Targets beginning with '-' are not intended to be called on their own.)
+
+    Example of inserting an obfuscator after compilation could look like this:
+
+        <target name="-post-compile">
+            <obfuscate>
+                <fileset dir="${build.classes.dir}"/>
+            </obfuscate>
+        </target>
+
+    For list of available properties check the imported 
+    nbproject/build-impl.xml file. 
+
+
+    Another way to customize the build is by overriding existing main targets.
+    The targets of interest are: 
+
+      -init-macrodef-javac:     defines macro for javac compilation
+      -init-macrodef-junit:     defines macro for junit execution
+      -init-macrodef-debug:     defines macro for class debugging
+      -init-macrodef-java:      defines macro for class execution
+      -do-jar-with-manifest:    JAR building (if you are using a manifest)
+      -do-jar-without-manifest: JAR building (if you are not using a manifest)
+      run:                      execution of project 
+      -javadoc-build:           Javadoc generation
+      test-report:              JUnit report generation
+
+    An example of overriding the target for project execution could look like this:
+
+        <target name="run" depends="javabullet-impl.jar">
+            <exec dir="bin" executable="launcher.exe">
+                <arg file="${dist.jar}"/>
+            </exec>
+        </target>
+
+    Notice that the overridden target depends on the jar target and not only on 
+    the compile target as the regular run target does. Again, for a list of available 
+    properties which you can use, check the target you are overriding in the
+    nbproject/build-impl.xml file. 
+
+    -->
+	
+	<target name="-do-jar-without-manifest">
+		<!-- jbullet.jar -->
+		<jar destfile="dist/jbullet.jar" basedir="build/classes" excludes="**/*.java,**/*.form,javabullet/demos/**"/>
+		
+		<!-- jbullet-demos.jar -->
+		<jar destfile="dist/jbullet-demos.jar" basedir="build/classes" includes="javabullet/demos/**" excludes="**/*.java,**/*.form,**/*.diff"/>
+	</target>
+	
+	<target name="webstart" depends="jar">
+		<mkdir dir="dist/webstart"/>
+		<copy todir="dist/webstart">
+			<fileset file="dist/jbullet.jar"/>
+			<fileset file="dist/jbullet-demos.jar"/>
+			<fileset dir="lib/vecmath" includes="**/*.jar"/>
+			<fileset dir="lib/jogl" includes="**/*.jar"/>
+		</copy>
+		
+		<input message="keyPass" addproperty="keypass"/>
+		<input message="storePass" addproperty="storepass" defaultvalue="${keypass}"/>
+			
+		<signjar keystore="keystore" alias="jezek2" keypass="${keypass}" storepass="${storepass}">
+			<fileset dir="dist/webstart">
+				<include name="*.jar"/>
+			</fileset>
+		</signjar>
+	</target>
+
+	<target name="applet" depends="jar">
+		<!-- jbullet-applet.jar -->
+		<jar destfile="dist/jbullet-applet.jar">
+			<zipfileset src="dist/jbullet.jar" excludes="META-INF/**" filemode="644" dirmode="755"/>
+			<zipfileset src="dist/jbullet-demos.jar" excludes="META-INF/**" filemode="644" dirmode="755"/>
+			<zipfileset src="lib/vecmath/vecmath.jar" excludes="META-INF/**" filemode="644" dirmode="755"/>
+			<zipfileset src="lib/swing-layout/swing-layout-1.0.3.jar" excludes="META-INF/**" filemode="644" dirmode="755"/>
+		</jar>
+	</target>
+	
+</project>
diff --git a/src/jbullet/changelog.txt b/src/jbullet/changelog.txt
new file mode 100644
index 0000000..7c8d30c
--- /dev/null
+++ b/src/jbullet/changelog.txt
@@ -0,0 +1,46 @@
+Release 20080311:
+- Added some JavaDoc documentation
+- Added RaycastVehicle and VehicleDemo
+- Refactored accessing of vertex data
+- Added CylinderShape
+- Implemented ray/trimesh hit detection
+- Added applet demo
+- Added binaries and dependant libraries into package
+
+Release 20080303:
+- Refactored enums
+- Fixed bug that caused occasional jitter
+- Added ConvexConcaveCollisionAlgorithm
+- Memory optimalizations
+- Implemented quantized BVH nodes
+- Made ConcaveDemo working
+
+Release 20080206:
+- Memory optimalizations
+- Added heap info
+- Implemented HeapSort
+- Added optional support for GNU Trove
+- Added BspDemo and fixed ConvexHullShape
+- Added ConcaveDemo and it's supporting classes
+- Abstracted OpenGL rendering
+
+Release 20080122:
+- Fixed convex/plane collision detection
+- Added GLDebugDrawer and fixed some bugs
+- Added CapsuleShape
+- Added ConeTwistConstraint, HingeConstraint and Generic6DofConstraint
+- Added GenericJointDemo
+- Optimized drawing of spheres and cylinders using display lists
+- Fixed collision of boxes
+- Added text overlay
+
+Release 20080116:
+- Moved all push/popProfile to try/finally blocks
+- Added final for Vectors/Transforms/etc fields where applicable, and fixed some discovered bugs
+- Fixed bug with non-functional removeOverlappingPair
+- Enabled ground BoxShape in BasicDemo
+- Implemented drawing of BoxShape
+- Fixed VectorUtil.maxAxis
+
+Release 20080111:
+- Initial release based on Bullet 2.66
diff --git a/src/jbullet/lib/java/lang/Enum.class b/src/jbullet/lib/java/lang/Enum.class
new file mode 100644
index 0000000..656aa68
--- /dev/null
+++ b/src/jbullet/lib/java/lang/Enum.class
diff --git a/src/jbullet/lib/java/lang/EnumConstantNotPresentException.class b/src/jbullet/lib/java/lang/EnumConstantNotPresentException.class
new file mode 100644
index 0000000..7f22c80
--- /dev/null
+++ b/src/jbullet/lib/java/lang/EnumConstantNotPresentException.class
diff --git a/src/jbullet/lib/nblibraries-private.properties b/src/jbullet/lib/nblibraries-private.properties
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/src/jbullet/lib/nblibraries-private.properties
diff --git a/src/jbullet/lib/nblibraries.properties b/src/jbullet/lib/nblibraries.properties
new file mode 100644
index 0000000..41ca6cf
--- /dev/null
+++ b/src/jbullet/lib/nblibraries.properties
@@ -0,0 +1,4 @@
+libs.trove.classpath=\
+    ${base}/trove.jar
+libs.vecmath.classpath=\
+    ${base}/vecmath.jar
diff --git a/src/jbullet/lib/trove.jar b/src/jbullet/lib/trove.jar
new file mode 100644
index 0000000..0f1b063
--- /dev/null
+++ b/src/jbullet/lib/trove.jar
diff --git a/src/jbullet/lib/vecmath.jar b/src/jbullet/lib/vecmath.jar
new file mode 100644
index 0000000..ddfd73b
--- /dev/null
+++ b/src/jbullet/lib/vecmath.jar
diff --git a/src/jbullet/make-all.sh b/src/jbullet/make-all.sh
new file mode 100644
index 0000000..2c4ca3e
--- /dev/null
+++ b/src/jbullet/make-all.sh
@@ -0,0 +1,5 @@
+#! /bin/sh
+
+sh make-cdc-es1.sh jar
+sh make-jar.sh 
+cp -v ragdoll.jar /jordan/data/packs/sun/mobile/apx2500.phone_image
diff --git a/src/jbullet/make-cdc-es1.sh b/src/jbullet/make-cdc-es1.sh
new file mode 100644
index 0000000..204b6b9
--- /dev/null
+++ b/src/jbullet/make-cdc-es1.sh
@@ -0,0 +1,7 @@
+#! /bin/sh
+
+#    -Djavac.bootclasspath.jar=$(pwd)/../../../gluegen/make/lib/cdc_fp.jar \
+#ant -Djavac.source=1.5 -Djavac.bootclasspath.jar=/usr/local/projects/SUN/JOGL2/gluegen/make/lib/cdc_fp.jar 2>&1 | tee make-cdc-es1.log
+ant -v \
+    -Djogl.home=$JOGL_HOME \
+    $* 2>&1 | tee make-cdc-es1.log
diff --git a/src/jbullet/make-jar.sh b/src/jbullet/make-jar.sh
new file mode 100644
index 0000000..c991a61
--- /dev/null
+++ b/src/jbullet/make-jar.sh
@@ -0,0 +1,8 @@
+rm -f ragdoll.jar 
+cd build/classes/
+jar xf ../../lib/vecmath.jar 
+#jar xf ../../lib/trove.jar
+cp -a  ../../lib/java .
+rm -rf META-INF
+find . -name \*nope -exec rm -fv \{\} \;
+jar cf ../../ragdoll.jar .
diff --git a/src/jbullet/nbproject/build-impl.xml b/src/jbullet/nbproject/build-impl.xml
new file mode 100644
index 0000000..70cc4e9
--- /dev/null
+++ b/src/jbullet/nbproject/build-impl.xml
@@ -0,0 +1,650 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--
+*** GENERATED FROM project.xml - DO NOT EDIT  ***
+***         EDIT ../build.xml INSTEAD         ***
+
+For the purpose of easier reading the script
+is divided into following sections:
+
+  - initialization
+  - compilation
+  - jar
+  - execution
+  - debugging
+  - javadoc
+  - junit compilation
+  - junit execution
+  - junit debugging
+  - applet
+  - cleanup
+
+        -->
+<project xmlns:j2seproject1="http://www.netbeans.org/ns/j2se-project/1" xmlns:j2seproject3="http://www.netbeans.org/ns/j2se-project/3" xmlns:jaxrpc="http://www.netbeans.org/ns/j2se-project/jax-rpc" basedir=".." default="default" name="jbullet-impl">
+    <target depends="test,jar,javadoc" description="Build and test whole project." name="default"/>
+    <!-- 
+                ======================
+                INITIALIZATION SECTION 
+                ======================
+            -->
+    <target name="-pre-init">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target depends="-pre-init" name="-init-private">
+        <property file="nbproject/private/config.properties"/>
+        <property file="nbproject/private/configs/${config}.properties"/>
+        <property file="nbproject/private/private.properties"/>
+    </target>
+    <target depends="-pre-init,-init-private" name="-init-libraries">
+        <property location="lib/nblibraries.properties" name="libraries.1.path"/>
+        <dirname file="${libraries.1.path}" property="libraries.1.dir.nativedirsep"/>
+        <pathconvert dirsep="/" property="libraries.1.dir">
+            <path path="${libraries.1.dir.nativedirsep}"/>
+        </pathconvert>
+        <basename file="${libraries.1.path}" property="libraries.1.basename" suffix=".properties"/>
+        <touch file="${libraries.1.dir}/${libraries.1.basename}-private.properties"/>
+        <loadproperties srcfile="${libraries.1.dir}/${libraries.1.basename}-private.properties">
+            <filterchain>
+                <replacestring from="$${base}" to="${libraries.1.dir}"/>
+            </filterchain>
+        </loadproperties>
+        <loadproperties srcfile="${libraries.1.path}">
+            <filterchain>
+                <replacestring from="$${base}" to="${libraries.1.dir}"/>
+            </filterchain>
+        </loadproperties>
+    </target>
+    <target depends="-pre-init,-init-private,-init-libraries" name="-init-user">
+        <property file="${user.properties.file}"/>
+        <!-- The two properties below are usually overridden -->
+        <!-- by the active platform. Just a fallback. -->
+        <property name="default.javac.source" value="1.4"/>
+        <property name="default.javac.target" value="1.4"/>
+    </target>
+    <target depends="-pre-init,-init-private,-init-libraries,-init-user" name="-init-project">
+        <property file="nbproject/configs/${config}.properties"/>
+        <property file="nbproject/project.properties"/>
+    </target>
+    <target depends="-pre-init,-init-private,-init-libraries,-init-user,-init-project,-init-macrodef-property" name="-do-init">
+        <available file="${manifest.file}" property="manifest.available"/>
+        <condition property="manifest.available+main.class">
+            <and>
+                <isset property="manifest.available"/>
+                <isset property="main.class"/>
+                <not>
+                    <equals arg1="${main.class}" arg2="" trim="true"/>
+                </not>
+            </and>
+        </condition>
+        <condition property="manifest.available+main.class+mkdist.available">
+            <and>
+                <istrue value="${manifest.available+main.class}"/>
+                <isset property="libs.CopyLibs.classpath"/>
+            </and>
+        </condition>
+        <condition property="have.tests">
+            <or>
+                <available file="${test.src.dir}"/>
+            </or>
+        </condition>
+        <condition property="have.sources">
+            <or>
+                <available file="${src.dir}"/>
+            </or>
+        </condition>
+        <condition property="netbeans.home+have.tests">
+            <and>
+                <isset property="netbeans.home"/>
+                <isset property="have.tests"/>
+            </and>
+        </condition>
+        <condition property="no.javadoc.preview">
+            <and>
+                <isset property="javadoc.preview"/>
+                <isfalse value="${javadoc.preview}"/>
+            </and>
+        </condition>
+        <property name="run.jvmargs" value=""/>
+        <condition property="javac.compilerargs" value="-bootclasspath ${javac.bootclasspath}">
+                <isset property="javac.bootclasspath"/>
+        </condition>
+        <property name="work.dir" value="${basedir}"/>
+        <condition property="no.deps">
+            <and>
+                <istrue value="${no.dependencies}"/>
+            </and>
+        </condition>
+        <property name="javac.debug" value="true"/>
+        <property name="javadoc.preview" value="true"/>
+        <property name="application.args" value=""/>
+        <property name="source.encoding" value="${file.encoding}"/>
+        <condition property="javadoc.encoding.used" value="${javadoc.encoding}">
+            <and>
+                <isset property="javadoc.encoding"/>
+                <not>
+                    <equals arg1="${javadoc.encoding}" arg2=""/>
+                </not>
+            </and>
+        </condition>
+        <property name="javadoc.encoding.used" value="${source.encoding}"/>
+        <property name="includes" value="**"/>
+        <property name="excludes" value=""/>
+        <property name="do.depend" value="false"/>
+        <condition property="do.depend.true">
+            <istrue value="${do.depend}"/>
+        </condition>
+        <condition else="" property="javac.compilerargs.jaxws" value="-Djava.endorsed.dirs='${jaxws.endorsed.dir}'">
+            <and>
+                <isset property="jaxws.endorsed.dir"/>
+                <available file="nbproject/jaxws-build.xml"/>
+            </and>
+        </condition>
+    </target>
+    <target name="-post-init">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target depends="-pre-init,-init-private,-init-libraries,-init-user,-init-project,-do-init" name="-init-check">
+        <fail unless="src.dir">Must set src.dir</fail>
+        <fail unless="test.src.dir">Must set test.src.dir</fail>
+        <fail unless="build.dir">Must set build.dir</fail>
+        <fail unless="dist.dir">Must set dist.dir</fail>
+        <fail unless="build.classes.dir">Must set build.classes.dir</fail>
+        <fail unless="dist.javadoc.dir">Must set dist.javadoc.dir</fail>
+        <fail unless="build.test.classes.dir">Must set build.test.classes.dir</fail>
+        <fail unless="build.test.results.dir">Must set build.test.results.dir</fail>
+        <fail unless="build.classes.excludes">Must set build.classes.excludes</fail>
+        <fail unless="dist.jar">Must set dist.jar</fail>
+    </target>
+    <target name="-init-macrodef-property">
+        <macrodef name="property" uri="http://www.netbeans.org/ns/j2se-project/1">
+            <attribute name="name"/>
+            <attribute name="value"/>
+            <sequential>
+                <property name="@{name}" value="${@{value}}"/>
+            </sequential>
+        </macrodef>
+    </target>
+    <target name="-init-macrodef-javac">
+        <macrodef name="javac" uri="http://www.netbeans.org/ns/j2se-project/3">
+            <attribute default="${src.dir}" name="srcdir"/>
+            <attribute default="${build.classes.dir}" name="destdir"/>
+            <attribute default="${javac.classpath}" name="classpath"/>
+            <attribute default="${includes}" name="includes"/>
+            <attribute default="${excludes}" name="excludes"/>
+            <attribute default="${javac.debug}" name="debug"/>
+            <attribute default="" name="sourcepath"/>
+            <element name="customize" optional="true"/>
+            <sequential>
+                <javac debug="@{debug}" deprecation="${javac.deprecation}" destdir="@{destdir}" encoding="${source.encoding}" excludes="@{excludes}" includeantruntime="false" includes="@{includes}" source="${javac.source}" sourcepath="@{sourcepath}" srcdir="@{srcdir}" target="${javac.target}" bootclasspath="${javac.bootclasspath.jar}">
+                    <classpath>
+                        <path path="@{classpath}"/>
+                    </classpath>
+                    <compilerarg line="${javac.compilerargs} ${javac.compilerargs.jaxws}"/>
+                    <customize/>
+                </javac>
+            </sequential>
+        </macrodef>
+        <macrodef name="depend" uri="http://www.netbeans.org/ns/j2se-project/3">
+            <attribute default="${src.dir}" name="srcdir"/>
+            <attribute default="${build.classes.dir}" name="destdir"/>
+            <attribute default="${javac.classpath}" name="classpath"/>
+            <sequential>
+                <depend cache="${build.dir}/depcache" destdir="@{destdir}" excludes="${excludes}" includes="${includes}" srcdir="@{srcdir}">
+                    <classpath>
+                        <path path="@{classpath}"/>
+                    </classpath>
+                </depend>
+            </sequential>
+        </macrodef>
+        <macrodef name="force-recompile" uri="http://www.netbeans.org/ns/j2se-project/3">
+            <attribute default="${build.classes.dir}" name="destdir"/>
+            <sequential>
+                <fail unless="javac.includes">Must set javac.includes</fail>
+                <pathconvert pathsep="," property="javac.includes.binary">
+                    <path>
+                        <filelist dir="@{destdir}" files="${javac.includes}"/>
+                    </path>
+                    <globmapper from="*.java" to="*.class"/>
+                </pathconvert>
+                <delete>
+                    <files includes="${javac.includes.binary}"/>
+                </delete>
+            </sequential>
+        </macrodef>
+    </target>
+    <target name="-init-macrodef-junit">
+        <macrodef name="junit" uri="http://www.netbeans.org/ns/j2se-project/3">
+            <attribute default="${includes}" name="includes"/>
+            <attribute default="${excludes}" name="excludes"/>
+            <attribute default="**" name="testincludes"/>
+            <sequential>
+                <junit dir="${work.dir}" errorproperty="tests.failed" failureproperty="tests.failed" fork="true" showoutput="true">
+                    <batchtest todir="${build.test.results.dir}">
+                        <fileset dir="${test.src.dir}" excludes="@{excludes},${excludes}" includes="@{includes}">
+                            <filename name="@{testincludes}"/>
+                        </fileset>
+                    </batchtest>
+                    <classpath>
+                        <path path="${run.test.classpath}"/>
+                    </classpath>
+                    <syspropertyset>
+                        <propertyref prefix="test-sys-prop."/>
+                        <mapper from="test-sys-prop.*" to="*" type="glob"/>
+                    </syspropertyset>
+                    <formatter type="brief" usefile="false"/>
+                    <formatter type="xml"/>
+                    <jvmarg line="${run.jvmargs}"/>
+                </junit>
+            </sequential>
+        </macrodef>
+    </target>
+    <target name="-init-macrodef-nbjpda">
+        <macrodef name="nbjpdastart" uri="http://www.netbeans.org/ns/j2se-project/1">
+            <attribute default="${main.class}" name="name"/>
+            <attribute default="${debug.classpath}" name="classpath"/>
+            <attribute default="" name="stopclassname"/>
+            <sequential>
+                <nbjpdastart addressproperty="jpda.address" name="@{name}" stopclassname="@{stopclassname}" transport="dt_socket">
+                    <classpath>
+                        <path path="@{classpath}"/>
+                    </classpath>
+                </nbjpdastart>
+            </sequential>
+        </macrodef>
+        <macrodef name="nbjpdareload" uri="http://www.netbeans.org/ns/j2se-project/1">
+            <attribute default="${build.classes.dir}" name="dir"/>
+            <sequential>
+                <nbjpdareload>
+                    <fileset dir="@{dir}" includes="${fix.classes}">
+                        <include name="${fix.includes}*.class"/>
+                    </fileset>
+                </nbjpdareload>
+            </sequential>
+        </macrodef>
+    </target>
+    <target name="-init-debug-args">
+        <property name="version-output" value="java version &quot;${ant.java.version}"/>
+        <condition property="have-jdk-older-than-1.4">
+            <or>
+                <contains string="${version-output}" substring="java version &quot;1.0"/>
+                <contains string="${version-output}" substring="java version &quot;1.1"/>
+                <contains string="${version-output}" substring="java version &quot;1.2"/>
+                <contains string="${version-output}" substring="java version &quot;1.3"/>
+            </or>
+        </condition>
+        <condition else="-Xdebug" property="debug-args-line" value="-Xdebug -Xnoagent -Djava.compiler=none">
+            <istrue value="${have-jdk-older-than-1.4}"/>
+        </condition>
+    </target>
+    <target depends="-init-debug-args" name="-init-macrodef-debug">
+        <macrodef name="debug" uri="http://www.netbeans.org/ns/j2se-project/3">
+            <attribute default="${main.class}" name="classname"/>
+            <attribute default="${debug.classpath}" name="classpath"/>
+            <element name="customize" optional="true"/>
+            <sequential>
+                <java classname="@{classname}" dir="${work.dir}" fork="true">
+                    <jvmarg line="${debug-args-line}"/>
+                    <jvmarg value="-Xrunjdwp:transport=dt_socket,address=${jpda.address}"/>
+                    <jvmarg line="${run.jvmargs}"/>
+                    <classpath>
+                        <path path="@{classpath}"/>
+                    </classpath>
+                    <syspropertyset>
+                        <propertyref prefix="run-sys-prop."/>
+                        <mapper from="run-sys-prop.*" to="*" type="glob"/>
+                    </syspropertyset>
+                    <customize/>
+                </java>
+            </sequential>
+        </macrodef>
+    </target>
+    <target name="-init-macrodef-java">
+        <macrodef name="java" uri="http://www.netbeans.org/ns/j2se-project/1">
+            <attribute default="${main.class}" name="classname"/>
+            <element name="customize" optional="true"/>
+            <sequential>
+                <java classname="@{classname}" dir="${work.dir}" fork="true">
+                    <jvmarg line="${run.jvmargs}"/>
+                    <classpath>
+                        <path path="${run.classpath}"/>
+                    </classpath>
+                    <syspropertyset>
+                        <propertyref prefix="run-sys-prop."/>
+                        <mapper from="run-sys-prop.*" to="*" type="glob"/>
+                    </syspropertyset>
+                    <customize/>
+                </java>
+            </sequential>
+        </macrodef>
+    </target>
+    <target name="-init-presetdef-jar">
+        <presetdef name="jar" uri="http://www.netbeans.org/ns/j2se-project/1">
+            <jar compress="${jar.compress}" jarfile="${dist.jar}">
+                <j2seproject1:fileset dir="${build.classes.dir}"/>
+            </jar>
+        </presetdef>
+    </target>
+    <target depends="-pre-init,-init-private,-init-libraries,-init-user,-init-project,-do-init,-post-init,-init-check,-init-macrodef-property,-init-macrodef-javac,-init-macrodef-junit,-init-macrodef-nbjpda,-init-macrodef-debug,-init-macrodef-java,-init-presetdef-jar" name="init"/>
+    <!--
+                ===================
+                COMPILATION SECTION
+                ===================
+            -->
+    <target depends="init" name="deps-jar" unless="no.deps"/>
+    <target depends="init,deps-jar" name="-pre-pre-compile">
+        <mkdir dir="${build.classes.dir}"/>
+    </target>
+    <target name="-pre-compile">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target if="do.depend.true" name="-compile-depend">
+        <j2seproject3:depend/>
+    </target>
+    <target depends="init,deps-jar,-pre-pre-compile,-pre-compile,-compile-depend" if="have.sources" name="-do-compile">
+        <j2seproject3:javac/>
+        <copy todir="${build.classes.dir}">
+            <fileset dir="${src.dir}" excludes="${build.classes.excludes},${excludes}" includes="${includes}"/>
+        </copy>
+    </target>
+    <target name="-post-compile">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target depends="init,deps-jar,-pre-pre-compile,-pre-compile,-do-compile,-post-compile" description="Compile project." name="compile"/>
+    <target name="-pre-compile-single">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target depends="init,deps-jar,-pre-pre-compile" name="-do-compile-single">
+        <fail unless="javac.includes">Must select some files in the IDE or set javac.includes</fail>
+        <j2seproject3:force-recompile/>
+        <j2seproject3:javac excludes="" includes="${javac.includes}" sourcepath="${src.dir}"/>
+    </target>
+    <target name="-post-compile-single">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target depends="init,deps-jar,-pre-pre-compile,-pre-compile-single,-do-compile-single,-post-compile-single" name="compile-single"/>
+    <!--
+                ====================
+                JAR BUILDING SECTION
+                ====================
+            -->
+    <target depends="init" name="-pre-pre-jar">
+        <dirname file="${dist.jar}" property="dist.jar.dir"/>
+        <mkdir dir="${dist.jar.dir}"/>
+    </target>
+    <target name="-pre-jar">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target depends="init,compile,-pre-pre-jar,-pre-jar" name="-do-jar-without-manifest" unless="manifest.available">
+        <j2seproject1:jar/>
+    </target>
+    <target depends="init,compile,-pre-pre-jar,-pre-jar" if="manifest.available" name="-do-jar-with-manifest" unless="manifest.available+main.class">
+        <j2seproject1:jar manifest="${manifest.file}"/>
+    </target>
+    <target depends="init,compile,-pre-pre-jar,-pre-jar" if="manifest.available+main.class" name="-do-jar-with-mainclass" unless="manifest.available+main.class+mkdist.available">
+        <j2seproject1:jar manifest="${manifest.file}">
+            <j2seproject1:manifest>
+                <j2seproject1:attribute name="Main-Class" value="${main.class}"/>
+            </j2seproject1:manifest>
+        </j2seproject1:jar>
+        <echo>To run this application from the command line without Ant, try:</echo>
+        <property location="${build.classes.dir}" name="build.classes.dir.resolved"/>
+        <property location="${dist.jar}" name="dist.jar.resolved"/>
+        <pathconvert property="run.classpath.with.dist.jar">
+            <path path="${run.classpath}"/>
+            <map from="${build.classes.dir.resolved}" to="${dist.jar.resolved}"/>
+        </pathconvert>
+        <echo>java -cp "${run.classpath.with.dist.jar}" ${main.class}</echo>
+    </target>
+    <target depends="init,compile,-pre-pre-jar,-pre-jar" if="manifest.available+main.class+mkdist.available" name="-do-jar-with-libraries">
+        <property location="${build.classes.dir}" name="build.classes.dir.resolved"/>
+        <pathconvert property="run.classpath.without.build.classes.dir">
+            <path path="${run.classpath}"/>
+            <map from="${build.classes.dir.resolved}" to=""/>
+        </pathconvert>
+        <pathconvert pathsep=" " property="jar.classpath">
+            <path path="${run.classpath.without.build.classes.dir}"/>
+            <chainedmapper>
+                <flattenmapper/>
+                <globmapper from="*" to="lib/*"/>
+            </chainedmapper>
+        </pathconvert>
+        <taskdef classname="org.netbeans.modules.java.j2seproject.copylibstask.CopyLibs" classpath="${libs.CopyLibs.classpath}" name="copylibs"/>
+        <copylibs compress="${jar.compress}" jarfile="${dist.jar}" manifest="${manifest.file}" runtimeclasspath="${run.classpath.without.build.classes.dir}">
+            <fileset dir="${build.classes.dir}"/>
+            <manifest>
+                <attribute name="Main-Class" value="${main.class}"/>
+                <attribute name="Class-Path" value="${jar.classpath}"/>
+            </manifest>
+        </copylibs>
+        <echo>To run this application from the command line without Ant, try:</echo>
+        <property location="${dist.jar}" name="dist.jar.resolved"/>
+        <echo>java -jar "${dist.jar.resolved}"</echo>
+    </target>
+    <target name="-post-jar">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target depends="init,compile,-pre-jar,-do-jar-with-manifest,-do-jar-without-manifest,-do-jar-with-mainclass,-do-jar-with-libraries,-post-jar" description="Build JAR." name="jar"/>
+    <!--
+                =================
+                EXECUTION SECTION
+                =================
+            -->
+    <target depends="init,compile" description="Run a main class." name="run">
+        <j2seproject1:java>
+            <customize>
+                <arg line="${application.args}"/>
+            </customize>
+        </j2seproject1:java>
+    </target>
+    <target name="-do-not-recompile">
+        <property name="javac.includes.binary" value=""/>
+    </target>
+    <target depends="init,-do-not-recompile,compile-single" name="run-single">
+        <fail unless="run.class">Must select one file in the IDE or set run.class</fail>
+        <j2seproject1:java classname="${run.class}"/>
+    </target>
+    <!--
+                =================
+                DEBUGGING SECTION
+                =================
+            -->
+    <target depends="init" if="netbeans.home" name="-debug-start-debugger">
+        <j2seproject1:nbjpdastart name="${debug.class}"/>
+    </target>
+    <target depends="init,compile" name="-debug-start-debuggee">
+        <j2seproject3:debug>
+            <customize>
+                <arg line="${application.args}"/>
+            </customize>
+        </j2seproject3:debug>
+    </target>
+    <target depends="init,compile,-debug-start-debugger,-debug-start-debuggee" description="Debug project in IDE." if="netbeans.home" name="debug"/>
+    <target depends="init" if="netbeans.home" name="-debug-start-debugger-stepinto">
+        <j2seproject1:nbjpdastart stopclassname="${main.class}"/>
+    </target>
+    <target depends="init,compile,-debug-start-debugger-stepinto,-debug-start-debuggee" if="netbeans.home" name="debug-stepinto"/>
+    <target depends="init,compile-single" if="netbeans.home" name="-debug-start-debuggee-single">
+        <fail unless="debug.class">Must select one file in the IDE or set debug.class</fail>
+        <j2seproject3:debug classname="${debug.class}"/>
+    </target>
+    <target depends="init,-do-not-recompile,compile-single,-debug-start-debugger,-debug-start-debuggee-single" if="netbeans.home" name="debug-single"/>
+    <target depends="init" name="-pre-debug-fix">
+        <fail unless="fix.includes">Must set fix.includes</fail>
+        <property name="javac.includes" value="${fix.includes}.java"/>
+    </target>
+    <target depends="init,-pre-debug-fix,compile-single" if="netbeans.home" name="-do-debug-fix">
+        <j2seproject1:nbjpdareload/>
+    </target>
+    <target depends="init,-pre-debug-fix,-do-debug-fix" if="netbeans.home" name="debug-fix"/>
+    <!--
+                ===============
+                JAVADOC SECTION
+                ===============
+            -->
+    <target depends="init" name="-javadoc-build">
+        <mkdir dir="${dist.javadoc.dir}"/>
+        <javadoc additionalparam="${javadoc.additionalparam}" author="${javadoc.author}" charset="UTF-8" destdir="${dist.javadoc.dir}" docencoding="UTF-8" encoding="${javadoc.encoding.used}" failonerror="true" noindex="${javadoc.noindex}" nonavbar="${javadoc.nonavbar}" notree="${javadoc.notree}" private="${javadoc.private}" source="${javac.source}" splitindex="${javadoc.splitindex}" use="${javadoc.use}" useexternalfile="true" version="${javadoc.version}" windowtitle="${javadoc.windowtitle}">
+            <classpath>
+                <path path="${javac.classpath}"/>
+            </classpath>
+            <fileset dir="${src.dir}" excludes="${excludes}" includes="${includes}">
+                <filename name="**/*.java"/>
+            </fileset>
+        </javadoc>
+    </target>
+    <target depends="init,-javadoc-build" if="netbeans.home" name="-javadoc-browse" unless="no.javadoc.preview">
+        <nbbrowse file="${dist.javadoc.dir}/index.html"/>
+    </target>
+    <target depends="init,-javadoc-build,-javadoc-browse" description="Build Javadoc." name="javadoc"/>
+    <!--
+                =========================
+                JUNIT COMPILATION SECTION
+                =========================
+            -->
+    <target depends="init,compile" if="have.tests" name="-pre-pre-compile-test">
+        <mkdir dir="${build.test.classes.dir}"/>
+    </target>
+    <target name="-pre-compile-test">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target if="do.depend.true" name="-compile-test-depend">
+        <j2seproject3:depend classpath="${javac.test.classpath}" destdir="${build.test.classes.dir}" srcdir="${test.src.dir}"/>
+    </target>
+    <target depends="init,compile,-pre-pre-compile-test,-pre-compile-test,-compile-test-depend" if="have.tests" name="-do-compile-test">
+        <j2seproject3:javac classpath="${javac.test.classpath}" debug="true" destdir="${build.test.classes.dir}" srcdir="${test.src.dir}"/>
+        <copy todir="${build.test.classes.dir}">
+            <fileset dir="${test.src.dir}" excludes="${build.classes.excludes},${excludes}" includes="${includes}"/>
+        </copy>
+    </target>
+    <target name="-post-compile-test">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target depends="init,compile,-pre-pre-compile-test,-pre-compile-test,-do-compile-test,-post-compile-test" name="compile-test"/>
+    <target name="-pre-compile-test-single">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target depends="init,compile,-pre-pre-compile-test,-pre-compile-test-single" if="have.tests" name="-do-compile-test-single">
+        <fail unless="javac.includes">Must select some files in the IDE or set javac.includes</fail>
+        <j2seproject3:force-recompile destdir="${build.test.classes.dir}"/>
+        <j2seproject3:javac classpath="${javac.test.classpath}" debug="true" destdir="${build.test.classes.dir}" excludes="" includes="${javac.includes}" sourcepath="${test.src.dir}" srcdir="${test.src.dir}"/>
+        <copy todir="${build.test.classes.dir}">
+            <fileset dir="${test.src.dir}" excludes="${build.classes.excludes},${excludes}" includes="${includes}"/>
+        </copy>
+    </target>
+    <target name="-post-compile-test-single">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target depends="init,compile,-pre-pre-compile-test,-pre-compile-test-single,-do-compile-test-single,-post-compile-test-single" name="compile-test-single"/>
+    <!--
+                =======================
+                JUNIT EXECUTION SECTION
+                =======================
+            -->
+    <target depends="init" if="have.tests" name="-pre-test-run">
+        <mkdir dir="${build.test.results.dir}"/>
+    </target>
+    <target depends="init,compile-test,-pre-test-run" if="have.tests" name="-do-test-run">
+        <j2seproject3:junit testincludes="**/*Test.java"/>
+    </target>
+    <target depends="init,compile-test,-pre-test-run,-do-test-run" if="have.tests" name="-post-test-run">
+        <fail if="tests.failed">Some tests failed; see details above.</fail>
+    </target>
+    <target depends="init" if="have.tests" name="test-report"/>
+    <target depends="init" if="netbeans.home+have.tests" name="-test-browse"/>
+    <target depends="init,compile-test,-pre-test-run,-do-test-run,test-report,-post-test-run,-test-browse" description="Run unit tests." name="test"/>
+    <target depends="init" if="have.tests" name="-pre-test-run-single">
+        <mkdir dir="${build.test.results.dir}"/>
+    </target>
+    <target depends="init,compile-test-single,-pre-test-run-single" if="have.tests" name="-do-test-run-single">
+        <fail unless="test.includes">Must select some files in the IDE or set test.includes</fail>
+        <j2seproject3:junit excludes="" includes="${test.includes}"/>
+    </target>
+    <target depends="init,compile-test-single,-pre-test-run-single,-do-test-run-single" if="have.tests" name="-post-test-run-single">
+        <fail if="tests.failed">Some tests failed; see details above.</fail>
+    </target>
+    <target depends="init,-do-not-recompile,compile-test-single,-pre-test-run-single,-do-test-run-single,-post-test-run-single" description="Run single unit test." name="test-single"/>
+    <!--
+                =======================
+                JUNIT DEBUGGING SECTION
+                =======================
+            -->
+    <target depends="init,compile-test" if="have.tests" name="-debug-start-debuggee-test">
+        <fail unless="test.class">Must select one file in the IDE or set test.class</fail>
+        <property location="${build.test.results.dir}/TEST-${test.class}.xml" name="test.report.file"/>
+        <delete file="${test.report.file}"/>
+        <mkdir dir="${build.test.results.dir}"/>
+        <j2seproject3:debug classname="org.apache.tools.ant.taskdefs.optional.junit.JUnitTestRunner" classpath="${ant.home}/lib/ant.jar:${ant.home}/lib/ant-junit.jar:${debug.test.classpath}">
+            <customize>
+                <syspropertyset>
+                    <propertyref prefix="test-sys-prop."/>
+                    <mapper from="test-sys-prop.*" to="*" type="glob"/>
+                </syspropertyset>
+                <arg value="${test.class}"/>
+                <arg value="showoutput=true"/>
+                <arg value="formatter=org.apache.tools.ant.taskdefs.optional.junit.BriefJUnitResultFormatter"/>
+                <arg value="formatter=org.apache.tools.ant.taskdefs.optional.junit.XMLJUnitResultFormatter,${test.report.file}"/>
+            </customize>
+        </j2seproject3:debug>
+    </target>
+    <target depends="init,compile-test" if="netbeans.home+have.tests" name="-debug-start-debugger-test">
+        <j2seproject1:nbjpdastart classpath="${debug.test.classpath}" name="${test.class}"/>
+    </target>
+    <target depends="init,-do-not-recompile,compile-test-single,-debug-start-debugger-test,-debug-start-debuggee-test" name="debug-test"/>
+    <target depends="init,-pre-debug-fix,compile-test-single" if="netbeans.home" name="-do-debug-fix-test">
+        <j2seproject1:nbjpdareload dir="${build.test.classes.dir}"/>
+    </target>
+    <target depends="init,-pre-debug-fix,-do-debug-fix-test" if="netbeans.home" name="debug-fix-test"/>
+    <!--
+                =========================
+                APPLET EXECUTION SECTION
+                =========================
+            -->
+    <target depends="init,compile-single" name="run-applet">
+        <fail unless="applet.url">Must select one file in the IDE or set applet.url</fail>
+        <j2seproject1:java classname="sun.applet.AppletViewer">
+            <customize>
+                <arg value="${applet.url}"/>
+            </customize>
+        </j2seproject1:java>
+    </target>
+    <!--
+                =========================
+                APPLET DEBUGGING  SECTION
+                =========================
+            -->
+    <target depends="init,compile-single" if="netbeans.home" name="-debug-start-debuggee-applet">
+        <fail unless="applet.url">Must select one file in the IDE or set applet.url</fail>
+        <j2seproject3:debug classname="sun.applet.AppletViewer">
+            <customize>
+                <arg value="${applet.url}"/>
+            </customize>
+        </j2seproject3:debug>
+    </target>
+    <target depends="init,compile-single,-debug-start-debugger,-debug-start-debuggee-applet" if="netbeans.home" name="debug-applet"/>
+    <!--
+                ===============
+                CLEANUP SECTION
+                ===============
+            -->
+    <target depends="init" name="deps-clean" unless="no.deps"/>
+    <target depends="init" name="-do-clean">
+        <delete dir="${build.dir}"/>
+        <delete dir="${dist.dir}"/>
+    </target>
+    <target name="-post-clean">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target depends="init,deps-clean,-do-clean,-post-clean" description="Clean build products." name="clean"/>
+</project>
diff --git a/src/jbullet/nbproject/genfiles.properties b/src/jbullet/nbproject/genfiles.properties
new file mode 100644
index 0000000..80f3a63
--- /dev/null
+++ b/src/jbullet/nbproject/genfiles.properties
@@ -0,0 +1,11 @@
+build.xml.data.CRC32=ff8e82d1
+build.xml.script.CRC32=26cff537
+build.xml.stylesheet.CRC32=be360661
+# This file is used by a NetBeans-based IDE to track changes in generated files such as build-impl.xml.
+# Do not edit this file. You may delete it but then the IDE will never regenerate such files for you.
+nbproject/build-impl.xml.data.CRC32=aa374877
+nbproject/build-impl.xml.script.CRC32=daffc434
+nbproject/build-impl.xml.stylesheet.CRC32=487672f9
+nbproject/profiler-build-impl.xml.data.CRC32=ff8e82d1
+nbproject/profiler-build-impl.xml.script.CRC32=abda56ed
+nbproject/profiler-build-impl.xml.stylesheet.CRC32=42cb6bcf
diff --git a/src/jbullet/nbproject/profiler-build-impl.xml b/src/jbullet/nbproject/profiler-build-impl.xml
new file mode 100644
index 0000000..7c8995d
--- /dev/null
+++ b/src/jbullet/nbproject/profiler-build-impl.xml
@@ -0,0 +1,131 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--
+*** GENERATED FROM project.xml - DO NOT EDIT  ***
+***         EDIT ../build.xml INSTEAD         ***
+
+For the purpose of easier reading the script
+is divided into following sections:
+
+  - initialization
+  - profiling
+  - applet profiling
+
+-->
+<project name="-profiler-impl" default="profile" basedir="..">
+    <target name="default" depends="profile" description="Build and profile the project."/>
+    <!--
+    ======================
+    INITIALIZATION SECTION
+    ======================
+    -->
+    <target name="profile-init" depends="-profile-pre-init, init, -profile-post-init, -profile-init-macrodef-profile, -profile-init-check"/>
+    <target name="-profile-pre-init">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target name="-profile-post-init">
+        <!-- Empty placeholder for easier customization. -->
+        <!-- You can override this target in the ../build.xml file. -->
+    </target>
+    <target name="-profile-init-macrodef-profile">
+        <macrodef name="resolve">
+            <attribute name="name"/>
+            <attribute name="value"/>
+            <sequential>
+                <property name="@{name}" value="${env.@{value}}"/>
+            </sequential>
+        </macrodef>
+        <macrodef name="profile">
+            <attribute name="classname" default="${main.class}"/>
+            <element name="customize" optional="true"/>
+            <sequential>
+                <property environment="env"/>
+                <resolve name="profiler.current.path" value="${profiler.info.pathvar}"/>
+                <java fork="true" classname="@{classname}" dir="${profiler.info.dir}" jvm="${profiler.info.jvm}">
+                    <jvmarg value="${profiler.info.jvmargs.agent}"/>
+                    <jvmarg line="${profiler.info.jvmargs}"/>
+                    <env key="${profiler.info.pathvar}" path="${profiler.info.agentpath}:${profiler.current.path}"/>
+                    <arg line="${application.args}"/>
+                    <classpath>
+                        <path path="${run.classpath}"/>
+                    </classpath>
+                    <syspropertyset>
+                        <propertyref prefix="run-sys-prop."/>
+                        <mapper type="glob" from="run-sys-prop.*" to="*"/>
+                    </syspropertyset>
+                    <customize/>
+                </java>
+            </sequential>
+        </macrodef>
+    </target>
+    <target name="-profile-init-check" depends="-profile-pre-init, init, -profile-post-init, -profile-init-macrodef-profile">
+        <fail unless="profiler.info.jvm">Must set JVM to use for profiling in profiler.info.jvm</fail>
+        <fail unless="profiler.info.jvmargs.agent">Must set profiler agent JVM arguments in profiler.info.jvmargs.agent</fail>
+    </target>
+    <!--
+    =================
+    PROFILING SECTION
+    =================
+    -->
+    <target name="profile" if="netbeans.home" depends="profile-init,compile" description="Profile a project in the IDE.">
+        <nbprofiledirect>
+            <classpath>
+                <path path="${run.classpath}"/>
+            </classpath>
+        </nbprofiledirect>
+        <profile/>
+    </target>
+    <target name="profile-single" if="netbeans.home" depends="profile-init,compile-single" description="Profile a selected class in the IDE.">
+        <fail unless="profile.class">Must select one file in the IDE or set profile.class</fail>
+        <nbprofiledirect>
+            <classpath>
+                <path path="${run.classpath}"/>
+            </classpath>
+        </nbprofiledirect>
+        <profile classname="${profile.class}"/>
+    </target>
+    <!--
+    =========================
+    APPLET PROFILING  SECTION
+    =========================
+    -->
+    <target name="profile-applet" if="netbeans.home" depends="profile-init,compile-single">
+        <nbprofiledirect>
+            <classpath>
+                <path path="${run.classpath}"/>
+            </classpath>
+        </nbprofiledirect>
+        <profile classname="sun.applet.AppletViewer">
+            <customize>
+                <arg value="${applet.url}"/>
+            </customize>
+        </profile>
+    </target>
+    <!--
+    =========================
+    TESTS PROFILING  SECTION
+    =========================
+    -->
+    <target name="profile-test-single" if="netbeans.home" depends="profile-init,compile-test-single">
+        <nbprofiledirect>
+            <classpath>
+                <path path="${run.test.classpath}"/>
+            </classpath>
+        </nbprofiledirect>
+        <junit showoutput="true" fork="true" dir="${profiler.info.dir}" jvm="${profiler.info.jvm}" failureproperty="tests.failed" errorproperty="tests.failed">
+            <env key="${profiler.info.pathvar}" path="${profiler.info.agentpath}:${profiler.current.path}"/>
+            <jvmarg value="${profiler.info.jvmargs.agent}"/>
+            <jvmarg line="${profiler.info.jvmargs}"/>
+            <test name="${profile.class}"/>
+            <classpath>
+                <path path="${run.test.classpath}"/>
+            </classpath>
+            <syspropertyset>
+                <propertyref prefix="test-sys-prop."/>
+                <mapper type="glob" from="test-sys-prop.*" to="*"/>
+            </syspropertyset>
+            <formatter type="brief" usefile="false"/>
+            <formatter type="xml"/>
+        </junit>
+    </target>
+</project>
diff --git a/src/jbullet/nbproject/project.properties b/src/jbullet/nbproject/project.properties
new file mode 100644
index 0000000..660b95a
--- /dev/null
+++ b/src/jbullet/nbproject/project.properties
@@ -0,0 +1,67 @@
+application.title=javabullet
+application.vendor=jezek2
+build.classes.dir=${build.dir}/classes
+build.classes.excludes=**/*.java,**/*.form
+# This directory is removed when the project is cleaned:
+build.dir=build
+build.generated.dir=${build.dir}/generated
+# Only compile against the classpath explicitly listed here:
+build.sysclasspath=ignore
+build.test.classes.dir=${build.dir}/test/classes
+build.test.results.dir=${build.dir}/test/results
+debug.classpath=\
+    ${run.classpath}
+debug.test.classpath=\
+    ${run.test.classpath}
+# This directory is removed when the project is cleaned:
+dist.dir=dist
+dist.jar=${dist.dir}/jbullet.jar
+dist.javadoc.dir=${dist.dir}/javadoc
+excludes=
+includes=**
+jar.compress=true
+javac.classpath=\
+    ${libs.vecmath.classpath}:\
+    ${jogl.home}/build/jogl.core.jar:${jogl.home}/build/jogl.gles1.jar:${jogl.home}/build/jogl.egl.jar:${jogl.home}/build/newt.jar:\
+    ${libs.trove.classpath}
+# Space-separated list of extra javac options
+javac.compilerargs=
+javac.deprecation=false
+javac.source=1.5
+javac.target=1.5
+javac.test.classpath=\
+    ${javac.classpath}:\
+    ${build.classes.dir}
+javadoc.additionalparam=-group "Base library" "javabullet:javabullet.linearmath:javabullet.util" -group "Collision library" "javabullet.collision*" -group "Dynamics library" "javabullet.dynamics*"
+javadoc.author=false
+javadoc.encoding=${source.encoding}
+javadoc.noindex=false
+javadoc.nonavbar=false
+javadoc.notree=false
+javadoc.private=false
+javadoc.splitindex=true
+javadoc.use=true
+javadoc.version=false
+javadoc.windowtitle=JBullet
+jnlp.codebase.type=local
+jnlp.codebase.url=
+jnlp.enabled=false
+jnlp.offline-allowed=false
+jnlp.signed=false
+main.class=javabullet.demos.basic.BasicDemo
+manifest.file=manifest.mf
+meta.inf.dir=${src.dir}/META-INF
+platform.active=default_platform
+run.classpath=\
+    ${javac.classpath}:\
+    ${build.classes.dir}
+# Space-separated list of JVM arguments used when running the project
+# (you may also define separate properties like run-sys-prop.name=value instead of -Dname=value
+# or test-sys-prop.name=value to set system properties for unit tests):
+run.jvmargs=-ea -Djava.library.path=lib/jogl/linux-i586
+run.test.classpath=\
+    ${javac.test.classpath}:\
+    ${build.test.classes.dir}
+source.encoding=UTF-8
+src.dir=src
+test.src.dir=test
diff --git a/src/jbullet/nbproject/project.xml b/src/jbullet/nbproject/project.xml
new file mode 100644
index 0000000..f32ffbd
--- /dev/null
+++ b/src/jbullet/nbproject/project.xml
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project xmlns="http://www.netbeans.org/ns/project/1">
+    <type>org.netbeans.modules.java.j2seproject</type>
+    <configuration>
+        <data xmlns="http://www.netbeans.org/ns/j2se-project/3">
+            <name>jbullet</name>
+            <minimum-ant-version>1.6.5</minimum-ant-version>
+            <source-roots>
+                <root id="src.dir"/>
+            </source-roots>
+            <test-roots>
+                <root id="test.src.dir"/>
+            </test-roots>
+        </data>
+        <libraries xmlns="http://www.netbeans.org/ns/ant-project-libraries/1">
+            <definitions>lib/nblibraries.properties</definitions>
+        </libraries>
+    </configuration>
+</project>
diff --git a/src/jbullet/setenv-jbullet.sh b/src/jbullet/setenv-jbullet.sh
new file mode 100644
index 0000000..2464022
--- /dev/null
+++ b/src/jbullet/setenv-jbullet.sh
@@ -0,0 +1,34 @@
+#! /bin/sh
+
+JOGLPROF=$1
+shift
+
+if [ -z "$JOGLPROF" ] ; then
+    JOGLPROF=JOGL_ES1_MIN
+fi
+THISHOME=`pwd`
+
+cd ../..
+. ./setenv-jogl.sh $JOGLPROF
+
+cd $THISHOME
+
+THISDIR=`pwd`
+
+PATH=$JAVA_HOME:$J2RE_HOME:$PATH
+export PATH
+
+export LIBXCB_ALLOW_SLOPPY_LOCK=1
+
+LIB=$THISDIR/lib
+
+JOGL_HOME=$THISDIR/../../../jogl
+CLASSPATH_TROVE=$LIB/trove.jar
+CLASSPATH_VECM=$LIB/vecmath.jar
+CLASSPATH=$CLASSPATH:$THISDIR/build/classes:$CLASSPATH_TROVE:$CLASSPATH_VECM
+export JOGL_HOME CLASSPATH_TROVE CLASSPATH_VECM CLASSPATH 
+
+#
+# java javabullet.demos.genericjoint.GenericJointDemo
+#
+
diff --git a/src/jbullet/src/javabullet/ArrayPool.java b/src/jbullet/src/javabullet/ArrayPool.java
new file mode 100644
index 0000000..d1793e1
--- /dev/null
+++ b/src/jbullet/src/javabullet/ArrayPool.java
@@ -0,0 +1,149 @@
+/*
+ * 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;
+
+import java.lang.reflect.Array;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.Comparator;
+
+/**
+ * Object pool for arrays.
+ * 
+ * @author jezek2
+ */
+public class ArrayPool<T> {
+
+	private Class componentType;
+	private ArrayList list = new ArrayList();
+	private Comparator comparator;
+	private IntValue key = new IntValue();
+	
+	/**
+	 * Creates object pool.
+	 * 
+	 * @param componentType
+	 */
+	public ArrayPool(Class componentType) {
+		this.componentType = componentType;
+		
+		if (componentType == float.class) {
+			comparator = floatComparator;
+		}
+		else if (!componentType.isPrimitive()) {
+			comparator = objectComparator;
+		}
+		else {
+			throw new UnsupportedOperationException("unsupported type "+componentType);
+		}
+	}
+	
+	@SuppressWarnings("unchecked")
+	private T create(int length) {
+		return (T)Array.newInstance(componentType, length);
+	}
+	
+	/**
+	 * Returns array of exactly the same length as demanded, or create one if not
+	 * present in the pool.
+	 * 
+	 * @param length
+	 * @return array
+	 */
+	@SuppressWarnings("unchecked")
+	public T getFixed(int length) {
+		key.value = length;
+		int index = Collections.binarySearch(list, key, comparator);
+		if (index < 0) {
+			return create(length);
+		}
+		return (T)list.remove(index);
+	}
+
+	/**
+	 * Returns array that has same or greater length, or create one if not present
+	 * in the pool.
+	 * 
+	 * @param length the minimum length required
+	 * @return array
+	 */
+	@SuppressWarnings("unchecked")
+	public T getAtLeast(int length) {
+		key.value = length;
+		int index = Collections.binarySearch(list, key, comparator);
+		if (index < 0) {
+			index = -index - 1;
+			if (index < list.size()) {
+				return (T)list.remove(index);
+			}
+			else {
+				return create(length);
+			}
+		}
+		return (T)list.remove(index);
+	}
+	
+	/**
+	 * Releases array into object pool.
+	 * 
+	 * @param array previously obtained array from this pool
+	 */
+	@SuppressWarnings("unchecked")
+	public void release(T array) {
+		int index = Collections.binarySearch(list, array, comparator);
+		if (index < 0) index = -index - 1;
+		list.add(index, array);
+		
+		// remove references from object arrays:
+		if (comparator == objectComparator) {
+			Object[] objArray = (Object[])array;
+			for (int i=0; i<objArray.length; i++) {
+				objArray[i] = null;
+			}
+		}
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+
+	private static Comparator floatComparator = new Comparator() {
+		public int compare(Object o1, Object o2) {
+			int len1 = (o1 instanceof IntValue)? ((IntValue)o1).value : ((float[])o1).length;
+			int len2 = (o2 instanceof IntValue)? ((IntValue)o2).value : ((float[])o2).length;
+			return len1 > len2? 1 : len1 < len2 ? -1 : 0;
+		}
+	};
+	
+	private static Comparator objectComparator = new Comparator() {
+		public int compare(Object o1, Object o2) {
+			int len1 = (o1 instanceof IntValue)? ((IntValue)o1).value : ((Object[])o1).length;
+			int len2 = (o2 instanceof IntValue)? ((IntValue)o2).value : ((Object[])o2).length;
+			return len1 > len2? 1 : len1 < len2 ? -1 : 0;
+		}
+	};
+	
+	private static class IntValue {
+		public int value;
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/BulletGlobals.java b/src/jbullet/src/javabullet/BulletGlobals.java
new file mode 100644
index 0000000..91a21ac
--- /dev/null
+++ b/src/jbullet/src/javabullet/BulletGlobals.java
@@ -0,0 +1,143 @@
+/*
+ * 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;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.Map.Entry;
+import javax.vecmath.Vector3f;
+
+/**
+ * Bullet's global variables and constants.
+ * 
+ * @author jezek2
+ */
+public class BulletGlobals {
+	
+	public static final boolean DEBUG = true;
+	public static final boolean ENABLE_PROFILE = false;
+	
+	public static final float CONVEX_DISTANCE_MARGIN = 0.04f;
+	public static final float FLT_EPSILON = 1.19209290e-07f;
+	public static final float SIMD_EPSILON = FLT_EPSILON;
+	
+	public static final float SIMD_2_PI = 6.283185307179586232f;
+	public static final float SIMD_PI = SIMD_2_PI * 0.5f;
+	public static final float SIMD_HALF_PI = SIMD_2_PI * 0.25f;
+	public static final float SIMD_RADS_PER_DEG = SIMD_2_PI / 360f;
+	public static final float SIMD_DEGS_PER_RAD = 360f / SIMD_2_PI;
+	public static final float SIMD_INFINITY = Float.MAX_VALUE;
+
+	public static ContactDestroyedCallback gContactDestroyedCallback;
+	public static ContactAddedCallback gContactAddedCallback;
+	public static float gContactBreakingThreshold = 0.02f;
+
+	// RigidBody
+	public static float gDeactivationTime = 2f;
+	public static boolean gDisableDeactivation = false;
+	
+	public static int gTotalContactPoints;
+	
+	// GjkPairDetector
+	// temp globals, to improve GJK/EPA/penetration calculations
+	public static int gNumDeepPenetrationChecks = 0;
+	public static int gNumGjkChecks = 0;
+	
+	public static int gNumAlignedAllocs;
+	public static int gNumAlignedFree;
+	public static int gTotalBytesAlignedAllocs;	
+	
+	public static int gPickingConstraintId = 0;
+	public static final Vector3f gOldPickingPos = new Vector3f();
+	public static float gOldPickingDist = 0.f;
+	
+	public static int gOverlappingPairs = 0;
+	public static int gRemovePairs = 0;
+	public static int gAddedPairs = 0;
+	public static int gFindPairs = 0;
+	
+	public static final Vector3f ZERO_VECTOR3 = new Vector3f(0f, 0f, 0f);
+	
+	private static final List<ProfileBlock> profileStack = new ArrayList<ProfileBlock>();
+	private static final Map<String,Long> profiles = new HashMap<String,Long>();
+
+	// JAVA NOTE: added for statistics in applet demo
+	public static long stepSimulationTime;
+	public static long updateTime;
+	
+	public static void pushProfile(String name) {
+		if (!ENABLE_PROFILE) return;
+		
+		ProfileBlock block = new ProfileBlock();
+		block.name = name;
+		block.startTime = System.currentTimeMillis();
+		profileStack.add(block);
+	}
+	
+	public static void popProfile() {
+		if (!ENABLE_PROFILE) return;
+		
+		ProfileBlock block = profileStack.remove(profileStack.size() - 1);
+		long time = System.currentTimeMillis();
+		
+		Long totalTime = profiles.get(block.name);
+		if (totalTime == null) totalTime = 0L;
+		totalTime += (time - block.startTime);
+		profiles.put(block.name, totalTime);
+	}
+	
+	public static void printProfiles() {
+		ArrayList<Entry<String,Long>> list = new ArrayList<Entry<String,Long>>(profiles.entrySet());
+		Collections.sort(list, new Comparator<Entry<String,Long>>() {
+			public int compare(Entry<String,Long> e1, Entry<String,Long> e2) {
+				return e1.getValue().compareTo(e2.getValue());
+			}
+		});
+		
+		for (Entry<String,Long> e : /*profiles.entrySet()*/list) {
+			System.out.println(e.getKey()+" = "+e.getValue()+" ms");
+		}
+	}
+	
+	static {
+		if (ENABLE_PROFILE) {
+			Runtime.getRuntime().addShutdownHook(new Thread() {
+				@Override
+				public void run() {
+					printProfiles();
+				}
+			});
+		}
+	}
+	
+	private static class ProfileBlock {
+		public String name;
+		public long startTime;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/BulletPool.java b/src/jbullet/src/javabullet/BulletPool.java
new file mode 100644
index 0000000..10d720c
--- /dev/null
+++ b/src/jbullet/src/javabullet/BulletPool.java
@@ -0,0 +1,64 @@
+/*
+ * 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;
+
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Maintains per-thread object pools for different types.
+ * 
+ * @author jezek2
+ */
+public class BulletPool {
+
+	private BulletPool() {}
+	
+	private static ThreadLocal<Map> threadLocal = new ThreadLocal<Map>() {
+		@Override
+		protected Map initialValue() {
+			return new HashMap();
+		}
+	};
+	
+	/**
+	 * Returns per-thread object pool for given type, or create one if it doesn't exist.
+	 * 
+	 * @param cls type
+	 * @return object pool
+	 */
+	@SuppressWarnings("unchecked")
+	public static <T> ObjectPool<T> get(Class<T> cls) {
+		Map map = threadLocal.get();
+		
+		ObjectPool<T> pool = (ObjectPool<T>)map.get(cls);
+		if (pool == null) {
+			pool = new ObjectPool<T>(cls);
+			map.put(cls, pool);
+		}
+		
+		return pool;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/BulletStack.java b/src/jbullet/src/javabullet/BulletStack.java
new file mode 100644
index 0000000..a9152a7
--- /dev/null
+++ b/src/jbullet/src/javabullet/BulletStack.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;
+
+/**
+ * Per-thread stack based object pools for common types.
+ * 
+ * @see StackList
+ * 
+ * @author jezek2
+ */
+public class BulletStack {
+
+	private BulletStack() {}
+	
+	private static final ThreadLocal<BulletStack> threadLocal = new ThreadLocal<BulletStack>() {
+		@Override
+		protected BulletStack initialValue() {
+			return new BulletStack();
+		}
+	};
+	
+	/**
+	 * Returns stack for current thread, or create one if not present.
+	 * 
+	 * @return stack
+	 */
+	public static BulletStack get() {
+		return threadLocal.get();
+	}
+	
+	// common math:
+	public final VectorStackList vectors = new VectorStackList();
+	public final TransformStackList transforms = new TransformStackList();
+	public final MatrixStackList matrices = new MatrixStackList();
+	
+	// others:
+	public final Vector4StackList vectors4 = new Vector4StackList();
+	public final QuatStackList quats = new QuatStackList();
+
+	public final ArrayPool<float[]> floatArrays = new ArrayPool<float[]>(float.class);
+	
+	/**
+	 * Pushes Vector3f, Transform and Matrix3f stacks.
+	 */
+	public void pushCommonMath() {
+		vectors.push();
+		transforms.push();
+		matrices.push();
+	}
+	
+	/**
+	 * Pops Vector3f, Transform and Matrix3f stacks.
+	 */
+	public void popCommonMath() {
+		vectors.pop();
+		transforms.pop();
+		matrices.pop();
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/ContactAddedCallback.java b/src/jbullet/src/javabullet/ContactAddedCallback.java
new file mode 100644
index 0000000..3745939
--- /dev/null
+++ b/src/jbullet/src/javabullet/ContactAddedCallback.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;
+
+import javabullet.collision.dispatch.CollisionObject;
+import javabullet.collision.narrowphase.ManifoldPoint;
+
+/**
+ *
+ * @author jezek2
+ */
+public interface ContactAddedCallback {
+
+	public boolean invoke(ManifoldPoint cp, CollisionObject colObj0, int partId0, int index0, CollisionObject colObj1, int partId1, int index1);
+	
+}
diff --git a/src/jbullet/src/javabullet/ContactDestroyedCallback.java b/src/jbullet/src/javabullet/ContactDestroyedCallback.java
new file mode 100644
index 0000000..8b8498e
--- /dev/null
+++ b/src/jbullet/src/javabullet/ContactDestroyedCallback.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;
+
+/**
+ *
+ * @author jezek2
+ */
+public interface ContactDestroyedCallback {
+
+	public boolean invoke(Object userPersistentData);
+	
+}
diff --git a/src/jbullet/src/javabullet/MatrixStackList.java b/src/jbullet/src/javabullet/MatrixStackList.java
new file mode 100644
index 0000000..c592095
--- /dev/null
+++ b/src/jbullet/src/javabullet/MatrixStackList.java
@@ -0,0 +1,51 @@
+/*
+ * 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;
+
+import javax.vecmath.Matrix3f;
+
+/**
+ * Stack-based object pool for {@link Matrix3f}.
+ * 
+ * @author jezek2
+ */
+public class MatrixStackList extends StackList<Matrix3f> {
+
+	public Matrix3f get(Matrix3f mat) {
+		Matrix3f obj = get();
+		obj.set(mat);
+		return obj;
+	}
+	
+	@Override
+	protected Matrix3f create() {
+		return new Matrix3f();
+	}
+
+	@Override
+	protected void copy(Matrix3f dest, Matrix3f src) {
+		dest.set(src);
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/ObjectPool.java b/src/jbullet/src/javabullet/ObjectPool.java
new file mode 100644
index 0000000..00092db
--- /dev/null
+++ b/src/jbullet/src/javabullet/ObjectPool.java
@@ -0,0 +1,77 @@
+/*
+ * 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;
+
+import java.util.ArrayList;
+
+/**
+ * Object pool.
+ * 
+ * @author jezek2
+ */
+public class ObjectPool<T> {
+	
+	private Class<T> cls;
+	private ArrayList<T> list = new ArrayList<T>();
+	
+	public ObjectPool(Class<T> cls) {
+		this.cls = cls;
+	}
+
+	private T create() {
+		try {
+			return cls.newInstance();
+		}
+		catch (InstantiationException e) {
+			throw new IllegalStateException(e);
+		}
+		catch (IllegalAccessException e) {
+			throw new IllegalStateException(e);
+		}
+	}
+	
+	/**
+	 * Returns instance from pool, or create one if pool is empty.
+	 * 
+	 * @return instance
+	 */
+	public T get() {
+		if (list.size() > 0) {
+			return list.remove(list.size() - 1);
+		}
+		else {
+			return create();
+		}
+	}
+	
+	/**
+	 * Release instance into pool.
+	 * 
+	 * @param obj previously obtained instance from pool
+	 */
+	public void release(T obj) {
+		list.add(obj);
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/ObjectStackList.java b/src/jbullet/src/javabullet/ObjectStackList.java
new file mode 100644
index 0000000..2972218
--- /dev/null
+++ b/src/jbullet/src/javabullet/ObjectStackList.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;
+
+/**
+ * Stack-based object pool for arbitrary objects, returning not supported.
+ * 
+ * @author jezek2
+ */
+public class ObjectStackList<T> extends StackList<T> {
+
+	private Class<T> cls;
+	
+	public ObjectStackList(Class<T> cls) {
+		super(false);
+		this.cls = cls;
+	}
+
+	@Override
+	protected T create() {
+		try {
+			return cls.newInstance();
+		}
+		catch (InstantiationException e) {
+			throw new IllegalStateException(e);
+		}
+		catch (IllegalAccessException e) {
+			throw new IllegalStateException(e);
+		}
+	}
+
+	@Override
+	protected void copy(T dest, T src) {
+		throw new UnsupportedOperationException();
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/QuatStackList.java b/src/jbullet/src/javabullet/QuatStackList.java
new file mode 100644
index 0000000..053798e
--- /dev/null
+++ b/src/jbullet/src/javabullet/QuatStackList.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;
+
+import javax.vecmath.Quat4f;
+
+/**
+ * Stack-based object pool for {@link Quat4f}.
+ * 
+ * @author jezek2
+ */
+public class QuatStackList extends StackList<Quat4f> {
+
+	public Quat4f get(float x, float y, float z, float w) {
+		Quat4f v = get();
+		v.set(x, y, z, w);
+		return v;
+	}
+
+	public Quat4f get(Quat4f quat) {
+		Quat4f obj = get();
+		obj.set(quat);
+		return obj;
+	}
+
+	@Override
+	protected Quat4f create() {
+		return new Quat4f();
+	}
+
+	@Override
+	protected void copy(Quat4f dest, Quat4f src) {
+		dest.set(src);
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/StackList.java b/src/jbullet/src/javabullet/StackList.java
new file mode 100644
index 0000000..a693ea8
--- /dev/null
+++ b/src/jbullet/src/javabullet/StackList.java
@@ -0,0 +1,136 @@
+/*
+ * 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;
+
+import java.util.ArrayList;
+
+/**
+ * Stack-based object pool, see the example for usage. You must use the {@link #returning}
+ * method for returning stack-allocated instance.<p>
+ * 
+ * Example code:
+ * 
+ * <pre>
+ * StackList&lt;Vector3f&gt; vectors;
+ * ...
+ * 
+ * vectors.push();
+ * try {
+ *     Vector3f vec = vectors.get();
+ *     ...
+ *     return vectors.returning(vec);
+ * }
+ * finally {
+ *     vectors.pop();
+ * }
+ * </pre>
+ * 
+ * @author jezek2
+ */
+public abstract class StackList<T> {
+
+	private final ArrayList<T> list = new ArrayList<T>();
+	private T returnObj;
+	
+	private int[] stack = new int[512];
+	private int stackCount = 0;
+	
+	private int pos = 0;
+	
+	public StackList() {
+		returnObj = create();
+	}
+	
+	protected StackList(boolean unused) {
+	}
+	
+	/**
+	 * Pushes the stack.
+	 */
+	public final void push() {
+		/*if (stackCount == stack.length-1) {
+			resizeStack();
+		}*/
+		
+		stack[stackCount++] = pos;
+	}
+
+	/**
+	 * Pops the stack.
+	 */
+	public final void pop() {
+		pos = stack[--stackCount];
+	}
+	
+	/**
+	 * Returns instance from stack pool, or create one if not present. The returned
+	 * instance will be automatically reused when {@link #pop} is called.
+	 * 
+	 * @return instance
+	 */
+	public T get() {
+		//if (true) return create();
+		
+		if (pos == list.size()) {
+			expand();
+		}
+		
+		return list.get(pos++);
+	}
+	
+	/**
+	 * Copies given instance into one slot static instance and returns it. It's
+	 * essential that caller of method (that uses this method for returning instances)
+	 * immediately copies it into own variable before any other usage.
+	 * 
+	 * @param obj stack-allocated instance
+	 * @return one slot instance for returning purposes
+	 */
+	public final T returning(T obj) {
+		//if (true) { T ret = create(); copy(ret, obj); return ret; }
+		
+		copy(returnObj, obj);
+		return returnObj;
+	}
+	
+	/**
+	 * Creates a new instance of type.
+	 * 
+	 * @return instance
+	 */
+	protected abstract T create();
+	
+	/**
+	 * Copies data from one instance to another.
+	 * 
+	 * @param dest
+	 * @param src
+	 */
+	protected abstract void copy(T dest, T src);
+
+	private void expand() {
+		list.add(create());
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/TransformStackList.java b/src/jbullet/src/javabullet/TransformStackList.java
new file mode 100644
index 0000000..085f66d
--- /dev/null
+++ b/src/jbullet/src/javabullet/TransformStackList.java
@@ -0,0 +1,59 @@
+/*
+ * 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;
+
+import javabullet.linearmath.Transform;
+import javax.vecmath.Matrix3f;
+
+/**
+ * Stack-based object pool for {@link Transform}.
+ * 
+ * @author jezek2
+ */
+public class TransformStackList extends StackList<Transform> {
+	
+	public Transform get(Transform tr) {
+		Transform obj = get();
+		obj.set(tr);
+		return obj;
+	}
+
+	public Transform get(Matrix3f mat) {
+		Transform obj = get();
+		obj.basis.set(mat);
+		obj.origin.set(0f, 0f, 0f);
+		return obj;
+	}
+
+	@Override
+	protected Transform create() {
+		return new Transform();
+	}
+	
+	@Override
+	protected void copy(Transform dest, Transform src) {
+		dest.set(src);
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/Vector4StackList.java b/src/jbullet/src/javabullet/Vector4StackList.java
new file mode 100644
index 0000000..056c05e
--- /dev/null
+++ b/src/jbullet/src/javabullet/Vector4StackList.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;
+
+import javax.vecmath.Vector4f;
+
+/**
+ * Stack-based object pool for {@link Vector4f}.
+ * 
+ * @author jezek2
+ */
+public class Vector4StackList extends StackList<Vector4f> {
+
+	public Vector4f get(float x, float y, float z, float w) {
+		Vector4f v = get();
+		v.set(x, y, z, w);
+		return v;
+	}
+
+	public Vector4f get(Vector4f vec) {
+		Vector4f v = get();
+		v.set(vec);
+		return v;
+	}
+
+	@Override
+	protected Vector4f create() {
+		return new Vector4f();
+	}
+
+	@Override
+	protected void copy(Vector4f dest, Vector4f src) {
+		dest.set(src);
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/VectorStackList.java b/src/jbullet/src/javabullet/VectorStackList.java
new file mode 100644
index 0000000..6882932
--- /dev/null
+++ b/src/jbullet/src/javabullet/VectorStackList.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;
+
+import javax.vecmath.Vector3f;
+
+/**
+ * Stack-based object pool for {@link Vector3f}.
+ * 
+ * @author jezek2
+ */
+public class VectorStackList extends StackList<Vector3f> {
+
+	public Vector3f get(float x, float y, float z) {
+		Vector3f v = get();
+		v.set(x, y, z);
+		return v;
+	}
+
+	public Vector3f get(Vector3f vec) {
+		Vector3f v = get();
+		v.set(vec);
+		return v;
+	}
+
+	@Override
+	protected Vector3f create() {
+		return new Vector3f();
+	}
+
+	@Override
+	protected void copy(Vector3f dest, Vector3f src) {
+		dest.set(src);
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/BroadphaseInterface.java b/src/jbullet/src/javabullet/collision/broadphase/BroadphaseInterface.java
new file mode 100644
index 0000000..131bb56
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/BroadphaseInterface.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.collision.broadphase;
+
+import javax.vecmath.Vector3f;
+
+/**
+ * BroadphaseInterface for AABB overlapping object pairs.
+ * 
+ * @author jezek2
+ */
+public interface BroadphaseInterface {
+
+	public BroadphaseProxy createProxy(Vector3f aabbMin, Vector3f aabbMax, BroadphaseNativeType shapeType, Object userPtr, short collisionFilterGroup, short collisionFilterMask, Dispatcher dispatcher);
+
+	public void destroyProxy(BroadphaseProxy proxy, Dispatcher dispatcher);
+
+	public void setAabb(BroadphaseProxy proxy, Vector3f aabbMin, Vector3f aabbMax, Dispatcher dispatcher);
+
+	///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb
+	public void calculateOverlappingPairs(Dispatcher dispatcher);
+
+	public OverlappingPairCache getOverlappingPairCache();
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/BroadphaseNativeType.java b/src/jbullet/src/javabullet/collision/broadphase/BroadphaseNativeType.java
new file mode 100644
index 0000000..8d066fe
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/BroadphaseNativeType.java
@@ -0,0 +1,102 @@
+/*
+ * 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.collision.broadphase;
+
+/**
+ * Dispatcher uses these types.<p>
+ * 
+ * IMPORTANT NOTE: The types are ordered polyhedral, implicit convex and concave
+ * to facilitate type checking.
+ * 
+ * @author jezek2
+ */
+public enum BroadphaseNativeType {
+	
+	// polyhedral convex shapes:
+	BOX_SHAPE_PROXYTYPE,
+	TRIANGLE_SHAPE_PROXYTYPE,
+	TETRAHEDRAL_SHAPE_PROXYTYPE,
+	CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE,
+	CONVEX_HULL_SHAPE_PROXYTYPE,
+	
+	// implicit convex shapes:
+	IMPLICIT_CONVEX_SHAPES_START_HERE,
+	SPHERE_SHAPE_PROXYTYPE,
+	MULTI_SPHERE_SHAPE_PROXYTYPE,
+	CAPSULE_SHAPE_PROXYTYPE,
+	CONE_SHAPE_PROXYTYPE,
+	CONVEX_SHAPE_PROXYTYPE,
+	CYLINDER_SHAPE_PROXYTYPE,
+	UNIFORM_SCALING_SHAPE_PROXYTYPE,
+	MINKOWSKI_SUM_SHAPE_PROXYTYPE,
+	MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE,
+	
+	// concave shapes:
+	CONCAVE_SHAPES_START_HERE,
+	
+	// keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
+	TRIANGLE_MESH_SHAPE_PROXYTYPE,
+	
+	// used for demo integration FAST/Swift collision library and Bullet:
+	FAST_CONCAVE_MESH_PROXYTYPE,
+	
+	// terrain:
+	TERRAIN_SHAPE_PROXYTYPE,
+	
+	// used for GIMPACT Trimesh integration:
+	GIMPACT_SHAPE_PROXYTYPE,
+	EMPTY_SHAPE_PROXYTYPE,
+	STATIC_PLANE_PROXYTYPE,
+	CONCAVE_SHAPES_END_HERE,
+	COMPOUND_SHAPE_PROXYTYPE,
+	MAX_BROADPHASE_COLLISION_TYPES;
+	
+	private static BroadphaseNativeType[] values = values();
+	
+	public static BroadphaseNativeType forValue(int value) {
+		return values[value];
+	}
+	
+	public boolean isPolyhedral() {
+		return (ordinal() < IMPLICIT_CONVEX_SHAPES_START_HERE.ordinal());
+	}
+
+	public boolean isConvex() {
+		return (ordinal() < CONCAVE_SHAPES_START_HERE.ordinal());
+	}
+
+	public boolean isConcave() {
+		return ((ordinal() > CONCAVE_SHAPES_START_HERE.ordinal()) &&
+				(ordinal() < CONCAVE_SHAPES_END_HERE.ordinal()));
+	}
+
+	public boolean isCompound() {
+		return (ordinal() == COMPOUND_SHAPE_PROXYTYPE.ordinal());
+	}
+
+	public boolean isInfinite() {
+		return (ordinal() == STATIC_PLANE_PROXYTYPE.ordinal());
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/BroadphasePair.java b/src/jbullet/src/javabullet/collision/broadphase/BroadphasePair.java
new file mode 100644
index 0000000..c94c269
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/BroadphasePair.java
@@ -0,0 +1,66 @@
+/*
+ * 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.collision.broadphase;
+
+/**
+ *
+ * @author jezek2
+ */
+public class BroadphasePair {
+
+	public BroadphaseProxy pProxy0;
+	public BroadphaseProxy pProxy1;
+	public CollisionAlgorithm algorithm;
+	public Object userInfo;
+
+	public BroadphasePair() {
+	}
+
+	public void set(BroadphaseProxy pProxy0, BroadphaseProxy pProxy1) {
+		this.pProxy0 = pProxy0;
+		this.pProxy1 = pProxy1;
+		this.algorithm = null;
+		this.userInfo = null;
+	}
+	
+	public void set(BroadphasePair p) {
+		pProxy0 = p.pProxy0;
+		pProxy1 = p.pProxy1;
+		algorithm = p.algorithm;
+		userInfo = p.userInfo;
+	}
+
+	@Override
+	public boolean equals(Object obj) {
+		if (obj == null || !(obj instanceof BroadphasePair)) return false;
+		BroadphasePair k = (BroadphasePair)obj;
+		return (pProxy0 == k.pProxy0 && pProxy1 == k.pProxy1) || (pProxy0 == k.pProxy1 && pProxy1 == k.pProxy0);
+	}
+
+	@Override
+	public int hashCode() {
+		return pProxy0.hashCode() ^ pProxy1.hashCode();
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/BroadphaseProxy.java b/src/jbullet/src/javabullet/collision/broadphase/BroadphaseProxy.java
new file mode 100644
index 0000000..d98425d
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/BroadphaseProxy.java
@@ -0,0 +1,54 @@
+/*
+ * 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.collision.broadphase;
+
+/**
+ *
+ * @author jezek2
+ */
+public class BroadphaseProxy {
+
+	// Usually the client CollisionObject or Rigidbody class
+	public Object clientObject;
+	
+	// TODO: mask
+	public short collisionFilterGroup;
+	public short collisionFilterMask;
+	
+	public int uniqueId; // uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
+
+	public BroadphaseProxy() {
+	}
+	
+	public BroadphaseProxy(Object userPtr, short collisionFilterGroup, short collisionFilterMask) {
+		this.clientObject = userPtr;
+		this.collisionFilterGroup = collisionFilterGroup;
+		this.collisionFilterMask = collisionFilterMask;
+	}
+
+	public int getUid() {
+		return uniqueId;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/CollisionAlgorithm.java b/src/jbullet/src/javabullet/collision/broadphase/CollisionAlgorithm.java
new file mode 100644
index 0000000..8f6bc1a
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/CollisionAlgorithm.java
@@ -0,0 +1,53 @@
+/*
+ * 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.collision.broadphase;
+
+import javabullet.BulletStack;
+import javabullet.collision.dispatch.CollisionObject;
+import javabullet.collision.dispatch.ManifoldResult;
+
+/**
+ *
+ * @author jezek2
+ */
+public abstract class CollisionAlgorithm {
+
+	protected final BulletStack stack = BulletStack.get();
+	
+	protected Dispatcher dispatcher;
+
+	public CollisionAlgorithm() {
+	}
+
+	public CollisionAlgorithm(CollisionAlgorithmConstructionInfo ci) {
+		dispatcher = ci.dispatcher1;
+	}
+	
+	public abstract void destroy();
+
+	public abstract void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
+
+	public abstract float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/CollisionAlgorithmConstructionInfo.java b/src/jbullet/src/javabullet/collision/broadphase/CollisionAlgorithmConstructionInfo.java
new file mode 100644
index 0000000..94f977a
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/CollisionAlgorithmConstructionInfo.java
@@ -0,0 +1,39 @@
+/*
+ * 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.collision.broadphase;
+
+import javabullet.collision.narrowphase.PersistentManifold;
+
+/**
+ *
+ * @author jezek2
+ */
+public class CollisionAlgorithmConstructionInfo {
+
+	public Dispatcher dispatcher1;
+	public PersistentManifold manifold;
+
+	//public int getDispatcherId();
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/CollisionFilterGroups.java b/src/jbullet/src/javabullet/collision/broadphase/CollisionFilterGroups.java
new file mode 100644
index 0000000..49ee387
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/CollisionFilterGroups.java
@@ -0,0 +1,39 @@
+/*
+ * 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.collision.broadphase;
+
+/**
+ *
+ * @author jezek2
+ */
+public class CollisionFilterGroups {
+
+	public static final int DEFAULT_FILTER   = 1;
+	public static final int STATIC_FILTER    = 2;
+	public static final int KINEMATIC_FILTER = 4;
+	public static final int DEBRIS_FILTER    = 8;
+	public static final int SENSOR_TRIGGER   = 16;
+	public static final int ALL_FILTER       = DEFAULT_FILTER | STATIC_FILTER | KINEMATIC_FILTER | DEBRIS_FILTER | SENSOR_TRIGGER;
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/DispatchFunc.java b/src/jbullet/src/javabullet/collision/broadphase/DispatchFunc.java
new file mode 100644
index 0000000..7d8120f
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/DispatchFunc.java
@@ -0,0 +1,45 @@
+/*
+ * 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.collision.broadphase;
+
+/**
+ *
+ * @author jezek2
+ */
+public enum DispatchFunc {
+
+	DISPATCH_DISCRETE(1),
+	DISPATCH_CONTINUOUS(2);
+	
+	private int value;
+	
+	private DispatchFunc(int value) {
+		this.value = value;
+	}
+
+	public int getValue() {
+		return value;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/Dispatcher.java b/src/jbullet/src/javabullet/collision/broadphase/Dispatcher.java
new file mode 100644
index 0000000..553f07d
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/Dispatcher.java
@@ -0,0 +1,66 @@
+/*
+ * 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.collision.broadphase;
+
+import java.util.List;
+import javabullet.collision.dispatch.CollisionObject;
+import javabullet.collision.narrowphase.PersistentManifold;
+
+/**
+ * Dispatcher can be used in combination with broadphase to dispatch overlapping pairs.
+ * For example for pairwise collision detection or user callbacks (game logic).
+ * 
+ * @author jezek2
+ */
+public abstract class Dispatcher {
+
+	public final CollisionAlgorithm findAlgorithm(CollisionObject body0, CollisionObject body1) {
+		return findAlgorithm(body0, body1, null);
+	}
+
+	public abstract CollisionAlgorithm findAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold);
+
+	public abstract PersistentManifold getNewManifold(Object body0, Object body1);
+
+	public abstract void releaseManifold(PersistentManifold manifold);
+
+	public abstract void clearManifold(PersistentManifold manifold);
+
+	public abstract boolean needsCollision(CollisionObject body0, CollisionObject body1);
+
+	public abstract boolean needsResponse(CollisionObject body0, CollisionObject body1);
+
+	public abstract void dispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo, Dispatcher dispatcher);
+
+	public abstract int getNumManifolds();
+
+	public abstract PersistentManifold getManifoldByIndexInternal(int index);
+
+	public abstract List<PersistentManifold> getInternalManifoldPointer();
+
+	//public abstract Object allocateCollisionAlgorithm(int size);
+
+	//public abstract void freeCollisionAlgorithm(Object ptr);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/DispatcherInfo.java b/src/jbullet/src/javabullet/collision/broadphase/DispatcherInfo.java
new file mode 100644
index 0000000..f37326a
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/DispatcherInfo.java
@@ -0,0 +1,49 @@
+/*
+ * 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.collision.broadphase;
+
+import javabullet.linearmath.IDebugDraw;
+
+/**
+ *
+ * @author jezek2
+ */
+public class DispatcherInfo {
+
+	public float timeStep;
+	public int stepCount;
+	public DispatchFunc dispatchFunc;
+	public float timeOfImpact;
+	public boolean useContinuous;
+	public IDebugDraw debugDraw;
+	public boolean enableSatConvex;
+	public boolean enableSPU;
+	//btStackAlloc*	m_stackAllocator;
+
+	public DispatcherInfo() {
+		dispatchFunc = DispatchFunc.DISPATCH_DISCRETE;
+		timeOfImpact = 1f;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/OverlapCallback.java b/src/jbullet/src/javabullet/collision/broadphase/OverlapCallback.java
new file mode 100644
index 0000000..73e0208
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/OverlapCallback.java
@@ -0,0 +1,35 @@
+/*
+ * 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.collision.broadphase;
+
+/**
+ *
+ * @author jezek2
+ */
+public interface OverlapCallback {
+
+	//return true for deletion of the pair
+	public boolean processOverlap(BroadphasePair pair);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/OverlapFilterCallback.java b/src/jbullet/src/javabullet/collision/broadphase/OverlapFilterCallback.java
new file mode 100644
index 0000000..d288924
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/OverlapFilterCallback.java
@@ -0,0 +1,35 @@
+/*
+ * 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.collision.broadphase;
+
+/**
+ *
+ * @author jezek2
+ */
+public interface OverlapFilterCallback {
+
+	// return true when pairs need collision
+	public boolean needBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/OverlappingPairCache.java b/src/jbullet/src/javabullet/collision/broadphase/OverlappingPairCache.java
new file mode 100644
index 0000000..dcfbaca
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/OverlappingPairCache.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.collision.broadphase;
+
+import java.util.Collection;
+import java.util.Iterator;
+import javabullet.BulletGlobals;
+import javabullet.BulletPool;
+import javabullet.ObjectPool;
+import javabullet.util.HashUtil;
+import javabullet.util.HashUtil.IMap;
+import javabullet.util.HashUtil.IObjectProcedure;
+
+/**
+ *
+ * @author jezek2
+ */
+public class OverlappingPairCache {
+
+	private final ObjectPool<BroadphasePair> pairsPool = BulletPool.get(BroadphasePair.class);
+	
+	private final IMap<BroadphasePair,BroadphasePair> overlappingPairs = HashUtil.createMap();
+	private OverlapFilterCallback overlapFilterCallback;
+
+	public OverlappingPairCache() {
+	}
+
+	/**
+	 * Add a pair and return the new pair. If the pair already exists,
+	 * no new pair is created and the old one is returned.
+	 */
+	public BroadphasePair addOverlappingPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1) {
+		BulletGlobals.gAddedPairs++;
+
+		if (!needsBroadphaseCollision(proxy0, proxy1)) {
+			return null;
+		}
+
+		BroadphasePair pair = pairsPool.get();
+		pair.set(proxy0, proxy1);
+		
+		BroadphasePair old = overlappingPairs.get(pair);
+		if (old != null) {
+			pairsPool.release(pair);
+			return old;
+		}
+		overlappingPairs.put(pair, pair);
+		return pair;
+	}
+
+	public Object removeOverlappingPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1, Dispatcher dispatcher) {
+		BulletGlobals.gRemovePairs++;
+
+		BroadphasePair key = pairsPool.get();
+		key.set(proxy0, proxy1);
+		BroadphasePair pair = overlappingPairs.remove(key);
+		pairsPool.release(key);
+		
+		if (pair == null) {
+			return null;
+		}
+
+		cleanOverlappingPair(pair, dispatcher);
+		pairsPool.release(pair);
+
+		return pair.userInfo;
+	}
+
+	public boolean needsBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1) {
+		if (overlapFilterCallback != null) {
+			return overlapFilterCallback.needBroadphaseCollision(proxy0, proxy1);
+		}
+
+		boolean collides = (proxy0.collisionFilterGroup & proxy1.collisionFilterMask) != 0;
+		collides = collides && (proxy1.collisionFilterGroup & proxy0.collisionFilterMask) != 0;
+
+		return collides;
+	}
+
+	private class ProcessAllOverlappingPairsCallback implements IObjectProcedure<BroadphasePair> {
+		public OverlapCallback callback;
+		public Dispatcher dispatcher;
+		
+		public boolean execute(BroadphasePair pair) {
+			if (callback.processOverlap(pair)) {
+				//removeOverlappingPair(pair.pProxy0, pair.pProxy1, dispatcher);
+				cleanOverlappingPair(pair, dispatcher);
+				BulletGlobals.gRemovePairs++;
+				BulletGlobals.gOverlappingPairs--;
+				pairsPool.release(pair);
+				return false;
+			}
+			return true;
+		}
+	}
+	
+	private ProcessAllOverlappingPairsCallback processAllOverlappingPairsCallback = new ProcessAllOverlappingPairsCallback();
+	
+	public void processAllOverlappingPairs(OverlapCallback callback, Dispatcher dispatcher) {
+		processAllOverlappingPairsCallback.callback = callback;
+		processAllOverlappingPairsCallback.dispatcher = dispatcher;
+		overlappingPairs.retainEntries(processAllOverlappingPairsCallback);
+	}
+
+	public void removeOverlappingPairsContainingProxy(BroadphaseProxy proxy, Dispatcher dispatcher) {
+		processAllOverlappingPairs(new RemovePairCallback(proxy), dispatcher);
+	}
+
+	public void cleanProxyFromPairs(BroadphaseProxy proxy, Dispatcher dispatcher) {
+		processAllOverlappingPairs(new CleanPairCallback(proxy, this, dispatcher), dispatcher);
+	}
+
+	public IMap<BroadphasePair,BroadphasePair> getOverlappingPairArray() {
+		return overlappingPairs;
+	}
+
+	public void cleanOverlappingPair(BroadphasePair pair, Dispatcher dispatcher) {
+		if (pair.algorithm != null) {
+			pair.algorithm.destroy();
+			// TODO: dispatcher.freeCollisionAlgorithm(pair.m_algorithm);
+			pair.algorithm = null;
+		}
+	}
+
+	public BroadphasePair findPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1) {
+		BulletGlobals.gFindPairs++;
+
+		BroadphasePair key = pairsPool.get();
+		key.set(proxy0, proxy1);
+		BroadphasePair value = overlappingPairs.get(key);
+		pairsPool.release(key);
+		return value;
+	}
+
+	public int getCount() {
+		return overlappingPairs.size();
+	}
+
+//	btBroadphasePair* GetPairs() { return m_pairs; }
+	public OverlapFilterCallback getOverlapFilterCallback() {
+		return overlapFilterCallback;
+	}
+
+	public void setOverlapFilterCallback(OverlapFilterCallback overlapFilterCallback) {
+		this.overlapFilterCallback = overlapFilterCallback;
+	}
+
+	public int getNumOverlappingPairs() {
+		return overlappingPairs.size();
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+
+	private static class RemovePairCallback implements OverlapCallback {
+		private BroadphaseProxy obsoleteProxy;
+
+		public RemovePairCallback(BroadphaseProxy obsoleteProxy) {
+			this.obsoleteProxy = obsoleteProxy;
+		}
+
+		public boolean processOverlap(BroadphasePair pair) {
+			return ((pair.pProxy0 == obsoleteProxy) ||
+					(pair.pProxy1 == obsoleteProxy));
+		}
+	}
+
+	private static class CleanPairCallback implements OverlapCallback {
+		private BroadphaseProxy cleanProxy;
+		private OverlappingPairCache pairCache;
+		private Dispatcher dispatcher;
+
+		public CleanPairCallback(BroadphaseProxy cleanProxy, OverlappingPairCache pairCache, Dispatcher dispatcher) {
+			this.cleanProxy = cleanProxy;
+			this.pairCache = pairCache;
+			this.dispatcher = dispatcher;
+		}
+
+		public boolean processOverlap(BroadphasePair pair) {
+			if ((pair.pProxy0 == cleanProxy) ||
+					(pair.pProxy1 == cleanProxy)) {
+				pairCache.cleanOverlappingPair(pair, dispatcher);
+			}
+			return false;
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/OverlappingPairCallback.java b/src/jbullet/src/javabullet/collision/broadphase/OverlappingPairCallback.java
new file mode 100644
index 0000000..9a832ab
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/OverlappingPairCallback.java
@@ -0,0 +1,39 @@
+/*
+ * 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.collision.broadphase;
+
+/**
+ * OverlappingPairCallback provides user callback to keep track of overlap between objects, like a collision sensor.
+ * 
+ * @author jezek2
+ */
+public interface OverlappingPairCallback {
+
+	public void addOverlappingPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1);
+
+	public void removeOverlappingPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1);
+
+	public void removeOverlappingPairsContainingProxy(BroadphaseProxy proxy0);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/SimpleBroadphase.java b/src/jbullet/src/javabullet/collision/broadphase/SimpleBroadphase.java
new file mode 100644
index 0000000..be688d9
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/SimpleBroadphase.java
@@ -0,0 +1,112 @@
+/*
+ * 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.collision.broadphase;
+
+import java.util.ArrayList;
+import java.util.List;
+import javax.vecmath.Vector3f;
+
+/**
+ * SimpleBroadphase is a brute force aabb culling broadphase based on O(n^2) aabb checks.
+ * SimpleBroadphase is just a unit-test implementation to verify and test other broadphases.
+ * So please don't use this class, but use bt32BitAxisSweep3 or btAxisSweep3 instead!
+ * 
+ * @author jezek2
+ */
+public class SimpleBroadphase implements BroadphaseInterface {
+
+	private final List<SimpleBroadphaseProxy> handles = new ArrayList<SimpleBroadphaseProxy>();
+	private int maxHandles;						// max number of handles
+	private OverlappingPairCache pairCache;
+	private boolean ownsPairCache;
+
+	public SimpleBroadphase() {
+		this(16384, null);
+	}
+
+	public SimpleBroadphase(int maxProxies) {
+		this(maxProxies, null);
+	}
+	
+	public SimpleBroadphase(int maxProxies, OverlappingPairCache overlappingPairCache) {
+		this.pairCache = overlappingPairCache;
+
+		if (overlappingPairCache == null) {
+			pairCache = new OverlappingPairCache();
+			ownsPairCache = true;
+		}
+	}
+
+	public BroadphaseProxy createProxy(Vector3f aabbMin, Vector3f aabbMax, BroadphaseNativeType shapeType, Object userPtr, short collisionFilterGroup, short collisionFilterMask, Dispatcher dispatcher) {
+		assert (aabbMin.x <= aabbMax.x && aabbMin.y <= aabbMax.y && aabbMin.z <= aabbMax.z);
+
+		SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask);
+		proxy.uniqueId = handles.size();
+		handles.add(proxy);
+		return proxy;
+	}
+
+	public void destroyProxy(BroadphaseProxy proxyOrg, Dispatcher dispatcher) {
+		handles.remove(proxyOrg);
+
+		pairCache.removeOverlappingPairsContainingProxy(proxyOrg, dispatcher);
+	}
+
+	public void setAabb(BroadphaseProxy proxy, Vector3f aabbMin, Vector3f aabbMax, Dispatcher dispatcher) {
+		SimpleBroadphaseProxy sbp = (SimpleBroadphaseProxy)proxy;
+		sbp.min.set(aabbMin);
+		sbp.max.set(aabbMax);
+	}
+
+	private static boolean aabbOverlap(SimpleBroadphaseProxy proxy0, SimpleBroadphaseProxy proxy1) {
+		return proxy0.min.x <= proxy1.max.x && proxy1.min.x <= proxy0.max.x &&
+				proxy0.min.y <= proxy1.max.y && proxy1.min.y <= proxy0.max.y &&
+				proxy0.min.z <= proxy1.max.z && proxy1.min.z <= proxy0.max.z;
+	}
+
+	public void calculateOverlappingPairs(Dispatcher dispatcher) {
+		for (int i=0; i<handles.size(); i++) {
+			SimpleBroadphaseProxy proxy0 = handles.get(i);
+			for (int j=0; j<handles.size(); j++) {
+				SimpleBroadphaseProxy proxy1 = handles.get(j);
+				if (proxy0 == proxy1) continue;
+				
+				if (aabbOverlap(proxy0, proxy1)) {
+					if (pairCache.findPair(proxy0, proxy1) == null) {
+						pairCache.addOverlappingPair(proxy0, proxy1);
+					}
+				}
+				else {
+					if (pairCache.findPair(proxy0, proxy1) != null) {
+						pairCache.removeOverlappingPair(proxy0, proxy1, dispatcher);
+					}
+				}
+			}
+		}
+	}
+
+	public OverlappingPairCache getOverlappingPairCache() {
+		return pairCache;
+	}
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/SimpleBroadphaseProxy.java b/src/jbullet/src/javabullet/collision/broadphase/SimpleBroadphaseProxy.java
new file mode 100644
index 0000000..1ec73d4
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/SimpleBroadphaseProxy.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.collision.broadphase;
+
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class SimpleBroadphaseProxy extends BroadphaseProxy {
+
+	protected final Vector3f min = new Vector3f();
+	protected final Vector3f max = new Vector3f();
+	
+	public SimpleBroadphaseProxy() {
+	}
+
+	public SimpleBroadphaseProxy(Vector3f minpt, Vector3f maxpt, BroadphaseNativeType shapeType, Object userPtr, short collisionFilterGroup, short collisionFilterMask) {
+		super(userPtr, collisionFilterGroup, collisionFilterMask);
+		this.min.set(minpt);
+		this.max.set(maxpt);
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/broadphase/package-info.java b/src/jbullet/src/javabullet/collision/broadphase/package-info.java
new file mode 100644
index 0000000..4d78be3
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/broadphase/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.
+ */
+
+/**
+ * Broadphase collision code for fast determining of overlapping pairs.
+ */
+package javabullet.collision.broadphase;
+
diff --git a/src/jbullet/src/javabullet/collision/dispatch/CollisionAlgorithmCreateFunc.java b/src/jbullet/src/javabullet/collision/dispatch/CollisionAlgorithmCreateFunc.java
new file mode 100644
index 0000000..6b3e86c
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/CollisionAlgorithmCreateFunc.java
@@ -0,0 +1,40 @@
+/*
+ * 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.collision.dispatch;
+
+import javabullet.collision.broadphase.CollisionAlgorithm;
+import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo;
+
+/**
+ * Used by the CollisionDispatcher to register and create instances for CollisionAlgorithm.
+ * 
+ * @author jezek2
+ */
+public abstract class CollisionAlgorithmCreateFunc {
+
+	public boolean swapped;
+	
+	public abstract CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/CollisionConfiguration.java b/src/jbullet/src/javabullet/collision/dispatch/CollisionConfiguration.java
new file mode 100644
index 0000000..5992624
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/CollisionConfiguration.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.collision.dispatch;
+
+import javabullet.collision.broadphase.BroadphaseNativeType;
+
+/**
+ * CollisionConfiguration allows to configure Bullet collision detection
+ * stack allocator size, default collision algorithms and persistent manifold pool size
+ * todo: describe the meaning
+ * 
+ * @author jezek2
+ */
+public abstract class CollisionConfiguration {
+
+	/*
+	///memory pools
+	virtual btPoolAllocator* getPersistentManifoldPool() = 0;
+	virtual btPoolAllocator* getCollisionAlgorithmPool() = 0;
+	virtual btStackAlloc*	getStackAllocator() = 0;
+	 */
+	
+	public abstract CollisionAlgorithmCreateFunc getCollisionAlgorithmCreateFunc(BroadphaseNativeType proxyType0, BroadphaseNativeType proxyType1);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/CollisionDispatcher.java b/src/jbullet/src/javabullet/collision/dispatch/CollisionDispatcher.java
new file mode 100644
index 0000000..a01ca77
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/CollisionDispatcher.java
@@ -0,0 +1,292 @@
+/*
+ * 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.collision.dispatch;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+import javabullet.BulletPool;
+import javabullet.ObjectPool;
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.collision.broadphase.BroadphasePair;
+import javabullet.collision.broadphase.CollisionAlgorithm;
+import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo;
+import javabullet.collision.broadphase.DispatchFunc;
+import javabullet.collision.broadphase.Dispatcher;
+import javabullet.collision.broadphase.DispatcherInfo;
+import javabullet.collision.broadphase.OverlapCallback;
+import javabullet.collision.broadphase.OverlappingPairCache;
+import javabullet.collision.narrowphase.PersistentManifold;
+
+/**
+ * CollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs.
+ * Time of Impact, Closest Points and Penetration Depth.
+ * 
+ * @author jezek2
+ */
+public class CollisionDispatcher extends Dispatcher {
+	
+	protected final ObjectPool<PersistentManifold> manifoldsPool = BulletPool.get(PersistentManifold.class);
+
+	private static final int MAX_BROADPHASE_COLLISION_TYPES = BroadphaseNativeType.MAX_BROADPHASE_COLLISION_TYPES.ordinal();
+	private int count = 0;
+	private final List<PersistentManifold> manifoldsPtr = new ArrayList<PersistentManifold>();
+	private boolean useIslands = true;
+	private boolean staticWarningReported = false;
+	private ManifoldResult defaultManifoldResult;
+	private NearCallback nearCallback;
+	//private PoolAllocator*	m_collisionAlgorithmPoolAllocator;
+	//private PoolAllocator*	m_persistentManifoldPoolAllocator;
+	private final CollisionAlgorithmCreateFunc[][] doubleDispatch = new CollisionAlgorithmCreateFunc[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES];
+	private CollisionConfiguration collisionConfiguration;
+	private static int gNumManifold = 0;
+
+	public CollisionDispatcher(CollisionConfiguration collisionConfiguration) {
+		this.collisionConfiguration = collisionConfiguration;
+
+		setNearCallback(new DefaultNearCallback());
+
+		//m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool();
+		//m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool();
+
+		for (int i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++) {
+			for (int j = 0; j < MAX_BROADPHASE_COLLISION_TYPES; j++) {
+				doubleDispatch[i][j] = collisionConfiguration.getCollisionAlgorithmCreateFunc(
+					BroadphaseNativeType.forValue(i),
+					BroadphaseNativeType.forValue(j)
+				);
+				assert (doubleDispatch[i][j] != null);
+			}
+		}
+	}
+
+	public void registerCollisionCreateFunc(int proxyType0, int proxyType1, CollisionAlgorithmCreateFunc createFunc) {
+		doubleDispatch[proxyType0][proxyType1] = createFunc;
+	}
+
+	public NearCallback getNearCallback() {
+		return nearCallback;
+	}
+
+	public void setNearCallback(NearCallback nearCallback) {
+		this.nearCallback = nearCallback;
+	}
+
+	public CollisionConfiguration getCollisionConfiguration() {
+		return collisionConfiguration;
+	}
+
+	public void setCollisionConfiguration(CollisionConfiguration collisionConfiguration) {
+		this.collisionConfiguration = collisionConfiguration;
+	}
+
+	@Override
+	public CollisionAlgorithm findAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold) {
+		CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo();
+
+		ci.dispatcher1 = this;
+		ci.manifold = sharedManifold;
+		CollisionAlgorithm algo = doubleDispatch[body0.getCollisionShape().getShapeType().ordinal()][body1.getCollisionShape().getShapeType().ordinal()].createCollisionAlgorithm(ci, body0, body1);
+
+		return algo;
+	}
+
+	@Override
+	public PersistentManifold getNewManifold(Object b0, Object b1) {
+		gNumManifold++;
+
+		//btAssert(gNumManifold < 65535);
+
+		CollisionObject body0 = (CollisionObject)b0;
+		CollisionObject body1 = (CollisionObject)b1;
+
+		/*
+		void* mem = 0;
+
+		if (m_persistentManifoldPoolAllocator->getFreeCount())
+		{
+			mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold));
+		} else
+		{
+			mem = btAlignedAlloc(sizeof(btPersistentManifold),16);
+
+		}
+		btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0);
+		manifold->m_index1a = m_manifoldsPtr.size();
+		m_manifoldsPtr.push_back(manifold);
+		*/
+		
+		PersistentManifold manifold = manifoldsPool.get();
+		manifold.init(body0,body1,0);
+		
+		manifold.index1a = manifoldsPtr.size();
+		manifoldsPtr.add(manifold);
+
+		return manifold;
+	}
+
+	@Override
+	public void releaseManifold(PersistentManifold manifold) {
+		gNumManifold--;
+
+		//printf("releaseManifold: gNumManifold %d\n",gNumManifold);
+		clearManifold(manifold);
+
+		// TODO: optimize
+		int findIndex = manifold.index1a;
+		assert (findIndex < manifoldsPtr.size());
+		Collections.swap(manifoldsPtr, findIndex, manifoldsPtr.size()-1);
+		manifoldsPtr.get(findIndex).index1a = findIndex;
+		manifoldsPtr.remove(manifoldsPtr.size()-1);
+
+		manifoldsPool.release(manifold);
+		/*
+		manifold->~btPersistentManifold();
+		if (m_persistentManifoldPoolAllocator->validPtr(manifold))
+		{
+			m_persistentManifoldPoolAllocator->freeMemory(manifold);
+		} else
+		{
+			btAlignedFree(manifold);
+		}
+		*/
+	}
+
+	@Override
+	public void clearManifold(PersistentManifold manifold) {
+		manifold.clearManifold();
+	}
+
+	@Override
+	public boolean needsCollision(CollisionObject body0, CollisionObject body1) {
+		assert (body0 != null);
+		assert (body1 != null);
+
+		boolean needsCollision = true;
+
+		//#ifdef BT_DEBUG
+		if (!staticWarningReported) {
+			// broadphase filtering already deals with this
+			if ((body0.isStaticObject() || body0.isKinematicObject()) &&
+					(body1.isStaticObject() || body1.isKinematicObject())) {
+				staticWarningReported = true;
+				System.err.println("warning CollisionDispatcher.needsCollision: static-static collision!");
+			}
+		}
+		//#endif //BT_DEBUG
+
+		if ((!body0.isActive()) && (!body1.isActive())) {
+			needsCollision = false;
+		}
+		else if (!body0.checkCollideWith(body1)) {
+			needsCollision = false;
+		}
+
+		return needsCollision;
+	}
+
+	@Override
+	public boolean needsResponse(CollisionObject body0, CollisionObject body1) {
+		//here you can do filtering
+		boolean hasResponse = (body0.hasContactResponse() && body1.hasContactResponse());
+		//no response between two static/kinematic bodies:
+		hasResponse = hasResponse && ((!body0.isStaticOrKinematicObject()) || (!body1.isStaticOrKinematicObject()));
+		return hasResponse;
+	}
+
+	private static class CollisionPairCallback implements OverlapCallback {
+		private DispatcherInfo dispatchInfo;
+		private CollisionDispatcher dispatcher;
+
+		public void init(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher) {
+			this.dispatchInfo = dispatchInfo;
+			this.dispatcher = dispatcher;
+		}
+		
+		public boolean processOverlap(BroadphasePair pair) {
+			dispatcher.getNearCallback().invoke(pair, dispatcher, dispatchInfo);
+			return false;
+		}
+	}
+	
+	private CollisionPairCallback collisionPairCallback = new CollisionPairCallback();
+	
+	@Override
+	public void dispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo, Dispatcher dispatcher) {
+		//m_blockedForChanges = true;
+		collisionPairCallback.init(dispatchInfo, this);
+		pairCache.processAllOverlappingPairs(collisionPairCallback, dispatcher);
+		//m_blockedForChanges = false;
+	}
+
+	@Override
+	public int getNumManifolds() {
+		return manifoldsPtr.size();
+	}
+
+	@Override
+	public PersistentManifold getManifoldByIndexInternal(int index) {
+		return manifoldsPtr.get(index);
+	}
+
+	@Override
+	public List<PersistentManifold> getInternalManifoldPointer() {
+		return manifoldsPtr;
+	}
+
+	////////////////////////////////////////////////////////////////////////////
+	
+	private static class DefaultNearCallback implements NearCallback {
+		private final ManifoldResult contactPointResult = new ManifoldResult();
+		
+		public void invoke(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo) {
+			CollisionObject colObj0 = (CollisionObject) collisionPair.pProxy0.clientObject;
+			CollisionObject colObj1 = (CollisionObject) collisionPair.pProxy1.clientObject;
+
+			if (dispatcher.needsCollision(colObj0, colObj1)) {
+				// dispatcher will keep algorithms persistent in the collision pair
+				if (collisionPair.algorithm == null) {
+					collisionPair.algorithm = dispatcher.findAlgorithm(colObj0, colObj1);
+				}
+
+				if (collisionPair.algorithm != null) {
+					//ManifoldResult contactPointResult = new ManifoldResult(colObj0, colObj1);
+					contactPointResult.init(colObj0, colObj1);
+
+					if (dispatchInfo.dispatchFunc == DispatchFunc.DISPATCH_DISCRETE) {
+						// discrete collision detection query
+						collisionPair.algorithm.processCollision(colObj0, colObj1, dispatchInfo, contactPointResult);
+					}
+					else {
+						// continuous collision detection query, time of impact (toi)
+						float toi = collisionPair.algorithm.calculateTimeOfImpact(colObj0, colObj1, dispatchInfo, contactPointResult);
+						if (dispatchInfo.timeOfImpact > toi) {
+							dispatchInfo.timeOfImpact = toi;
+						}
+					}
+				}
+			}
+		}
+	}
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/CollisionFlags.java b/src/jbullet/src/javabullet/collision/dispatch/CollisionFlags.java
new file mode 100644
index 0000000..9203f08
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/CollisionFlags.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.collision.dispatch;
+
+/**
+ *
+ * @author jezek2
+ */
+public class CollisionFlags {
+
+	public static final int STATIC_OBJECT            = 1;
+	public static final int KINEMATIC_OBJECT         = 2;
+	public static final int NO_CONTACT_RESPONSE      = 4;
+	public static final int CUSTOM_MATERIAL_CALLBACK = 8; // this allows per-triangle material (friction/restitution)
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/CollisionObject.java b/src/jbullet/src/javabullet/collision/dispatch/CollisionObject.java
new file mode 100644
index 0000000..483184b
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/CollisionObject.java
@@ -0,0 +1,282 @@
+/*
+ * 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.collision.dispatch;
+
+import javabullet.BulletStack;
+import javabullet.collision.broadphase.BroadphaseProxy;
+import javabullet.collision.shapes.CollisionShape;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * CollisionObject can be used to manage collision detection objects.
+ * CollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
+ * They can be added to the CollisionWorld.
+ * 
+ * @author jezek2
+ */
+public class CollisionObject {
+	
+	protected final BulletStack stack = BulletStack.get();
+
+	// island management, m_activationState1
+	public static final int ACTIVE_TAG = 1;
+	public static final int ISLAND_SLEEPING = 2;
+	public static final int WANTS_DEACTIVATION = 3;
+	public static final int DISABLE_DEACTIVATION = 4;
+	public static final int DISABLE_SIMULATION = 5;
+	protected Transform worldTransform = new Transform();
+
+	///m_interpolationWorldTransform is used for CCD and interpolation
+	///it can be either previous or future (predicted) transform
+	protected final Transform interpolationWorldTransform = new Transform();
+	//those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) 
+	//without destroying the continuous interpolated motion (which uses this interpolation velocities)
+	protected final Vector3f interpolationLinearVelocity = new Vector3f();
+	protected final Vector3f interpolationAngularVelocity = new Vector3f();
+	protected BroadphaseProxy broadphaseHandle;
+	protected CollisionShape collisionShape;
+	protected int collisionFlags;
+	protected int islandTag1;
+	protected int companionId;
+	protected int activationState1;
+	protected float deactivationTime;
+	protected float friction;
+	protected float restitution;
+
+	///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer
+	protected Object userObjectPointer;
+
+	///m_internalOwner is reserved to point to Bullet's btRigidBody. Don't use this, use m_userObjectPointer instead.
+	protected Object internalOwner;
+
+	///time of impact calculation
+	protected float hitFraction;
+	///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+	protected float ccdSweptSphereRadius;
+
+	/// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
+	protected float ccdSquareMotionThreshold;
+	/// If some object should have elaborate collision filtering by sub-classes
+	protected boolean checkCollideWith;
+
+	public CollisionObject() {
+		this.collisionFlags = CollisionFlags.STATIC_OBJECT;
+		this.islandTag1 = -1;
+		this.companionId = -1;
+		this.activationState1 = 1;
+		this.friction = 0.5f;
+		this.hitFraction = 1f;
+	}
+
+	public boolean checkCollideWithOverride(CollisionObject co) {
+		return true;
+	}
+
+	public boolean mergesSimulationIslands() {
+		///static objects, kinematic and object without contact response don't merge islands
+		return ((collisionFlags & (CollisionFlags.STATIC_OBJECT | CollisionFlags.KINEMATIC_OBJECT | CollisionFlags.NO_CONTACT_RESPONSE)) == 0);
+	}
+
+	public boolean isStaticObject() {
+		return (collisionFlags & CollisionFlags.STATIC_OBJECT) != 0;
+	}
+
+	public boolean isKinematicObject() {
+		return (collisionFlags & CollisionFlags.KINEMATIC_OBJECT) != 0;
+	}
+
+	public boolean isStaticOrKinematicObject() {
+		return (collisionFlags & (CollisionFlags.KINEMATIC_OBJECT | CollisionFlags.STATIC_OBJECT)) != 0;
+	}
+
+	public boolean hasContactResponse() {
+		return (collisionFlags & CollisionFlags.NO_CONTACT_RESPONSE) == 0;
+	}
+
+	public CollisionShape getCollisionShape() {
+		return collisionShape;
+	}
+
+	public void setCollisionShape(CollisionShape collisionShape) {
+		this.collisionShape = collisionShape;
+	}
+
+	public int getActivationState() {
+		return activationState1;
+	}
+
+	public void setActivationState(int newState) {
+		if ((activationState1 != DISABLE_DEACTIVATION) && (activationState1 != DISABLE_SIMULATION)) {
+			this.activationState1 = newState;
+		}
+	}
+
+	public float getDeactivationTime() {
+		return deactivationTime;
+	}
+
+	public void setDeactivationTime(float deactivationTime) {
+		this.deactivationTime = deactivationTime;
+	}
+
+	public void forceActivationState(int newState) {
+		this.activationState1 = newState;
+	}
+
+	public void activate() {
+		activate(false);
+	}
+
+	public void activate(boolean forceActivation) {
+		if (forceActivation || (collisionFlags & (CollisionFlags.STATIC_OBJECT | CollisionFlags.KINEMATIC_OBJECT)) == 0) {
+			setActivationState(ACTIVE_TAG);
+			deactivationTime = 0f;
+		}
+	}
+
+	public boolean isActive() {
+		return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION));
+	}
+
+	public float getRestitution() {
+		return restitution;
+	}
+
+	public void setRestitution(float restitution) {
+		this.restitution = restitution;
+	}
+
+	public float getFriction() {
+		return friction;
+	}
+
+	public void setFriction(float friction) {
+		this.friction = friction;
+	}
+
+	// reserved for Bullet internal usage
+	public Object getInternalOwner() {
+		return internalOwner;
+	}
+
+	public Transform getWorldTransform() {
+		return worldTransform;
+	}
+
+	public void setWorldTransform(Transform worldTransform) {
+		this.worldTransform.set(worldTransform);
+	}
+
+	public BroadphaseProxy getBroadphaseHandle() {
+		return broadphaseHandle;
+	}
+
+	public void setBroadphaseHandle(BroadphaseProxy broadphaseHandle) {
+		this.broadphaseHandle = broadphaseHandle;
+	}
+
+	public Transform getInterpolationWorldTransform() {
+		return interpolationWorldTransform;
+	}
+
+	public void setInterpolationWorldTransform(Transform interpolationWorldTransform) {
+		this.interpolationWorldTransform.set(interpolationWorldTransform);
+	}
+
+	public Vector3f getInterpolationLinearVelocity() {
+		return interpolationLinearVelocity;
+	}
+
+	public Vector3f getInterpolationAngularVelocity() {
+		return interpolationAngularVelocity;
+	}
+
+	public int getIslandTag() {
+		return islandTag1;
+	}
+
+	public void setIslandTag(int islandTag) {
+		this.islandTag1 = islandTag;
+	}
+
+	public int getCompanionId() {
+		return companionId;
+	}
+
+	public void setCompanionId(int companionId) {
+		this.companionId = companionId;
+	}
+
+	public float getHitFraction() {
+		return hitFraction;
+	}
+
+	public void setHitFraction(float hitFraction) {
+		this.hitFraction = hitFraction;
+	}
+
+	public int getCollisionFlags() {
+		return collisionFlags;
+	}
+
+	public void setCollisionFlags(int collisionFlags) {
+		this.collisionFlags = collisionFlags;
+	}
+
+	// Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+	public float getCcdSweptSphereRadius() {
+		return ccdSweptSphereRadius;
+	}
+
+	// Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+	public void setCcdSweptSphereRadius(float ccdSweptSphereRadius) {
+		this.ccdSweptSphereRadius = ccdSweptSphereRadius;
+	}
+
+	public float getCcdSquareMotionThreshold() {
+		return ccdSquareMotionThreshold;
+	}
+
+	// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
+	public void setCcdSquareMotionThreshold(float ccdSquareMotionThreshold) {
+		this.ccdSquareMotionThreshold = ccdSquareMotionThreshold;
+	}
+
+	public Object getUserPointer() {
+		return userObjectPointer;
+	}
+
+	public void setUserPointer(Object userObjectPointer) {
+		this.userObjectPointer = userObjectPointer;
+	}
+
+	public boolean checkCollideWith(CollisionObject co) {
+		if (checkCollideWith) {
+			return checkCollideWithOverride(co);
+		}
+
+		return true;
+	}
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/CollisionWorld.java b/src/jbullet/src/javabullet/collision/dispatch/CollisionWorld.java
new file mode 100644
index 0000000..95d3e7d
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/CollisionWorld.java
@@ -0,0 +1,567 @@
+/*
+ * 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.collision.dispatch;
+
+import java.util.ArrayList;
+import java.util.List;
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javabullet.collision.broadphase.BroadphaseInterface;
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.collision.broadphase.BroadphaseProxy;
+import javabullet.collision.broadphase.Dispatcher;
+import javabullet.collision.broadphase.DispatcherInfo;
+import javabullet.collision.broadphase.OverlappingPairCache;
+import javabullet.collision.narrowphase.ConvexCast.CastResult;
+import javabullet.collision.narrowphase.SubsimplexConvexCast;
+import javabullet.collision.narrowphase.TriangleRaycastCallback;
+import javabullet.collision.narrowphase.VoronoiSimplexSolver;
+import javabullet.collision.shapes.BvhTriangleMeshShape;
+import javabullet.collision.shapes.CollisionShape;
+import javabullet.collision.shapes.CompoundShape;
+import javabullet.collision.shapes.ConcaveShape;
+import javabullet.collision.shapes.ConvexShape;
+import javabullet.collision.shapes.SphereShape;
+import javabullet.linearmath.AabbUtil2;
+import javabullet.linearmath.IDebugDraw;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ * CollisionWorld is interface and container for the collision detection.
+ * 
+ * @author jezek2
+ */
+public class CollisionWorld {
+
+	protected final BulletStack stack = BulletStack.get();
+	
+	protected List<CollisionObject> collisionObjects = new ArrayList<CollisionObject>();
+	protected Dispatcher dispatcher1;
+	protected DispatcherInfo dispatchInfo = new DispatcherInfo();
+	//protected btStackAlloc*	m_stackAlloc;
+	protected BroadphaseInterface broadphasePairCache;
+	protected IDebugDraw debugDrawer;
+	
+	/**
+	 * This constructor doesn't own the dispatcher and paircache/broadphase.
+	 */
+	public CollisionWorld(Dispatcher dispatcher,BroadphaseInterface broadphasePairCache, CollisionConfiguration collisionConfiguration) {
+		this.dispatcher1 = dispatcher;
+		this.broadphasePairCache = broadphasePairCache;
+	}
+	
+	public void destroy() {
+		// clean up remaining objects
+		for (int i = 0; i < collisionObjects.size(); i++) {
+			CollisionObject collisionObject = collisionObjects.get(i);
+
+			BroadphaseProxy bp = collisionObject.getBroadphaseHandle();
+			if (bp != null) {
+				//
+				// only clear the cached algorithms
+				//
+				getBroadphase().getOverlappingPairCache().cleanProxyFromPairs(bp, dispatcher1);
+				getBroadphase().destroyProxy(bp, dispatcher1);
+			}
+		}
+	}
+	
+	public void addCollisionObject(CollisionObject collisionObject) {
+		addCollisionObject(collisionObject, (short)1, (short)1);
+	}
+
+	public void addCollisionObject(CollisionObject collisionObject, short collisionFilterGroup, short collisionFilterMask) {
+		stack.pushCommonMath();
+		try {
+			// check that the object isn't already added
+			assert (!collisionObjects.contains(collisionObject));
+
+			collisionObjects.add(collisionObject);
+
+			// calculate new AABB
+			// TODO: check if it's overwritten or not
+			Transform trans = stack.transforms.get(collisionObject.getWorldTransform());
+
+			Vector3f minAabb = stack.vectors.get();
+			Vector3f maxAabb = stack.vectors.get();
+			collisionObject.getCollisionShape().getAabb(trans, minAabb, maxAabb);
+
+			BroadphaseNativeType type = collisionObject.getCollisionShape().getShapeType();
+			collisionObject.setBroadphaseHandle(getBroadphase().createProxy(
+					minAabb,
+					maxAabb,
+					type,
+					collisionObject,
+					collisionFilterGroup,
+					collisionFilterMask,
+					dispatcher1));
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	public void performDiscreteCollisionDetection() {
+		BulletGlobals.pushProfile("performDiscreteCollisionDetection");
+		try {
+			//DispatcherInfo dispatchInfo = getDispatchInfo();
+
+			updateAabbs();
+
+			broadphasePairCache.calculateOverlappingPairs(dispatcher1);
+
+			Dispatcher dispatcher = getDispatcher();
+			{
+				BulletGlobals.pushProfile("dispatchAllCollisionPairs");
+				try {
+					if (dispatcher != null) {
+						dispatcher.dispatchAllCollisionPairs(broadphasePairCache.getOverlappingPairCache(), dispatchInfo, dispatcher1);
+					}
+				}
+				finally {
+					BulletGlobals.popProfile();
+				}
+			}
+		}
+		finally {
+			BulletGlobals.popProfile();
+		}
+	}
+	
+	public void removeCollisionObject(CollisionObject collisionObject) {
+		//bool removeFromBroadphase = false;
+
+		{
+			BroadphaseProxy bp = collisionObject.getBroadphaseHandle();
+			if (bp != null) {
+				//
+				// only clear the cached algorithms
+				//
+				getBroadphase().getOverlappingPairCache().cleanProxyFromPairs(bp, dispatcher1);
+				getBroadphase().destroyProxy(bp, dispatcher1);
+				collisionObject.setBroadphaseHandle(null);
+			}
+		}
+
+		//swapremove
+		collisionObjects.remove(collisionObject);
+	}
+
+	public BroadphaseInterface getBroadphase() {
+		return broadphasePairCache;
+	}
+	
+	public OverlappingPairCache getPairCache() {
+		return broadphasePairCache.getOverlappingPairCache();
+	}
+
+	public Dispatcher getDispatcher() {
+		return dispatcher1;
+	}
+
+	public DispatcherInfo getDispatchInfo() {
+		return dispatchInfo;
+	}
+	
+	private static boolean updateAabbs_reportMe = true;
+	
+	public void updateAabbs() {
+		BulletGlobals.pushProfile("updateAabbs");
+		stack.pushCommonMath();
+		try {
+			Transform predictedTrans = stack.transforms.get();
+			Vector3f minAabb = stack.vectors.get(), maxAabb = stack.vectors.get();
+			Vector3f tmp = stack.vectors.get();
+
+			for (int i = 0; i < collisionObjects.size(); i++) {
+				CollisionObject colObj = collisionObjects.get(i);
+
+				// only update aabb of active objects
+				if (colObj.isActive()) {
+					colObj.getCollisionShape().getAabb(colObj.getWorldTransform(), minAabb, maxAabb);
+					BroadphaseInterface bp = broadphasePairCache;
+
+					// moving objects should be moderately sized, probably something wrong if not
+					tmp.sub(maxAabb, minAabb); // TODO: optimize
+					if (colObj.isStaticObject() || (tmp.lengthSquared() < 1e12f)) {
+						bp.setAabb(colObj.getBroadphaseHandle(), minAabb, maxAabb, dispatcher1);
+					}
+					else {
+						// something went wrong, investigate
+						// this assert is unwanted in 3D modelers (danger of loosing work)
+						colObj.setActivationState(CollisionObject.DISABLE_SIMULATION);
+
+						if (updateAabbs_reportMe && debugDrawer != null) {
+							updateAabbs_reportMe = false;
+							debugDrawer.reportErrorWarning("Overflow in AABB, object removed from simulation");
+							debugDrawer.reportErrorWarning("If you can reproduce this, please email [email protected]\n");
+							debugDrawer.reportErrorWarning("Please include above information, your Platform, version of OS.\n");
+							debugDrawer.reportErrorWarning("Thanks.\n");
+						}
+					}
+				}
+			}
+		}
+		finally {
+			stack.popCommonMath();
+			BulletGlobals.popProfile();
+		}
+	}
+
+	public IDebugDraw getDebugDrawer() {
+		return debugDrawer;
+	}
+
+	public void setDebugDrawer(IDebugDraw debugDrawer) {
+		this.debugDrawer = debugDrawer;
+	}
+	
+	public int getNumCollisionObjects() {
+		return collisionObjects.size();
+	}
+
+	// TODO
+	public /*static*/ void rayTestSingle(Transform rayFromTrans, Transform rayToTrans,
+			CollisionObject collisionObject,
+			CollisionShape collisionShape,
+			Transform colObjWorldTransform,
+			RayResultCallback resultCallback, short collisionFilterMask) {
+		stack.pushCommonMath();
+		try {
+			SphereShape pointShape = new SphereShape(0f);
+			pointShape.setMargin(0f);
+			ConvexShape castShape = pointShape;
+
+			if (collisionShape.isConvex()) {
+				CastResult castResult = new CastResult();
+				castResult.fraction = resultCallback.closestHitFraction;
+
+				ConvexShape convexShape = (ConvexShape) collisionShape;
+				VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
+
+				//#define USE_SUBSIMPLEX_CONVEX_CAST 1
+				//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+				SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(castShape, convexShape, simplexSolver);
+				//#else
+				//btGjkConvexCast	convexCaster(castShape,convexShape,&simplexSolver);
+				//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
+				//#endif //#USE_SUBSIMPLEX_CONVEX_CAST
+
+				if (convexCaster.calcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) {
+					//add hit
+					if (castResult.normal.lengthSquared() > 0.0001f) {
+						if (castResult.fraction < resultCallback.closestHitFraction) {
+							//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+							//rotate normal into worldspace
+							rayFromTrans.basis.transform(castResult.normal);
+							//#endif //USE_SUBSIMPLEX_CONVEX_CAST
+
+							castResult.normal.normalize();
+							LocalRayResult localRayResult = new LocalRayResult(
+									collisionObject,
+									null,
+									castResult.normal,
+									castResult.fraction);
+
+							boolean normalInWorldSpace = true;
+							resultCallback.addSingleResult(localRayResult, normalInWorldSpace);
+						}
+					}
+				}
+			}
+			else {
+				if (collisionShape.isConcave()) {
+					if (collisionShape.getShapeType() == BroadphaseNativeType.TRIANGLE_MESH_SHAPE_PROXYTYPE) {
+						// optimized version for BvhTriangleMeshShape
+						BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
+						Transform worldTocollisionObject = stack.transforms.get();
+						worldTocollisionObject.inverse(colObjWorldTransform);
+						Vector3f rayFromLocal = stack.vectors.get(rayFromTrans.origin);
+						worldTocollisionObject.transform(rayFromLocal);
+						Vector3f rayToLocal = stack.vectors.get(rayToTrans.origin);
+						worldTocollisionObject.transform(rayToLocal);
+
+						BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
+						rcb.hitFraction = resultCallback.closestHitFraction;
+						triangleMesh.performRaycast(rcb, rayFromLocal, rayToLocal);
+					}
+					else {
+						ConcaveShape triangleMesh = (ConcaveShape)collisionShape;
+
+						Transform worldTocollisionObject = stack.transforms.get();
+						worldTocollisionObject.inverse(colObjWorldTransform);
+
+						Vector3f rayFromLocal = stack.vectors.get(rayFromTrans.origin);
+						worldTocollisionObject.transform(rayFromLocal);
+						Vector3f rayToLocal = stack.vectors.get(rayToTrans.origin);
+						worldTocollisionObject.transform(rayToLocal);
+
+						BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
+						rcb.hitFraction = resultCallback.closestHitFraction;
+
+						Vector3f rayAabbMinLocal = stack.vectors.get(rayFromLocal);
+						VectorUtil.setMin(rayAabbMinLocal, rayToLocal);
+						Vector3f rayAabbMaxLocal = stack.vectors.get(rayFromLocal);
+						VectorUtil.setMax(rayAabbMaxLocal, rayToLocal);
+
+						triangleMesh.processAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);
+					}
+				}
+				else {
+					// todo: use AABB tree or other BVH acceleration structure!
+					if (collisionShape.isCompound()) {
+						CompoundShape compoundShape = (CompoundShape) collisionShape;
+						int i = 0;
+						for (i = 0; i < compoundShape.getNumChildShapes(); i++) {
+							Transform childTrans = stack.transforms.get(compoundShape.getChildTransform(i));
+							CollisionShape childCollisionShape = compoundShape.getChildShape(i);
+							Transform childWorldTrans = stack.transforms.get(colObjWorldTransform);
+							childWorldTrans.mul(childTrans);
+							rayTestSingle(rayFromTrans, rayToTrans,
+									collisionObject,
+									childCollisionShape,
+									childWorldTrans,
+									resultCallback, collisionFilterMask);
+						}
+					}
+				}
+			}
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+	
+	public void rayTest(Vector3f rayFromWorld, Vector3f rayToWorld, RayResultCallback resultCallback) {
+		rayTest(rayFromWorld, rayToWorld, resultCallback, (short)-1);
+	}
+	
+	/**
+	 * rayTest performs a raycast on all objects in the CollisionWorld, and calls the resultCallback.
+	 * This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
+	 */
+	public void rayTest(Vector3f rayFromWorld, Vector3f rayToWorld, RayResultCallback resultCallback, short collisionFilterMask) {
+		stack.pushCommonMath();
+		try {
+			Transform rayFromTrans = stack.transforms.get(), rayToTrans = stack.transforms.get();
+			rayFromTrans.setIdentity();
+			rayFromTrans.origin.set(rayFromWorld);
+			rayToTrans.setIdentity();
+
+			rayToTrans.origin.set(rayToWorld);
+
+			// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
+			Vector3f collisionObjectAabbMin = stack.vectors.get(), collisionObjectAabbMax = stack.vectors.get();
+			float[] hitLambda = new float[1];
+
+			for (int i = 0; i < collisionObjects.size(); i++) {
+				// terminate further ray tests, once the closestHitFraction reached zero
+				if (resultCallback.closestHitFraction == 0f) {
+					break;
+				}
+
+				CollisionObject collisionObject = collisionObjects.get(i);
+				// only perform raycast if filterMask matches
+				if ((collisionObject.getBroadphaseHandle().collisionFilterGroup & collisionFilterMask) != 0) {
+					//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+					collisionObject.getCollisionShape().getAabb(collisionObject.getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax);
+
+					hitLambda[0] = resultCallback.closestHitFraction;
+					Vector3f hitNormal = stack.vectors.get();
+					if (AabbUtil2.rayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) {
+						rayTestSingle(rayFromTrans, rayToTrans,
+								collisionObject,
+								collisionObject.getCollisionShape(),
+								collisionObject.getWorldTransform(),
+								resultCallback,
+								(short) -1);
+					}
+				}
+
+			}
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+	
+	public List<CollisionObject> getCollisionObjectArray() {
+		return collisionObjects;
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	/**
+	 * LocalShapeInfo gives extra information for complex shapes.
+	 * Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart.
+	 */
+	public static class LocalShapeInfo {
+		public int shapePart;
+		public int triangleIndex;
+		//const btCollisionShape*	m_shapeTemp;
+		//const btTransform*	m_shapeLocalTransform;
+	}
+	
+	public static class LocalRayResult {
+		public CollisionObject collisionObject;
+		public LocalShapeInfo localShapeInfo;
+		public final Vector3f hitNormalLocal = new Vector3f();
+		public float hitFraction;
+
+		public LocalRayResult(CollisionObject collisionObject, LocalShapeInfo localShapeInfo, Vector3f hitNormalLocal, float hitFraction) {
+			this.collisionObject = collisionObject;
+			this.localShapeInfo = localShapeInfo;
+			this.hitNormalLocal.set(hitNormalLocal);
+			this.hitFraction = hitFraction;
+		}
+	}
+	
+	/**
+	 * RayResultCallback is used to report new raycast results.
+	 */
+	public static abstract class RayResultCallback {
+		public float closestHitFraction = 1f;
+		public CollisionObject collisionObject;
+		
+		public boolean hasHit() {
+			return (collisionObject != null);
+		}
+		
+		public abstract float addSingleResult(LocalRayResult rayResult, boolean normalInWorldSpace);
+	}
+	
+	public static class ClosestRayResultCallback extends RayResultCallback {
+		public final Vector3f rayFromWorld = new Vector3f(); //used to calculate hitPointWorld from hitFraction
+		public final Vector3f rayToWorld = new Vector3f();
+
+		public final Vector3f hitNormalWorld = new Vector3f();
+		public final Vector3f hitPointWorld = new Vector3f();
+		
+		public ClosestRayResultCallback(Vector3f rayFromWorld, Vector3f rayToWorld) {
+			this.rayFromWorld.set(rayFromWorld);
+			this.rayToWorld.set(rayToWorld);
+		}
+		
+		@Override
+		public float addSingleResult(LocalRayResult rayResult, boolean normalInWorldSpace) {
+			// caller already does the filter on the closestHitFraction
+			assert (rayResult.hitFraction <= closestHitFraction);
+
+			closestHitFraction = rayResult.hitFraction;
+			collisionObject = rayResult.collisionObject;
+			if (normalInWorldSpace) {
+				hitNormalWorld.set(rayResult.hitNormalLocal);
+			}
+			else {
+				// need to transform normal into worldspace
+				hitNormalWorld.set(rayResult.hitNormalLocal);
+				collisionObject.getWorldTransform().basis.transform(hitNormalWorld);
+			}
+
+			VectorUtil.setInterpolate3(hitPointWorld, rayFromWorld, rayToWorld, rayResult.hitFraction);
+			return rayResult.hitFraction;
+		}
+	}
+	
+	public static class LocalConvexResult {
+		public CollisionObject hitCollisionObject;
+		public LocalShapeInfo localShapeInfo;
+		public final Vector3f hitNormalLocal = new Vector3f();
+		public final Vector3f hitPointLocal = new Vector3f();
+		public float hitFraction;
+
+		public LocalConvexResult(CollisionObject hitCollisionObject, LocalShapeInfo localShapeInfo, Vector3f hitNormalLocal, Vector3f hitPointLocal, float hitFraction) {
+			this.hitCollisionObject = hitCollisionObject;
+			this.localShapeInfo = localShapeInfo;
+			this.hitNormalLocal.set(hitNormalLocal);
+			this.hitPointLocal.set(hitPointLocal);
+			this.hitFraction = hitFraction;
+		}
+	}
+	
+	public static abstract class ConvexResultCallback {
+		public float closestHitFraction = 1f;
+		
+		public boolean hasHit() {
+			return (closestHitFraction < 1f);
+		}
+		
+		public abstract float addSingleResult(LocalConvexResult convexResult, boolean normalInWorldSpace);
+	}
+	
+	public static class ClosestConvexResultCallback extends ConvexResultCallback {
+		public final Vector3f convexFromWorld = new Vector3f(); // used to calculate hitPointWorld from hitFraction
+		public final Vector3f convexToWorld = new Vector3f();
+		public final Vector3f hitNormalWorld = new Vector3f();
+		public final Vector3f hitPointWorld = new Vector3f();
+		public CollisionObject hitCollisionObject;
+
+		@Override
+		public float addSingleResult(LocalConvexResult convexResult, boolean normalInWorldSpace) {
+			// caller already does the filter on the m_closestHitFraction
+			assert (convexResult.hitFraction <= closestHitFraction);
+
+			closestHitFraction = convexResult.hitFraction;
+			hitCollisionObject = convexResult.hitCollisionObject;
+			if (normalInWorldSpace) {
+				hitNormalWorld.set(convexResult.hitNormalLocal);
+			}
+			else {
+				// need to transform normal into worldspace
+				hitNormalWorld.set(convexResult.hitNormalLocal);
+				hitCollisionObject.getWorldTransform().basis.transform(hitNormalWorld);
+			}
+
+			hitPointWorld.set(convexResult.hitPointLocal);
+			return convexResult.hitFraction;
+		}
+	}
+	
+	private static class BridgeTriangleRaycastCallback extends TriangleRaycastCallback {
+		public RayResultCallback resultCallback;
+		public CollisionObject collisionObject;
+		public ConcaveShape triangleMesh;
+
+		public BridgeTriangleRaycastCallback(Vector3f from, Vector3f to, RayResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh) {
+			super(from, to);
+			this.resultCallback = resultCallback;
+			this.collisionObject = collisionObject;
+			this.triangleMesh = triangleMesh;
+		}
+	
+		public float reportHit(Vector3f hitNormalLocal, float hitFraction, int partId, int triangleIndex) {
+			LocalShapeInfo shapeInfo = new LocalShapeInfo();
+			shapeInfo.shapePart = partId;
+			shapeInfo.triangleIndex = triangleIndex;
+
+			LocalRayResult rayResult = new LocalRayResult(collisionObject, shapeInfo, hitNormalLocal, hitFraction);
+
+			boolean normalInWorldSpace = false;
+			return resultCallback.addSingleResult(rayResult, normalInWorldSpace);
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/CompoundCollisionAlgorithm.java b/src/jbullet/src/javabullet/collision/dispatch/CompoundCollisionAlgorithm.java
new file mode 100644
index 0000000..7c36d4b
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/CompoundCollisionAlgorithm.java
@@ -0,0 +1,196 @@
+/*
+ * 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.collision.dispatch;
+
+import java.util.ArrayList;
+import java.util.List;
+import javabullet.collision.broadphase.CollisionAlgorithm;
+import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo;
+import javabullet.collision.broadphase.DispatcherInfo;
+import javabullet.collision.shapes.CollisionShape;
+import javabullet.collision.shapes.CompoundShape;
+import javabullet.linearmath.Transform;
+
+/**
+ * CompoundCollisionAlgorithm  supports collision between CompoundCollisionShapes and other collision shapes.
+ * Place holder, not fully implemented yet.
+ * 
+ * @author jezek2
+ */
+public class CompoundCollisionAlgorithm extends CollisionAlgorithm {
+
+	private final List<CollisionAlgorithm> childCollisionAlgorithms = new ArrayList<CollisionAlgorithm>();
+	private boolean isSwapped;
+	
+	public CompoundCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, boolean isSwapped) {
+		super(ci);
+		this.isSwapped = isSwapped;
+
+		CollisionObject colObj = isSwapped ? body1 : body0;
+		CollisionObject otherObj = isSwapped ? body0 : body1;
+		assert (colObj.getCollisionShape().isCompound());
+
+		CompoundShape compoundShape = (CompoundShape) colObj.getCollisionShape();
+		int numChildren = compoundShape.getNumChildShapes();
+		int i;
+
+		//childCollisionAlgorithms.resize(numChildren);
+		for (i = 0; i < numChildren; i++) {
+			CollisionShape childShape = compoundShape.getChildShape(i);
+			CollisionShape orgShape = colObj.getCollisionShape();
+			colObj.setCollisionShape(childShape);
+			childCollisionAlgorithms.add(ci.dispatcher1.findAlgorithm(colObj, otherObj));
+			colObj.setCollisionShape(orgShape);
+		}
+	}
+
+	@Override
+	public void destroy() {
+		int numChildren = childCollisionAlgorithms.size();
+		int i;
+		for (i = 0; i < numChildren; i++) {
+			childCollisionAlgorithms.get(i).destroy();
+			//m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
+		}
+	}
+	
+	@Override
+	public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
+		stack.transforms.push();
+		try {
+			CollisionObject colObj = isSwapped ? body1 : body0;
+			CollisionObject otherObj = isSwapped ? body0 : body1;
+
+			assert (colObj.getCollisionShape().isCompound());
+			CompoundShape compoundShape = (CompoundShape) colObj.getCollisionShape();
+
+			// We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
+			// If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
+			// given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
+			// determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
+			// then use each overlapping node AABB against Tree0
+			// and vise versa.
+
+			Transform tmpTrans = stack.transforms.get();
+			Transform orgTrans = stack.transforms.get();
+
+			int numChildren = childCollisionAlgorithms.size();
+			int i;
+			for (i = 0; i < numChildren; i++) {
+				// temporarily exchange parent btCollisionShape with childShape, and recurse
+				CollisionShape childShape = compoundShape.getChildShape(i);
+
+				// backup
+				orgTrans.set(colObj.getWorldTransform());
+				CollisionShape orgShape = colObj.getCollisionShape();
+
+				Transform childTrans = compoundShape.getChildTransform(i);
+				//btTransform	newChildWorldTrans = orgTrans*childTrans ;
+				tmpTrans.set(orgTrans);
+				tmpTrans.mul(childTrans);
+				colObj.setWorldTransform(tmpTrans);
+				// the contactpoint is still projected back using the original inverted worldtrans
+				colObj.setCollisionShape(childShape);
+				childCollisionAlgorithms.get(i).processCollision(colObj, otherObj, dispatchInfo, resultOut);
+				// revert back
+				colObj.setCollisionShape(orgShape);
+				colObj.setWorldTransform(orgTrans);
+			}
+		}
+		finally {
+			stack.transforms.pop();
+		}
+	}
+
+	@Override
+	public float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
+		stack.transforms.push();
+		try {
+			CollisionObject colObj = isSwapped ? body1 : body0;
+			CollisionObject otherObj = isSwapped ? body0 : body1;
+
+			assert (colObj.getCollisionShape().isCompound());
+
+			CompoundShape compoundShape = (CompoundShape) colObj.getCollisionShape();
+
+			// We will use the OptimizedBVH, AABB tree to cull potential child-overlaps
+			// If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals
+			// given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means:
+			// determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1
+			// then use each overlapping node AABB against Tree0
+			// and vise versa.
+
+			Transform tmpTrans = stack.transforms.get();
+			Transform orgTrans = stack.transforms.get();
+			float hitFraction = 1f;
+
+			int numChildren = childCollisionAlgorithms.size();
+			int i;
+			for (i = 0; i < numChildren; i++) {
+				// temporarily exchange parent btCollisionShape with childShape, and recurse
+				CollisionShape childShape = compoundShape.getChildShape(i);
+
+				// backup
+				orgTrans.set(colObj.getWorldTransform());
+				CollisionShape orgShape = colObj.getCollisionShape();
+
+				Transform childTrans = compoundShape.getChildTransform(i);
+				//btTransform	newChildWorldTrans = orgTrans*childTrans ;
+				tmpTrans.set(orgTrans);
+				tmpTrans.mul(childTrans);
+				colObj.setWorldTransform(tmpTrans);
+
+				colObj.setCollisionShape(childShape);
+				float frac = childCollisionAlgorithms.get(i).calculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut);
+				if (frac < hitFraction) {
+					hitFraction = frac;
+				}
+				// revert back
+				colObj.setCollisionShape(orgShape);
+				colObj.setWorldTransform(orgTrans);
+			}
+			return hitFraction;
+		}
+		finally {
+			stack.transforms.pop();
+		}
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	public static final CollisionAlgorithmCreateFunc createFunc = new CollisionAlgorithmCreateFunc() {
+		@Override
+		public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) {
+			return new CompoundCollisionAlgorithm(ci, body0, body1, false);
+		}
+	};
+	
+	public static final CollisionAlgorithmCreateFunc swappedCreateFunc = new CollisionAlgorithmCreateFunc() {
+		@Override
+		public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) {
+			return new CompoundCollisionAlgorithm(ci, body0, body1, true);
+		}
+	};
+
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/ConvexConcaveCollisionAlgorithm.java b/src/jbullet/src/javabullet/collision/dispatch/ConvexConcaveCollisionAlgorithm.java
new file mode 100644
index 0000000..ed2df6d
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/ConvexConcaveCollisionAlgorithm.java
@@ -0,0 +1,226 @@
+/*
+ * 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.collision.dispatch;
+
+import javabullet.collision.broadphase.CollisionAlgorithm;
+import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo;
+import javabullet.collision.broadphase.DispatcherInfo;
+import javabullet.collision.narrowphase.ConvexCast.CastResult;
+import javabullet.collision.narrowphase.SubsimplexConvexCast;
+import javabullet.collision.narrowphase.VoronoiSimplexSolver;
+import javabullet.collision.shapes.ConcaveShape;
+import javabullet.collision.shapes.SphereShape;
+import javabullet.collision.shapes.TriangleCallback;
+import javabullet.collision.shapes.TriangleShape;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ * ConvexConcaveCollisionAlgorithm supports collision between convex shapes
+ * and (concave) trianges meshes.
+ * 
+ * @author jezek2
+ */
+public class ConvexConcaveCollisionAlgorithm extends CollisionAlgorithm {
+
+	private boolean isSwapped;
+	private ConvexTriangleCallback btConvexTriangleCallback;
+	
+	public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, boolean isSwapped) {
+		super(ci);
+		this.isSwapped = isSwapped;
+		this.btConvexTriangleCallback = new ConvexTriangleCallback(dispatcher, body0, body1, isSwapped);
+	}
+	
+	@Override
+	public void destroy() {
+		btConvexTriangleCallback.destroy();
+	}
+
+	@Override
+	public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
+		CollisionObject convexBody = isSwapped ? body1 : body0;
+		CollisionObject triBody = isSwapped ? body0 : body1;
+
+		if (triBody.getCollisionShape().isConcave()) {
+			CollisionObject triOb = triBody;
+			ConcaveShape concaveShape = (ConcaveShape)triOb.getCollisionShape();
+
+			if (convexBody.getCollisionShape().isConvex()) {
+				float collisionMarginTriangle = concaveShape.getMargin();
+
+				resultOut.setPersistentManifold(btConvexTriangleCallback.manifoldPtr);
+				btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
+
+				// Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
+				//m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+
+				btConvexTriangleCallback.manifoldPtr.setBodies(convexBody, triBody);
+
+				concaveShape.processAllTriangles(btConvexTriangleCallback, btConvexTriangleCallback.getAabbMin(), btConvexTriangleCallback.getAabbMax());
+
+				resultOut.refreshContactPoints();
+			}
+		}
+	}
+
+	@Override
+	public float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
+		stack.pushCommonMath();
+		try {
+			Vector3f tmp = stack.vectors.get();
+
+			CollisionObject convexbody = isSwapped ? body1 : body0;
+			CollisionObject triBody = isSwapped ? body0 : body1;
+
+			// quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
+
+			// only perform CCD above a certain threshold, this prevents blocking on the long run
+			// because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
+			tmp.sub(convexbody.getInterpolationWorldTransform().origin, convexbody.getWorldTransform().origin);
+			float squareMot0 = tmp.lengthSquared();
+			if (squareMot0 < convexbody.getCcdSquareMotionThreshold()) {
+				return 1f;
+			}
+
+			//const btVector3& from = convexbody->m_worldTransform.getOrigin();
+			//btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin();
+			//todo: only do if the motion exceeds the 'radius'
+
+			Transform triInv = stack.transforms.get(triBody.getWorldTransform());
+			triInv.inverse();
+
+			Transform convexFromLocal = stack.transforms.get();
+			convexFromLocal.mul(triInv, convexbody.getWorldTransform());
+
+			Transform convexToLocal = stack.transforms.get();
+			convexToLocal.mul(triInv, convexbody.getInterpolationWorldTransform());
+
+			if (triBody.getCollisionShape().isConcave()) {
+				Vector3f rayAabbMin = stack.vectors.get(convexFromLocal.origin);
+				VectorUtil.setMin(rayAabbMin, convexToLocal.origin);
+
+				Vector3f rayAabbMax = stack.vectors.get(convexFromLocal.origin);
+				VectorUtil.setMax(rayAabbMax, convexToLocal.origin);
+
+				float ccdRadius0 = convexbody.getCcdSweptSphereRadius();
+
+				tmp.set(ccdRadius0, ccdRadius0, ccdRadius0);
+				rayAabbMin.sub(tmp);
+				rayAabbMax.add(tmp);
+
+				float curHitFraction = 1f; // is this available?
+				LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, convexbody.getCcdSweptSphereRadius(), curHitFraction);
+
+				raycastCallback.hitFraction = convexbody.getHitFraction();
+
+				CollisionObject concavebody = triBody;
+
+				ConcaveShape triangleMesh = (ConcaveShape)concavebody.getCollisionShape();
+
+				if (triangleMesh != null) {
+					triangleMesh.processAllTriangles(raycastCallback, rayAabbMin, rayAabbMax);
+				}
+
+				if (raycastCallback.hitFraction < convexbody.getHitFraction()) {
+					convexbody.setHitFraction(raycastCallback.hitFraction);
+					return raycastCallback.hitFraction;
+				}
+			}
+
+			return 1f;
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+	
+	public void clearCache() {
+		btConvexTriangleCallback.clearCache();
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	private static class LocalTriangleSphereCastCallback implements TriangleCallback {
+		public final Transform ccdSphereFromTrans = new Transform();
+		public final Transform ccdSphereToTrans = new Transform();
+		public final Transform meshTransform = new Transform();
+
+		public float ccdSphereRadius;
+		public float hitFraction;
+		
+		private final Transform ident = new Transform();
+		
+		public LocalTriangleSphereCastCallback(Transform from, Transform to, float ccdSphereRadius, float hitFraction) {
+			this.ccdSphereFromTrans.set(from);
+			this.ccdSphereToTrans.set(to);
+			this.ccdSphereRadius = ccdSphereRadius;
+			this.hitFraction = hitFraction;
+
+			// JAVA NOTE: moved here from processTriangle
+			ident.setIdentity();
+		}
+		
+		public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) {
+			// do a swept sphere for now
+			
+			//btTransform ident;
+			//ident.setIdentity();
+			
+			CastResult castResult = new CastResult();
+			castResult.fraction = hitFraction;
+			SphereShape pointShape = new SphereShape(ccdSphereRadius);
+			TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);
+			VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
+			SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver);
+			//GjkConvexCast	convexCaster(&pointShape,convexShape,&simplexSolver);
+			//ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
+			//local space?
+
+			if (convexCaster.calcTimeOfImpact(ccdSphereFromTrans, ccdSphereToTrans, ident, ident, castResult)) {
+				if (hitFraction > castResult.fraction) {
+					hitFraction = castResult.fraction;
+				}
+			}
+		}
+	}
+
+	////////////////////////////////////////////////////////////////////////////
+
+	public static class CreateFunc extends CollisionAlgorithmCreateFunc {
+		@Override
+		public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) {
+			return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false);
+		}
+	}
+	
+	public static class SwappedCreateFunc extends CollisionAlgorithmCreateFunc {
+		@Override
+		public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) {
+			return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true);
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/ConvexConvexAlgorithm.java b/src/jbullet/src/javabullet/collision/dispatch/ConvexConvexAlgorithm.java
new file mode 100644
index 0000000..6b674c3
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/ConvexConvexAlgorithm.java
@@ -0,0 +1,254 @@
+/*
+ * 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.collision.dispatch;
+
+import javabullet.BulletPool;
+import javabullet.ObjectPool;
+import javabullet.collision.broadphase.CollisionAlgorithm;
+import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo;
+import javabullet.collision.broadphase.DispatcherInfo;
+import javabullet.collision.narrowphase.ConvexCast;
+import javabullet.collision.narrowphase.ConvexPenetrationDepthSolver;
+import javabullet.collision.narrowphase.DiscreteCollisionDetectorInterface.ClosestPointInput;
+import javabullet.collision.narrowphase.GjkConvexCast;
+import javabullet.collision.narrowphase.GjkPairDetector;
+import javabullet.collision.narrowphase.PersistentManifold;
+import javabullet.collision.narrowphase.SimplexSolverInterface;
+import javabullet.collision.narrowphase.VoronoiSimplexSolver;
+import javabullet.collision.shapes.ConvexShape;
+import javabullet.collision.shapes.SphereShape;
+import javax.vecmath.Vector3f;
+
+/**
+ * ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations.
+ * 
+ * @author jezek2
+ */
+public class ConvexConvexAlgorithm extends CollisionAlgorithm {
+	
+	protected final ObjectPool<ClosestPointInput> pointInputsPool = BulletPool.get(ClosestPointInput.class);
+
+	private GjkPairDetector gjkPairDetector;
+
+	public boolean ownManifold = false;
+	public PersistentManifold manifoldPtr;
+	public boolean lowLevelOfDetail = false;
+	
+	public ConvexConvexAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, SimplexSolverInterface simplexSolver, ConvexPenetrationDepthSolver pdSolver) {
+		super(ci);
+		gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, pdSolver);
+		this.manifoldPtr = mf;
+	}
+	
+	@Override
+	public void destroy() {
+		if (ownManifold) {
+			if (manifoldPtr != null) {
+				dispatcher.releaseManifold(manifoldPtr);
+			}
+		}
+	}
+
+	public void setLowLevelOfDetail(boolean useLowLevel) {
+		this.lowLevelOfDetail = useLowLevel;
+	}
+
+	/**
+	 * Convex-Convex collision algorithm.
+	 */
+	@Override
+	public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
+		if (manifoldPtr == null) {
+			// swapped?
+			manifoldPtr = dispatcher.getNewManifold(body0, body1);
+			ownManifold = true;
+		}
+		resultOut.setPersistentManifold(manifoldPtr);
+
+//	#ifdef USE_BT_GJKEPA
+//		btConvexShape*				shape0(static_cast<btConvexShape*>(body0->getCollisionShape()));
+//		btConvexShape*				shape1(static_cast<btConvexShape*>(body1->getCollisionShape()));
+//		const btScalar				radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/);
+//		btGjkEpaSolver::sResults	results;
+//		if(btGjkEpaSolver::Collide(	shape0,body0->getWorldTransform(),
+//									shape1,body1->getWorldTransform(),
+//									radialmargin,results))
+//			{
+//			dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
+//			resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
+//			}
+//	#else
+
+		ConvexShape min0 = (ConvexShape) body0.getCollisionShape();
+		ConvexShape min1 = (ConvexShape) body1.getCollisionShape();
+
+		ClosestPointInput input = pointInputsPool.get();
+		input.init();
+
+		// JAVA NOTE: original: TODO: if (dispatchInfo.m_useContinuous)
+		gjkPairDetector.setMinkowskiA(min0);
+		gjkPairDetector.setMinkowskiB(min1);
+		input.maximumDistanceSquared = min0.getMargin() + min1.getMargin() + manifoldPtr.getContactBreakingThreshold();
+		input.maximumDistanceSquared *= input.maximumDistanceSquared;
+		//input.m_stackAlloc = dispatchInfo.m_stackAllocator;
+
+		//	input.m_maximumDistanceSquared = btScalar(1e30);
+
+		input.transformA.set(body0.getWorldTransform());
+		input.transformB.set(body1.getWorldTransform());
+
+		gjkPairDetector.getClosestPoints(input, resultOut, dispatchInfo.debugDraw);
+		
+		pointInputsPool.release(input);
+		//	#endif
+
+		if (ownManifold) {
+			resultOut.refreshContactPoints();
+		}
+	}
+
+	private static boolean disableCcd = false;
+	
+	@Override
+	public float calculateTimeOfImpact(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
+		stack.vectors.push();
+		try {
+			Vector3f tmp = stack.vectors.get();
+
+			// Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
+
+			// Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
+			// col0->m_worldTransform,
+			float resultFraction = 1f;
+
+			tmp.sub(col0.getInterpolationWorldTransform().origin, col0.getWorldTransform().origin);
+			float squareMot0 = tmp.lengthSquared();
+
+			tmp.sub(col1.getInterpolationWorldTransform().origin, col1.getWorldTransform().origin);
+			float squareMot1 = tmp.lengthSquared();
+
+			if (squareMot0 < col0.getCcdSquareMotionThreshold() &&
+					squareMot1 < col1.getCcdSquareMotionThreshold()) {
+				return resultFraction;
+			}
+
+			if (disableCcd) {
+				return 1f;
+			}
+
+			// An adhoc way of testing the Continuous Collision Detection algorithms
+			// One object is approximated as a sphere, to simplify things
+			// Starting in penetration should report no time of impact
+			// For proper CCD, better accuracy and handling of 'allowed' penetration should be added
+			// also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
+
+			// Convex0 against sphere for Convex1
+			{
+				ConvexShape convex0 = (ConvexShape) col0.getCollisionShape();
+
+				SphereShape sphere1 = new SphereShape(col1.getCcdSweptSphereRadius()); // todo: allow non-zero sphere sizes, for better approximation
+				ConvexCast.CastResult result = new ConvexCast.CastResult();
+				VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
+				//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+				///Simplification, one object is simplified as a sphere
+				GjkConvexCast ccd1 = new GjkConvexCast(convex0, sphere1, voronoiSimplex);
+				//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+				if (ccd1.calcTimeOfImpact(col0.getWorldTransform(), col0.getInterpolationWorldTransform(),
+						col1.getWorldTransform(), col1.getInterpolationWorldTransform(), result)) {
+					// store result.m_fraction in both bodies
+
+					if (col0.getHitFraction() > result.fraction) {
+						col0.setHitFraction(result.fraction);
+					}
+
+					if (col1.getHitFraction() > result.fraction) {
+						col1.setHitFraction(result.fraction);
+					}
+
+					if (resultFraction > result.fraction) {
+						resultFraction = result.fraction;
+					}
+				}
+			}
+
+			// Sphere (for convex0) against Convex1
+			{
+				ConvexShape convex1 = (ConvexShape) col1.getCollisionShape();
+
+				SphereShape sphere0 = new SphereShape(col0.getCcdSweptSphereRadius()); // todo: allow non-zero sphere sizes, for better approximation
+				ConvexCast.CastResult result = new ConvexCast.CastResult();
+				VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
+				//SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+				///Simplification, one object is simplified as a sphere
+				GjkConvexCast ccd1 = new GjkConvexCast(sphere0, convex1, voronoiSimplex);
+				//ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+				if (ccd1.calcTimeOfImpact(col0.getWorldTransform(), col0.getInterpolationWorldTransform(),
+						col1.getWorldTransform(), col1.getInterpolationWorldTransform(), result)) {
+					//store result.m_fraction in both bodies
+
+					if (col0.getHitFraction() > result.fraction) {
+						col0.setHitFraction(result.fraction);
+					}
+
+					if (col1.getHitFraction() > result.fraction) {
+						col1.setHitFraction(result.fraction);
+					}
+
+					if (resultFraction > result.fraction) {
+						resultFraction = result.fraction;
+					}
+
+				}
+			}
+
+			return resultFraction;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public PersistentManifold getManifold() {
+		return manifoldPtr;
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	public static class CreateFunc extends CollisionAlgorithmCreateFunc {
+		public ConvexPenetrationDepthSolver pdSolver;
+		public SimplexSolverInterface simplexSolver;
+
+		public CreateFunc(SimplexSolverInterface simplexSolver, ConvexPenetrationDepthSolver pdSolver) {
+			this.simplexSolver = simplexSolver;
+			this.pdSolver = pdSolver;
+		}
+
+		@Override
+		public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) {
+			//void* mem = ci.dispatcher1.allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm));
+			return new ConvexConvexAlgorithm(ci.manifold,ci,body0,body1,simplexSolver,pdSolver);
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/ConvexPlaneCollisionAlgorithm.java b/src/jbullet/src/javabullet/collision/dispatch/ConvexPlaneCollisionAlgorithm.java
new file mode 100644
index 0000000..9fad739
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/ConvexPlaneCollisionAlgorithm.java
@@ -0,0 +1,154 @@
+/*
+ * 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.collision.dispatch;
+
+import javabullet.collision.broadphase.CollisionAlgorithm;
+import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo;
+import javabullet.collision.broadphase.DispatcherInfo;
+import javabullet.collision.narrowphase.PersistentManifold;
+import javabullet.collision.shapes.ConvexShape;
+import javabullet.collision.shapes.StaticPlaneShape;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class ConvexPlaneCollisionAlgorithm extends CollisionAlgorithm {
+
+	private boolean ownManifold = false;
+	private PersistentManifold manifoldPtr;
+	private boolean isSwapped;
+	
+	public ConvexPlaneCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0, CollisionObject col1, boolean isSwapped) {
+		super(ci);
+		this.manifoldPtr = mf;
+		this.isSwapped = isSwapped;
+
+		CollisionObject convexObj = isSwapped ? col1 : col0;
+		CollisionObject planeObj = isSwapped ? col0 : col1;
+
+		if (manifoldPtr == null && dispatcher.needsCollision(convexObj, planeObj)) {
+			manifoldPtr = dispatcher.getNewManifold(convexObj, planeObj);
+			ownManifold = true;
+		}
+	}
+	
+	@Override
+	public void destroy() {
+		if (ownManifold) {
+			if (manifoldPtr != null) {
+				dispatcher.releaseManifold(manifoldPtr);
+			}
+		}
+	}
+	
+	
+	@Override
+	public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
+		if (manifoldPtr == null) {
+			return;
+		}
+
+		stack.pushCommonMath();
+		try {
+			CollisionObject convexObj = isSwapped ? body1 : body0;
+			CollisionObject planeObj = isSwapped ? body0 : body1;
+
+			ConvexShape convexShape = (ConvexShape) convexObj.getCollisionShape();
+			StaticPlaneShape planeShape = (StaticPlaneShape) planeObj.getCollisionShape();
+
+			boolean hasCollision = false;
+			Vector3f planeNormal = planeShape.getPlaneNormal();
+			float planeConstant = planeShape.getPlaneConstant();
+
+			Transform planeInConvex = stack.transforms.get();
+			planeInConvex.inverse(convexObj.getWorldTransform());
+			planeInConvex.mul(planeObj.getWorldTransform());
+
+			Transform convexInPlaneTrans = stack.transforms.get();
+			convexInPlaneTrans.inverse(planeObj.getWorldTransform());
+			convexInPlaneTrans.mul(convexObj.getWorldTransform());
+
+			Vector3f tmp = stack.vectors.get();
+			tmp.negate(planeNormal);
+			planeInConvex.basis.transform(tmp);
+
+			Vector3f vtx = stack.vectors.get(convexShape.localGetSupportingVertexWithoutMargin(tmp));
+			Vector3f vtxInPlane = stack.vectors.get(vtx);
+			convexInPlaneTrans.transform(vtxInPlane);
+
+			float distance = (planeNormal.dot(vtxInPlane) - planeConstant) - convexShape.getMargin();
+
+			Vector3f vtxInPlaneProjected = stack.vectors.get();
+			tmp.scale(distance, planeNormal);
+			vtxInPlaneProjected.sub(vtxInPlane, tmp);
+
+			Vector3f vtxInPlaneWorld = stack.vectors.get(vtxInPlaneProjected);
+			planeObj.getWorldTransform().transform(vtxInPlaneWorld);
+
+			hasCollision = distance < manifoldPtr.getContactBreakingThreshold();
+			resultOut.setPersistentManifold(manifoldPtr);
+			if (hasCollision) {
+				// report a contact. internally this will be kept persistent, and contact reduction is done
+				Vector3f normalOnSurfaceB = stack.vectors.get(planeNormal);
+				planeObj.getWorldTransform().basis.transform(normalOnSurfaceB);
+
+				Vector3f pOnB = stack.vectors.get(vtxInPlaneWorld);
+				resultOut.addContactPoint(normalOnSurfaceB, pOnB, distance);
+			}
+			if (ownManifold) {
+				if (manifoldPtr.getNumContacts() != 0) {
+					resultOut.refreshContactPoints();
+				}
+			}
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	@Override
+	public float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
+		// not yet
+		return 1f;
+	}
+
+	////////////////////////////////////////////////////////////////////////////
+	
+	public static class CreateFunc extends CollisionAlgorithmCreateFunc {
+
+		@Override
+		public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) {
+			if (!swapped) {
+				return new ConvexPlaneCollisionAlgorithm(null, ci, body0, body1, false);
+			}
+			else {
+				return new ConvexPlaneCollisionAlgorithm(null, ci, body0, body1, true);
+			}
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/ConvexTriangleCallback.java b/src/jbullet/src/javabullet/collision/dispatch/ConvexTriangleCallback.java
new file mode 100644
index 0000000..948b813
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/ConvexTriangleCallback.java
@@ -0,0 +1,191 @@
+/*
+ * 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.collision.dispatch;
+
+import javabullet.BulletStack;
+import javabullet.collision.broadphase.CollisionAlgorithm;
+import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo;
+import javabullet.collision.broadphase.Dispatcher;
+import javabullet.collision.broadphase.DispatcherInfo;
+import javabullet.collision.narrowphase.PersistentManifold;
+import javabullet.collision.shapes.CollisionShape;
+import javabullet.collision.shapes.TriangleCallback;
+import javabullet.collision.shapes.TriangleShape;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * For each triangle in the concave mesh that overlaps with the AABB of a convex
+ * (convexProxy field), processTriangle is called.
+ * 
+ * @author jezek2
+ */
+class ConvexTriangleCallback implements TriangleCallback {
+
+	protected final BulletStack stack = BulletStack.get();
+	
+	private CollisionObject convexBody;
+	private CollisionObject triBody;
+
+	private final Vector3f aabbMin = new Vector3f();
+	private final Vector3f aabbMax = new Vector3f();
+
+	private ManifoldResult resultOut;
+
+	private Dispatcher dispatcher;
+	private DispatcherInfo dispatchInfoPtr;
+	private float collisionMarginTriangle;
+	
+	public int triangleCount;
+	public PersistentManifold manifoldPtr;
+	
+	public ConvexTriangleCallback(Dispatcher dispatcher, CollisionObject body0, CollisionObject body1, boolean isSwapped) {
+		this.dispatcher = dispatcher;
+		this.dispatchInfoPtr = null;
+
+		convexBody = isSwapped ? body1 : body0;
+		triBody = isSwapped ? body0 : body1;
+
+		//
+		// create the manifold from the dispatcher 'manifold pool'
+		//
+		manifoldPtr = dispatcher.getNewManifold(convexBody, triBody);
+
+		clearCache();
+	}
+	
+	public void destroy() {
+		clearCache();
+		dispatcher.releaseManifold(manifoldPtr);
+	}
+
+	public void setTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
+		stack.pushCommonMath();
+		try {
+			this.dispatchInfoPtr = dispatchInfo;
+			this.collisionMarginTriangle = collisionMarginTriangle;
+			this.resultOut = resultOut;
+
+			// recalc aabbs
+			Transform convexInTriangleSpace = stack.transforms.get();
+
+			convexInTriangleSpace.inverse(triBody.getWorldTransform());
+			convexInTriangleSpace.mul(convexBody.getWorldTransform());
+
+			CollisionShape convexShape = (CollisionShape)convexBody.getCollisionShape();
+			//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
+			convexShape.getAabb(convexInTriangleSpace, aabbMin, aabbMax);
+			float extraMargin = collisionMarginTriangle;
+			Vector3f extra = stack.vectors.get(extraMargin, extraMargin, extraMargin);
+
+			aabbMax.add(extra);
+			aabbMin.sub(extra);
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	private CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo();
+	private TriangleShape tm = new TriangleShape();
+	
+	public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) {
+		stack.vectors.push();
+		try {
+			// just for debugging purposes
+			//printf("triangle %d",m_triangleCount++);
+
+			// aabb filter is already applied!	
+
+			ci.dispatcher1 = dispatcher;
+
+			CollisionObject ob = (CollisionObject) triBody;
+
+			// debug drawing of the overlapping triangles
+			if (dispatchInfoPtr != null && dispatchInfoPtr.debugDraw != null && dispatchInfoPtr.debugDraw.getDebugMode() > 0) {
+				Vector3f color = stack.vectors.get(255, 255, 0);
+				Transform tr = ob.getWorldTransform();
+
+				Vector3f tmp1 = stack.vectors.get();
+				Vector3f tmp2 = stack.vectors.get();
+
+				tmp1.set(triangle[0]); tr.transform(tmp1);
+				tmp2.set(triangle[1]); tr.transform(tmp2);
+				dispatchInfoPtr.debugDraw.drawLine(tmp1, tmp2, color);
+
+				tmp1.set(triangle[1]); tr.transform(tmp1);
+				tmp2.set(triangle[2]); tr.transform(tmp2);
+				dispatchInfoPtr.debugDraw.drawLine(tmp1, tmp2, color);
+
+				tmp1.set(triangle[2]); tr.transform(tmp1);
+				tmp2.set(triangle[0]); tr.transform(tmp2);
+				dispatchInfoPtr.debugDraw.drawLine(tmp1, tmp2, color);
+
+				//btVector3 center = triangle[0] + triangle[1]+triangle[2];
+				//center *= btScalar(0.333333);
+				//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color);
+				//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color);
+				//m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color);
+			}
+
+			//btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject);
+
+			if (convexBody.getCollisionShape().isConvex()) {
+				tm.init(triangle[0], triangle[1], triangle[2]);
+				tm.setMargin(collisionMarginTriangle);
+
+				CollisionShape tmpShape = ob.getCollisionShape();
+				ob.setCollisionShape(tm);
+
+				CollisionAlgorithm colAlgo = ci.dispatcher1.findAlgorithm(convexBody, triBody, manifoldPtr);
+				// this should use the btDispatcher, so the actual registered algorithm is used
+				//		btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody);
+
+				resultOut.setShapeIdentifiers(-1, -1, partId, triangleIndex);
+				//cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex);
+				//cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut);
+				colAlgo.processCollision(convexBody, triBody, dispatchInfoPtr, resultOut);
+				colAlgo.destroy();
+				//ci.dispatcher1.freeCollisionAlgorithm(colAlgo);
+				ob.setCollisionShape(tmpShape);
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void clearCache() {
+		dispatcher.clearManifold(manifoldPtr);
+	}
+
+	public Vector3f getAabbMin() {
+		return aabbMin;
+	}
+
+	public Vector3f getAabbMax() {
+		return aabbMax;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/DefaultCollisionConfiguration.java b/src/jbullet/src/javabullet/collision/dispatch/DefaultCollisionConfiguration.java
new file mode 100644
index 0000000..f38114e
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/DefaultCollisionConfiguration.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.collision.dispatch;
+
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.collision.narrowphase.GjkEpaPenetrationDepthSolver;
+import javabullet.collision.narrowphase.VoronoiSimplexSolver;
+import static javabullet.collision.broadphase.BroadphaseNativeType.*;
+
+/**
+ * CollisionConfiguration allows to configure Bullet collision detection
+ * stack allocator, pool memory allocators
+ * todo: describe the meaning
+ * 
+ * @author jezek2
+ */
+public class DefaultCollisionConfiguration extends CollisionConfiguration {
+
+	//default simplex/penetration depth solvers
+	private VoronoiSimplexSolver simplexSolver;
+	private GjkEpaPenetrationDepthSolver pdSolver;
+	
+	//default CreationFunctions, filling the m_doubleDispatch table
+	private CollisionAlgorithmCreateFunc convexConvexCreateFunc;
+	private CollisionAlgorithmCreateFunc convexConcaveCreateFunc;
+	private CollisionAlgorithmCreateFunc swappedConvexConcaveCreateFunc;
+	private CollisionAlgorithmCreateFunc compoundCreateFunc;
+	private CollisionAlgorithmCreateFunc swappedCompoundCreateFunc;
+	private CollisionAlgorithmCreateFunc emptyCreateFunc;
+	private CollisionAlgorithmCreateFunc sphereSphereCF;
+	private CollisionAlgorithmCreateFunc sphereBoxCF;
+	private CollisionAlgorithmCreateFunc boxSphereCF;
+	private CollisionAlgorithmCreateFunc sphereTriangleCF;
+	private CollisionAlgorithmCreateFunc triangleSphereCF;
+	private CollisionAlgorithmCreateFunc planeConvexCF;
+	private CollisionAlgorithmCreateFunc convexPlaneCF;
+	
+	public DefaultCollisionConfiguration() {
+		simplexSolver = new VoronoiSimplexSolver();
+		pdSolver = new GjkEpaPenetrationDepthSolver();
+
+		/*
+		//default CreationFunctions, filling the m_doubleDispatch table
+		*/
+		convexConvexCreateFunc = new ConvexConvexAlgorithm.CreateFunc(simplexSolver, pdSolver);
+		convexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.CreateFunc();
+		swappedConvexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.SwappedCreateFunc();
+		compoundCreateFunc = CompoundCollisionAlgorithm.createFunc;
+		swappedCompoundCreateFunc = CompoundCollisionAlgorithm.swappedCreateFunc;
+		emptyCreateFunc = EmptyAlgorithm.createFunc;
+
+		sphereSphereCF = SphereSphereCollisionAlgorithm.createFunc;
+		/*
+		m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc;
+		m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc;
+		m_boxSphereCF->m_swapped = true;
+		m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
+		m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc;
+		m_triangleSphereCF->m_swapped = true;
+		*/
+
+		// convex versus plane
+		convexPlaneCF = new ConvexPlaneCollisionAlgorithm.CreateFunc();
+		planeConvexCF = new ConvexPlaneCollisionAlgorithm.CreateFunc();
+		planeConvexCF.swapped = true;
+
+		/*
+		///calculate maximum element size, big enough to fit any collision algorithm in the memory pool
+		int maxSize = sizeof(btConvexConvexAlgorithm);
+		int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm);
+		int maxSize3 = sizeof(btCompoundCollisionAlgorithm);
+		int maxSize4 = sizeof(btEmptyAlgorithm);
+
+		int	collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2);
+		collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3);
+		collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4);
+
+		if (stackAlloc)
+		{
+			m_ownsStackAllocator = false;
+			this->m_stackAlloc = stackAlloc;
+		} else
+		{
+			m_ownsStackAllocator = true;
+			void* mem = btAlignedAlloc(sizeof(btStackAlloc),16);
+			m_stackAlloc = new(mem)btStackAlloc(DEFAULT_STACK_ALLOCATOR_SIZE);
+		}
+
+		if (persistentManifoldPool)
+		{
+			m_ownsPersistentManifoldPool = false;
+			m_persistentManifoldPool = persistentManifoldPool;
+		} else
+		{
+			m_ownsPersistentManifoldPool = true;
+			void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
+			m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),DEFAULT_MAX_OVERLAPPING_PAIRS);
+		}
+
+		if (collisionAlgorithmPool)
+		{
+			m_ownsCollisionAlgorithmPool = false;
+			m_collisionAlgorithmPool = collisionAlgorithmPool;
+		} else
+		{
+			m_ownsCollisionAlgorithmPool = true;
+			void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16);
+			m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,DEFAULT_MAX_OVERLAPPING_PAIRS);
+		}
+		*/
+	}
+	
+	@Override
+	public CollisionAlgorithmCreateFunc getCollisionAlgorithmCreateFunc(BroadphaseNativeType proxyType0, BroadphaseNativeType proxyType1) {
+		if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) {
+			return sphereSphereCF;
+		}
+
+		/*
+		if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE))
+		{
+			return	m_sphereBoxCF;
+		}
+
+		if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+		{
+			return	m_boxSphereCF;
+		}
+
+		if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE))
+		{
+			return	m_sphereTriangleCF;
+		}
+
+		if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE  ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE))
+		{
+			return	m_triangleSphereCF;
+		}
+		*/
+
+		if (proxyType0.isConvex() && (proxyType1 == STATIC_PLANE_PROXYTYPE))
+		{
+			return convexPlaneCF;
+		}
+
+		if (proxyType1.isConvex() && (proxyType0 == STATIC_PLANE_PROXYTYPE))
+		{
+			return planeConvexCF;
+		}
+
+		if (proxyType0.isConvex() && proxyType1.isConvex()) {
+			return convexConvexCreateFunc;
+		}
+
+		if (proxyType0.isConvex() && proxyType1.isConcave()) {
+			return convexConcaveCreateFunc;
+		}
+
+		if (proxyType1.isConvex() && proxyType0.isConcave()) {
+			return swappedConvexConcaveCreateFunc;
+		}
+
+		if (proxyType0.isCompound()) {
+			return compoundCreateFunc;
+		}
+		else {
+			if (proxyType1.isCompound()) {
+				return swappedCompoundCreateFunc;
+			}
+		}
+
+		// failed to find an algorithm
+		return emptyCreateFunc;
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/EmptyAlgorithm.java b/src/jbullet/src/javabullet/collision/dispatch/EmptyAlgorithm.java
new file mode 100644
index 0000000..470ad21
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/EmptyAlgorithm.java
@@ -0,0 +1,62 @@
+/*
+ * 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.collision.dispatch;
+
+import javabullet.collision.broadphase.CollisionAlgorithm;
+import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo;
+import javabullet.collision.broadphase.DispatcherInfo;
+
+/**
+ *
+ * @author jezek2
+ */
+public class EmptyAlgorithm extends CollisionAlgorithm {
+
+	public EmptyAlgorithm(CollisionAlgorithmConstructionInfo ci) {
+		super(ci);
+	}
+
+	@Override
+	public void destroy() {
+	}
+
+	@Override
+	public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
+	}
+
+	@Override
+	public float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
+		return 1f;
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	public static final CollisionAlgorithmCreateFunc createFunc = new CollisionAlgorithmCreateFunc() {
+		@Override
+		public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) {
+			return new EmptyAlgorithm(ci);
+		}
+	};
+
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/ManifoldResult.java b/src/jbullet/src/javabullet/collision/dispatch/ManifoldResult.java
new file mode 100644
index 0000000..38cff45
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/ManifoldResult.java
@@ -0,0 +1,186 @@
+/*
+ * 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.collision.dispatch;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletPool;
+import javabullet.BulletStack;
+import javabullet.ObjectPool;
+import javabullet.collision.narrowphase.DiscreteCollisionDetectorInterface;
+import javabullet.collision.narrowphase.ManifoldPoint;
+import javabullet.collision.narrowphase.PersistentManifold;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * ManifoldResult is a helper class to manage contact results.
+ * 
+ * @author jezek2
+ */
+public class ManifoldResult implements DiscreteCollisionDetectorInterface.Result {
+
+	protected final BulletStack stack = BulletStack.get();
+	protected final ObjectPool<ManifoldPoint> pointsPool = BulletPool.get(ManifoldPoint.class);
+	
+	private PersistentManifold manifoldPtr;
+
+	// we need this for compounds
+	private final Transform rootTransA = new Transform();
+	private final Transform rootTransB = new Transform();
+	private CollisionObject body0;
+	private CollisionObject body1;
+	private int partId0;
+	private int partId1;
+	private int index0;
+	private int index1;
+
+	public ManifoldResult() {
+	}
+
+	public ManifoldResult(CollisionObject body0, CollisionObject body1) {
+		init(body0, body1);
+	}
+
+	public void init(CollisionObject body0, CollisionObject body1) {
+		this.body0 = body0;
+		this.body1 = body1;
+		this.rootTransA.set(body0.getWorldTransform());
+		this.rootTransB.set(body1.getWorldTransform());
+	}
+
+	public PersistentManifold getPersistentManifold() {
+		return manifoldPtr;
+	}
+
+	public void setPersistentManifold(PersistentManifold manifoldPtr) {
+		this.manifoldPtr = manifoldPtr;
+	}
+
+	public void setShapeIdentifiers(int partId0, int index0, int partId1, int index1) {
+		this.partId0 = partId0;
+		this.partId1 = partId1;
+		this.index0 = index0;
+		this.index1 = index1;
+	}
+
+	public void addContactPoint(Vector3f normalOnBInWorld, Vector3f pointInWorld, float depth) {
+		assert (manifoldPtr != null);
+		//order in manifold needs to match
+
+		if (depth > manifoldPtr.getContactBreakingThreshold()) {
+			return;
+		}
+
+		stack.vectors.push();
+		try {
+			boolean isSwapped = manifoldPtr.getBody0() != body0;
+
+			Vector3f pointA = stack.vectors.get();
+			pointA.scaleAdd(depth, normalOnBInWorld, pointInWorld);
+
+			Vector3f localA = stack.vectors.get();
+			Vector3f localB = stack.vectors.get();
+
+			if (isSwapped) {
+				rootTransB.invXform(pointA, localA);
+				rootTransA.invXform(pointInWorld, localB);
+			}
+			else {
+				rootTransA.invXform(pointA, localA);
+				rootTransB.invXform(pointInWorld, localB);
+			}
+
+			ManifoldPoint newPt = pointsPool.get();
+			newPt.init(localA, localB, normalOnBInWorld, depth);
+			
+			newPt.positionWorldOnA.set(pointA);
+			newPt.positionWorldOnB.set(pointInWorld);
+
+			int insertIndex = manifoldPtr.getCacheEntry(newPt);
+
+			newPt.combinedFriction = calculateCombinedFriction(body0, body1);
+			newPt.combinedRestitution = calculateCombinedRestitution(body0, body1);
+
+
+			/// todo, check this for any side effects
+			if (insertIndex >= 0) {
+				//const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex);
+				manifoldPtr.replaceContactPoint(newPt, insertIndex);
+			}
+			else {
+				manifoldPtr.addManifoldPoint(newPt);
+			}
+
+			// User can override friction and/or restitution
+			if (BulletGlobals.gContactAddedCallback != null &&
+					// and if either of the two bodies requires custom material
+					((body0.getCollisionFlags() & CollisionFlags.CUSTOM_MATERIAL_CALLBACK) != 0 ||
+					(body1.getCollisionFlags() & CollisionFlags.CUSTOM_MATERIAL_CALLBACK) != 0)) {
+				//experimental feature info, for per-triangle material etc.
+				CollisionObject obj0 = isSwapped ? body1 : body0;
+				CollisionObject obj1 = isSwapped ? body0 : body1;
+				BulletGlobals.gContactAddedCallback.invoke(newPt, obj0, partId0, index0, obj1, partId1, index1);
+			}
+			
+			pointsPool.release(newPt);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
+	private static float calculateCombinedFriction(CollisionObject body0, CollisionObject body1) {
+		float friction = body0.getFriction() * body1.getFriction();
+
+		float MAX_FRICTION = 10f;
+		if (friction < -MAX_FRICTION) {
+			friction = -MAX_FRICTION;
+		}
+		if (friction > MAX_FRICTION) {
+			friction = MAX_FRICTION;
+		}
+		return friction;
+	}
+
+	private static float calculateCombinedRestitution(CollisionObject body0, CollisionObject body1) {
+		return body0.getRestitution() * body1.getRestitution();
+	}
+
+	public void refreshContactPoints() {
+		assert (manifoldPtr != null);
+		if (manifoldPtr.getNumContacts() == 0) {
+			return;
+		}
+
+		boolean isSwapped = manifoldPtr.getBody0() != body0;
+
+		if (isSwapped) {
+			manifoldPtr.refreshContactPoints(rootTransB, rootTransA);
+		}
+		else {
+			manifoldPtr.refreshContactPoints(rootTransA, rootTransB);
+		}
+	}
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/NearCallback.java b/src/jbullet/src/javabullet/collision/dispatch/NearCallback.java
new file mode 100644
index 0000000..b695073
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/NearCallback.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.collision.dispatch;
+
+import javabullet.collision.broadphase.BroadphasePair;
+import javabullet.collision.broadphase.DispatcherInfo;
+
+/**
+ *
+ * @author jezek2
+ */
+public interface NearCallback {
+
+	public void invoke(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/SimulationIslandManager.java b/src/jbullet/src/javabullet/collision/dispatch/SimulationIslandManager.java
new file mode 100644
index 0000000..a760bff
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/SimulationIslandManager.java
@@ -0,0 +1,335 @@
+/*
+ * 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.collision.dispatch;
+
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+import javabullet.BulletGlobals;
+import javabullet.collision.broadphase.BroadphasePair;
+import javabullet.collision.broadphase.Dispatcher;
+import javabullet.collision.narrowphase.PersistentManifold;
+import javabullet.linearmath.MiscUtil;
+import javabullet.util.HashUtil.IMap;
+import javabullet.util.HashUtil.IObjectProcedure;
+
+/**
+ * SimulationIslandManager creates and handles simulation islands, using UnionFind.
+ * 
+ * @author jezek2
+ */
+public class SimulationIslandManager {
+
+	private final UnionFind unionFind = new UnionFind();
+
+	private final List<PersistentManifold> islandmanifold = new ArrayList<PersistentManifold>();
+	private final List<CollisionObject> islandBodies = new ArrayList<CollisionObject>();
+	
+	public void initUnionFind(int n) {
+		unionFind.reset(n);
+	}
+	
+	public UnionFind getUnionFind() {
+		return unionFind;
+	}
+	
+	private class FindUnionsCallback implements IObjectProcedure<BroadphasePair> {
+		public boolean execute(BroadphasePair collisionPair) {
+			CollisionObject colObj0 = (CollisionObject) collisionPair.pProxy0.clientObject;
+			CollisionObject colObj1 = (CollisionObject) collisionPair.pProxy1.clientObject;
+
+			if (((colObj0 != null) && ((colObj0).mergesSimulationIslands())) &&
+					((colObj1 != null) && ((colObj1).mergesSimulationIslands()))) {
+				unionFind.unite((colObj0).getIslandTag(), (colObj1).getIslandTag());
+			}
+			return true;
+		}
+	}
+	
+	private FindUnionsCallback findUnionsCallback = new FindUnionsCallback();
+
+	public void findUnions(Dispatcher dispatcher, CollisionWorld colWorld) {
+		IMap<BroadphasePair,BroadphasePair> pairPtr = colWorld.getPairCache().getOverlappingPairArray();
+		pairPtr.forEachValue(findUnionsCallback);
+	}
+
+	public void updateActivationState(CollisionWorld colWorld, Dispatcher dispatcher) {
+		initUnionFind(colWorld.getCollisionObjectArray().size());
+
+		// put the index into m_controllers into m_tag	
+		{
+			int index = 0;
+			int i;
+			for (i = 0; i < colWorld.getCollisionObjectArray().size(); i++) {
+				CollisionObject collisionObject = colWorld.getCollisionObjectArray().get(i);
+				collisionObject.setIslandTag(index);
+				collisionObject.setCompanionId(-1);
+				collisionObject.setHitFraction(1f);
+				index++;
+			}
+		}
+		// do the union find
+
+		findUnions(dispatcher, colWorld);
+	}
+	
+	public void storeIslandActivationState(CollisionWorld colWorld) {
+		// put the islandId ('find' value) into m_tag	
+		{
+			int index = 0;
+			int i;
+			for (i = 0; i < colWorld.getCollisionObjectArray().size(); i++) {
+				CollisionObject collisionObject = colWorld.getCollisionObjectArray().get(i);
+				if (collisionObject.mergesSimulationIslands()) {
+					collisionObject.setIslandTag(unionFind.find(index));
+					collisionObject.setCompanionId(-1);
+				}
+				else {
+					collisionObject.setIslandTag(-1);
+					collisionObject.setCompanionId(-2);
+				}
+				index++;
+			}
+		}
+	}
+	
+	private static int getIslandId(PersistentManifold lhs) {
+		int islandId;
+		CollisionObject rcolObj0 = (CollisionObject) lhs.getBody0();
+		CollisionObject rcolObj1 = (CollisionObject) lhs.getBody1();
+		islandId = rcolObj0.getIslandTag() >= 0? rcolObj0.getIslandTag() : rcolObj1.getIslandTag();
+		return islandId;
+	}
+
+	public void buildAndProcessIslands(Dispatcher dispatcher, List<CollisionObject> collisionObjects, IslandCallback callback) {
+		BulletGlobals.pushProfile("islandUnionFindAndHeapSort");
+		try {
+			// we are going to sort the unionfind array, and store the element id in the size
+			// afterwards, we clean unionfind, to make sure no-one uses it anymore
+
+			getUnionFind().sortIslands();
+			int numElem = getUnionFind().getNumElements();
+
+			int endIslandIndex = 1;
+			int startIslandIndex;
+
+			// update the sleeping state for bodies, if all are sleeping
+			for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) {
+				int islandId = getUnionFind().getElement(startIslandIndex).id;
+				for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).id == islandId); endIslandIndex++) {
+				}
+
+				//int numSleeping = 0;
+
+				boolean allSleeping = true;
+
+				int idx;
+				for (idx = startIslandIndex; idx < endIslandIndex; idx++) {
+					int i = getUnionFind().getElement(idx).sz;
+
+					CollisionObject colObj0 = collisionObjects.get(i);
+					if ((colObj0.getIslandTag() != islandId) && (colObj0.getIslandTag() != -1)) {
+						System.err.println("error in island management\n");
+					}
+
+					assert ((colObj0.getIslandTag() == islandId) || (colObj0.getIslandTag() == -1));
+					if (colObj0.getIslandTag() == islandId) {
+						if (colObj0.getActivationState() == CollisionObject.ACTIVE_TAG) {
+							allSleeping = false;
+						}
+						if (colObj0.getActivationState() == CollisionObject.DISABLE_DEACTIVATION) {
+							allSleeping = false;
+						}
+					}
+				}
+
+
+				if (allSleeping) {
+					//int idx;
+					for (idx = startIslandIndex; idx < endIslandIndex; idx++) {
+						int i = getUnionFind().getElement(idx).sz;
+						CollisionObject colObj0 = collisionObjects.get(i);
+						if ((colObj0.getIslandTag() != islandId) && (colObj0.getIslandTag() != -1)) {
+							System.err.println("error in island management\n");
+						}
+
+						assert ((colObj0.getIslandTag() == islandId) || (colObj0.getIslandTag() == -1));
+
+						if (colObj0.getIslandTag() == islandId) {
+							colObj0.setActivationState(CollisionObject.ISLAND_SLEEPING);
+						}
+					}
+				}
+				else {
+
+					//int idx;
+					for (idx = startIslandIndex; idx < endIslandIndex; idx++) {
+						int i = getUnionFind().getElement(idx).sz;
+
+						CollisionObject colObj0 = collisionObjects.get(i);
+						if ((colObj0.getIslandTag() != islandId) && (colObj0.getIslandTag() != -1)) {
+							System.err.println("error in island management\n");
+						}
+
+						assert ((colObj0.getIslandTag() == islandId) || (colObj0.getIslandTag() == -1));
+
+						if (colObj0.getIslandTag() == islandId) {
+							if (colObj0.getActivationState() == CollisionObject.ISLAND_SLEEPING) {
+								colObj0.setActivationState(CollisionObject.WANTS_DEACTIVATION);
+							}
+						}
+					}
+				}
+			}
+
+
+			int i;
+			int maxNumManifolds = dispatcher.getNumManifolds();
+
+			//#define SPLIT_ISLANDS 1
+			//#ifdef SPLIT_ISLANDS
+			//#endif //SPLIT_ISLANDS
+
+			for (i = 0; i < maxNumManifolds; i++) {
+				PersistentManifold manifold = dispatcher.getManifoldByIndexInternal(i);
+
+				CollisionObject colObj0 = (CollisionObject) manifold.getBody0();
+				CollisionObject colObj1 = (CollisionObject) manifold.getBody1();
+
+				// todo: check sleeping conditions!
+				if (((colObj0 != null) && colObj0.getActivationState() != CollisionObject.ISLAND_SLEEPING) ||
+						((colObj1 != null) && colObj1.getActivationState() != CollisionObject.ISLAND_SLEEPING)) {
+
+					// kinematic objects don't merge islands, but wake up all connected objects
+					if (colObj0.isKinematicObject() && colObj0.getActivationState() != CollisionObject.ISLAND_SLEEPING) {
+						colObj1.activate();
+					}
+					if (colObj1.isKinematicObject() && colObj1.getActivationState() != CollisionObject.ISLAND_SLEEPING) {
+						colObj0.activate();
+					}
+					//#ifdef SPLIT_ISLANDS
+					//filtering for response
+					if (dispatcher.needsResponse(colObj0, colObj1)) {
+						islandmanifold.add(manifold);
+					}
+					//#endif //SPLIT_ISLANDS
+				}
+			}
+
+			//#ifndef SPLIT_ISLANDS
+			//btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer();
+			//
+			//callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1);
+			//#else
+			// Sort manifolds, based on islands
+			// Sort the vector using predicate and std::sort
+			//std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate);
+
+			int numManifolds = islandmanifold.size();
+
+			// we should do radix sort, it it much faster (O(n) instead of O (n log2(n))
+			//islandmanifold.heapSort(btPersistentManifoldSortPredicate());
+			
+			// JAVA NOTE: memory optimized sorting with caching of temporary array
+			//Collections.sort(islandmanifold, persistentManifoldComparator);
+			MiscUtil.heapSort(islandmanifold, persistentManifoldComparator);
+
+			// now process all active islands (sets of manifolds for now)
+
+			int startManifoldIndex = 0;
+			int endManifoldIndex = 1;
+
+			//int islandId;
+
+			//printf("Start Islands\n");
+
+			// traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated
+			for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) {
+				int islandId = getUnionFind().getElement(startIslandIndex).id;
+				boolean islandSleeping = false;
+
+				for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).id == islandId); endIslandIndex++) {
+					/*int*/ i = getUnionFind().getElement(endIslandIndex).sz;
+					CollisionObject colObj0 = collisionObjects.get(i);
+					islandBodies.add(colObj0);
+					if (!colObj0.isActive()) {
+						islandSleeping = true;
+					}
+				}
+
+
+				// find the accompanying contact manifold for this islandId
+				int numIslandManifolds = 0;
+				//List<PersistentManifold> startManifold = null;
+				int startManifold_idx = -1;
+
+				if (startManifoldIndex < numManifolds) {
+					int curIslandId = getIslandId(islandmanifold.get(startManifoldIndex));
+					if (curIslandId == islandId) {
+						//startManifold = &m_islandmanifold[startManifoldIndex];
+						//startManifold = islandmanifold.subList(startManifoldIndex, islandmanifold.size());
+						startManifold_idx = startManifoldIndex;
+
+						for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == getIslandId(islandmanifold.get(endManifoldIndex))); endManifoldIndex++) {
+
+						}
+						// Process the actual simulation, only if not sleeping/deactivated
+						numIslandManifolds = endManifoldIndex - startManifoldIndex;
+					}
+
+				}
+
+				if (!islandSleeping) {
+					callback.processIsland(islandBodies, islandBodies.size(), islandmanifold, startManifold_idx, numIslandManifolds, islandId);
+					//printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds);
+				}
+
+				if (numIslandManifolds != 0) {
+					startManifoldIndex = endManifoldIndex;
+				}
+
+				islandBodies.clear();
+			}
+			//#endif //SPLIT_ISLANDS
+
+			islandmanifold.clear();
+		}
+		finally {
+			BulletGlobals.popProfile();
+		}
+	}
+
+	////////////////////////////////////////////////////////////////////////////
+	
+	public interface IslandCallback {
+		public void processIsland(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifolds, int manifolds_offset, int numManifolds, int islandId);
+	}
+	
+	private static final Comparator<PersistentManifold> persistentManifoldComparator = new Comparator<PersistentManifold>() {
+		public int compare(PersistentManifold lhs, PersistentManifold rhs) {
+			return getIslandId(lhs) < getIslandId(rhs)? -1 : +1;
+		}
+	};
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/SphereSphereCollisionAlgorithm.java b/src/jbullet/src/javabullet/collision/dispatch/SphereSphereCollisionAlgorithm.java
new file mode 100644
index 0000000..66f73a2
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/SphereSphereCollisionAlgorithm.java
@@ -0,0 +1,137 @@
+/*
+ * 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.collision.dispatch;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javabullet.collision.broadphase.CollisionAlgorithm;
+import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo;
+import javabullet.collision.broadphase.DispatcherInfo;
+import javabullet.collision.narrowphase.PersistentManifold;
+import javabullet.collision.shapes.SphereShape;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class SphereSphereCollisionAlgorithm extends CollisionAlgorithm {
+	
+	private boolean ownManifold;
+	private PersistentManifold manifoldPtr;
+
+	public SphereSphereCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0, CollisionObject col1) {
+		super(ci);
+		manifoldPtr = mf;
+
+		if (manifoldPtr == null) {
+			manifoldPtr = dispatcher.getNewManifold(col0, col1);
+			ownManifold = true;
+		}
+	}
+
+	public SphereSphereCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci) {
+		super(ci);
+	}
+	
+	@Override
+	public void destroy() {
+		if (ownManifold) {
+			if (manifoldPtr != null) {
+				dispatcher.releaseManifold(manifoldPtr);
+			}
+		}
+	}
+	
+	@Override
+	public void processCollision(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
+		if (manifoldPtr == null) {
+			return;
+		}
+
+		stack.vectors.push();
+		try {
+			resultOut.setPersistentManifold(manifoldPtr);
+
+			SphereShape sphere0 = (SphereShape) col0.getCollisionShape();
+			SphereShape sphere1 = (SphereShape) col1.getCollisionShape();
+
+			Vector3f diff = stack.vectors.get();
+			diff.sub(col0.getWorldTransform().origin, col1.getWorldTransform().origin);
+
+			float len = diff.length();
+			float radius0 = sphere0.getRadius();
+			float radius1 = sphere1.getRadius();
+
+			manifoldPtr.clearManifold();
+
+			// if distance positive, don't generate a new contact
+			if (len > (radius0 + radius1)) {
+				return;
+			}
+			// distance (negative means penetration)
+			float dist = len - (radius0 + radius1);
+
+			Vector3f normalOnSurfaceB = stack.vectors.get(1f, 0f, 0f);
+			if (len > BulletGlobals.FLT_EPSILON) {
+				normalOnSurfaceB.scale(1f / len, diff);
+			}
+
+			Vector3f tmp = stack.vectors.get();
+
+			// point on A (worldspace)
+			Vector3f pos0 = stack.vectors.get();
+			tmp.scale(radius0, normalOnSurfaceB);
+			pos0.sub(col0.getWorldTransform().origin, tmp);
+
+			// point on B (worldspace)
+			Vector3f pos1 = stack.vectors.get();
+			tmp.scale(radius1, normalOnSurfaceB);
+			pos1.add(col1.getWorldTransform().origin, tmp);
+
+			// report a contact. internally this will be kept persistent, and contact reduction is done
+			resultOut.addContactPoint(normalOnSurfaceB, pos1, dist);
+
+			//no resultOut->refreshContactPoints(); needed, because of clearManifold (all points are new)
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) {
+		return 1f;
+	}
+
+	////////////////////////////////////////////////////////////////////////////
+	
+	public static final CollisionAlgorithmCreateFunc createFunc = new CollisionAlgorithmCreateFunc() {
+		@Override
+		public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) {
+			return new SphereSphereCollisionAlgorithm(null, ci, body0, body1);
+		}
+	};
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/UnionFind.java b/src/jbullet/src/javabullet/collision/dispatch/UnionFind.java
new file mode 100644
index 0000000..a4419ed
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/UnionFind.java
@@ -0,0 +1,149 @@
+/*
+ * 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.collision.dispatch;
+
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+import javabullet.linearmath.MiscUtil;
+
+/**
+ * UnionFind calculates connected subsets.
+ * Implements weighted Quick Union with path compression.
+ * Optimization: could use short ints instead of ints (halving memory, would limit the number of rigid bodies to 64k, sounds reasonable).
+ * 
+ * @author jezek2
+ */
+public class UnionFind {
+
+	private final List<Element> elements = new ArrayList<Element>();
+	
+	/**
+	 * This is a special operation, destroying the content of UnionFind.
+	 * It sorts the elements, based on island id, in order to make it easy to iterate over islands.
+	 */
+	public void sortIslands() {
+		// first store the original body index, and islandId
+		int numElements = elements.size();
+
+		for (int i = 0; i < numElements; i++) {
+			elements.get(i).id = find(i);
+			elements.get(i).sz = i;
+		}
+
+		// Sort the vector using predicate and std::sort
+		//std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate);
+		//perhaps use radix sort?
+		//elements.heapSort(btUnionFindElementSortPredicate());
+		
+		//Collections.sort(elements);
+		MiscUtil.heapSort(elements, elementComparator);
+	}
+
+	public void reset(int N) {
+		allocate(N);
+
+		for (int i = 0; i < N; i++) {
+			elements.get(i).id = i;
+			elements.get(i).sz = 1;
+		}
+	}
+
+	public int getNumElements() {
+		return elements.size();
+	}
+
+	public boolean isRoot(int x) {
+		return (x == elements.get(x).id);
+	}
+
+	public Element getElement(int index) {
+		return elements.get(index);
+	}
+
+	public void allocate(int N) {
+		MiscUtil.resize(elements, N, Element.class);
+	}
+
+	public void free() {
+		elements.clear();
+	}
+
+	public int find(int p, int q) {
+		return (find(p) == find(q))? 1 : 0;
+	}
+
+	public void unite(int p, int q) {
+		int i = find(p), j = find(q);
+		if (i == j) {
+			return;
+		}
+
+		//#ifndef USE_PATH_COMPRESSION
+		////weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) )
+		//if (m_elements[i].m_sz < m_elements[j].m_sz)
+		//{ 
+		//	m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; 
+		//}
+		//else 
+		//{ 
+		//	m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; 
+		//}
+		//#else
+		elements.get(i).id = j;
+		elements.get(j).sz += elements.get(i).sz;
+		//#endif //USE_PATH_COMPRESSION
+	}
+
+	public int find(int x) {
+		//assert(x < m_N);
+		//assert(x >= 0);
+
+		while (x != elements.get(x).id) {
+			// not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically
+
+			//#ifdef USE_PATH_COMPRESSION
+			elements.get(x).id = elements.get(elements.get(x).id).id;
+			//#endif //
+			x = elements.get(x).id;
+			//assert(x < m_N);
+			//assert(x >= 0);
+		}
+		return x;
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	public static class Element {
+		public int id;
+		public int sz;
+	}
+	
+	private static final Comparator<Element> elementComparator = new Comparator<Element>() {
+		public int compare(Element o1, Element o2) {
+			return o1.id < o2.id? -1 : +1;
+		}
+	};
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/dispatch/package-info.java b/src/jbullet/src/javabullet/collision/dispatch/package-info.java
new file mode 100644
index 0000000..683537c
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/dispatch/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.
+ */
+
+/**
+ * Dispatching code for collisions between various shapes.
+ */
+package javabullet.collision.dispatch;
+
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/ConvexCast.java b/src/jbullet/src/javabullet/collision/narrowphase/ConvexCast.java
new file mode 100644
index 0000000..fdf63e3
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/ConvexCast.java
@@ -0,0 +1,60 @@
+/*
+ * 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.collision.narrowphase;
+
+import javabullet.linearmath.IDebugDraw;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * ConvexCast is an interface for Casting.
+ * 
+ * @author jezek2
+ */
+public interface ConvexCast {
+
+	/**
+	 * Cast a convex against another convex object.
+	 */
+	public boolean calcTimeOfImpact(Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result);
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	/**
+	 * RayResult stores the closest result
+	 * alternatively, add a callback method to decide about closest/all results
+	 */
+	public static class CastResult {
+		public final Vector3f normal = new Vector3f();
+		public final Vector3f hitPoint = new Vector3f();
+		public float fraction = 1e30f;
+		public final Transform hitTransformA = new Transform();
+		public final Transform hitTransformB = new Transform();
+		public IDebugDraw debugDrawer;
+		
+		public void debugDraw(float fraction) {}
+		public void	drawCoordSystem(Transform trans) {}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/ConvexPenetrationDepthSolver.java b/src/jbullet/src/javabullet/collision/narrowphase/ConvexPenetrationDepthSolver.java
new file mode 100644
index 0000000..2b1ae15
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/ConvexPenetrationDepthSolver.java
@@ -0,0 +1,44 @@
+/*
+ * 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.collision.narrowphase;
+
+import javabullet.collision.shapes.ConvexShape;
+import javabullet.linearmath.IDebugDraw;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * ConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
+ * 
+ * @author jezek2
+ */
+public interface ConvexPenetrationDepthSolver {
+
+	public boolean calcPenDepth(SimplexSolverInterface simplexSolver,
+			ConvexShape convexA, ConvexShape convexB,
+			Transform transA, Transform transB,
+			Vector3f v, Vector3f pa, Vector3f pb,
+			IDebugDraw debugDraw/*, btStackAlloc* stackAlloc*/);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/DiscreteCollisionDetectorInterface.java b/src/jbullet/src/javabullet/collision/narrowphase/DiscreteCollisionDetectorInterface.java
new file mode 100644
index 0000000..fcd453c
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/DiscreteCollisionDetectorInterface.java
@@ -0,0 +1,69 @@
+/*
+ * 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.collision.narrowphase;
+
+import javabullet.linearmath.IDebugDraw;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * This interface is made to be used by an iterative approach to do TimeOfImpact calculations.
+ * This interface allows to query for closest points and penetration depth between two (convex) objects
+ * the closest point is on the second object (B), and the normal points from the surface on B towards A.
+ * distance is between closest points on B and closest point on A. So you can calculate closest point on A
+ * by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB
+ * 
+ * @author jezek2
+ */
+public interface DiscreteCollisionDetectorInterface {
+
+	public interface Result {
+		///setShapeIdentifiers provides experimental support for per-triangle material / custom material combiner
+		public void setShapeIdentifiers(int partId0, int index0, int partId1, int index1);
+
+		public void addContactPoint(Vector3f normalOnBInWorld, Vector3f pointInWorld, float depth);
+	}
+	
+	public static class ClosestPointInput {
+		public final Transform transformA = new Transform();
+		public final Transform transformB = new Transform();
+		public float maximumDistanceSquared;
+		//btStackAlloc* m_stackAlloc;
+
+		public ClosestPointInput() {
+			init();
+		}
+		
+		public void init() {
+			maximumDistanceSquared = Float.MAX_VALUE;
+		}
+	}
+
+	/**
+	 * Give either closest points (distance > 0) or penetration (distance)
+	 * the normal always points from B towards A.
+	 */
+	public void	getClosestPoints(ClosestPointInput input,Result output, IDebugDraw debugDraw);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/GjkConvexCast.java b/src/jbullet/src/javabullet/collision/narrowphase/GjkConvexCast.java
new file mode 100644
index 0000000..a5daad0
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/GjkConvexCast.java
@@ -0,0 +1,207 @@
+/*
+ * 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.collision.narrowphase;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletPool;
+import javabullet.BulletStack;
+import javabullet.ObjectPool;
+import javabullet.collision.narrowphase.DiscreteCollisionDetectorInterface.ClosestPointInput;
+import javabullet.collision.shapes.ConvexShape;
+import javabullet.collision.shapes.MinkowskiSumShape;
+import javabullet.collision.shapes.SphereShape;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * GjkConvexCast performs a raycast on a convex object using support mapping.
+ * 
+ * @author jezek2
+ */
+public class GjkConvexCast implements ConvexCast {
+
+	protected final BulletStack stack = BulletStack.get();
+	protected final ObjectPool<ClosestPointInput> pointInputsPool = BulletPool.get(ClosestPointInput.class);
+	
+	private SimplexSolverInterface simplexSolver;
+	private ConvexShape convexA;
+	private ConvexShape convexB;
+
+	public GjkConvexCast(ConvexShape convexA, ConvexShape convexB, SimplexSolverInterface simplexSolver) {
+		this.simplexSolver = simplexSolver;
+		this.convexA = convexA;
+		this.convexB = convexB;
+	}
+	
+	public boolean calcTimeOfImpact(Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result) {
+		stack.pushCommonMath();
+		try {
+			MinkowskiSumShape combi = new MinkowskiSumShape(convexA, convexB);
+			MinkowskiSumShape convex = combi;
+
+			Transform rayFromLocalA = stack.transforms.get();
+			Transform rayToLocalA = stack.transforms.get();
+
+			rayFromLocalA.inverse(fromA);
+			rayFromLocalA.mul(fromB);
+
+			rayToLocalA.inverse(toA);
+			rayToLocalA.mul(toB);
+
+			Transform trA = stack.transforms.get(), trB = stack.transforms.get();
+			trA.set(fromA);
+			trB.set(fromB);
+			trA.origin.set(0f, 0f, 0f);
+			trB.origin.set(0f, 0f, 0f);
+
+			convex.setTransformA(trA);
+			convex.setTransformB(trB);
+
+			float radius = 0.01f;
+
+			float lambda = 0f;
+			Vector3f s = stack.vectors.get(rayFromLocalA.origin);
+			Vector3f r = stack.vectors.get();
+			r.sub(rayToLocalA.origin, rayFromLocalA.origin);
+			Vector3f x = stack.vectors.get(s);
+			Vector3f n = stack.vectors.get();
+			n.set(0f, 0f, 0f);
+			boolean hasResult = false;
+			Vector3f c = stack.vectors.get();
+
+			float lastLambda = lambda;
+
+			// first solution, using GJK
+
+			// no penetration support for now, perhaps pass a pointer when we really want it
+			ConvexPenetrationDepthSolver penSolverPtr = null;
+
+			Transform identityTrans = stack.transforms.get();
+			identityTrans.setIdentity();
+
+			SphereShape raySphere = new SphereShape(0f);
+			raySphere.setMargin(0f);
+
+			Transform sphereTr = stack.transforms.get();
+			sphereTr.setIdentity();
+			sphereTr.origin.set(rayFromLocalA.origin);
+
+			result.drawCoordSystem(sphereTr);
+			{
+				PointCollector pointCollector1 = new PointCollector();
+				GjkPairDetector gjk = new GjkPairDetector(raySphere, convex, simplexSolver, penSolverPtr);
+
+				ClosestPointInput input = pointInputsPool.get();
+				input.init();
+				
+				input.transformA.set(sphereTr);
+				input.transformB.set(identityTrans);
+				gjk.getClosestPoints(input, pointCollector1, null);
+				
+				pointInputsPool.release(input);
+
+				hasResult = pointCollector1.hasResult;
+				c.set(pointCollector1.pointInWorld);
+				n.set(pointCollector1.normalOnBInWorld);
+			}
+
+			if (hasResult) {
+				Vector3f tmp = stack.vectors.get();
+
+				float dist;
+				tmp.sub(c, x);
+				dist = tmp.length();
+				if (dist < radius) {
+					// penetration
+					lastLambda = 1f;
+				}
+
+				// not close enough
+				while (dist > radius) {
+
+					n.sub(x, c);
+					float nDotr = n.dot(r);
+
+					if (nDotr >= -(BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
+						return false;
+					}
+
+					lambda = lambda - n.dot(n) / nDotr;
+					if (lambda <= lastLambda) {
+						break;
+					}
+
+					lastLambda = lambda;
+
+					x.scaleAdd(lambda, r, s);
+
+					sphereTr.origin.set(x);
+					result.drawCoordSystem(sphereTr);
+					PointCollector pointCollector = new PointCollector();
+					GjkPairDetector gjk = new GjkPairDetector(raySphere, convex, simplexSolver, penSolverPtr);
+					
+					ClosestPointInput input = pointInputsPool.get();
+					input.init();
+					
+					input.transformA.set(sphereTr);
+					input.transformB.set(identityTrans);
+					gjk.getClosestPoints(input, pointCollector, null);
+					
+					pointInputsPool.release(input);
+					
+					if (pointCollector.hasResult) {
+						if (pointCollector.distance < 0f) {
+							// degeneracy, report a hit
+							result.fraction = lastLambda;
+							result.normal.set(n);
+							result.hitPoint.set(pointCollector.pointInWorld);
+							return true;
+						}
+						c.set(pointCollector.pointInWorld);
+						tmp.sub(c, x);
+						dist = tmp.length();
+					}
+					else {
+						// ??
+						return false;
+					}
+
+				}
+
+				if (lastLambda < 1f) {
+					result.fraction = lastLambda;
+					result.normal.set(n);
+					result.hitPoint.set(c);
+					return true;
+				}
+			}
+
+			return false;
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/GjkEpaPenetrationDepthSolver.java b/src/jbullet/src/javabullet/collision/narrowphase/GjkEpaPenetrationDepthSolver.java
new file mode 100644
index 0000000..e8b74c0
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/GjkEpaPenetrationDepthSolver.java
@@ -0,0 +1,59 @@
+/*
+ * 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.collision.narrowphase;
+
+import javabullet.collision.shapes.ConvexShape;
+import javabullet.linearmath.IDebugDraw;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class GjkEpaPenetrationDepthSolver implements ConvexPenetrationDepthSolver {
+
+	public boolean calcPenDepth(SimplexSolverInterface simplexSolver,
+												  ConvexShape pConvexA, ConvexShape pConvexB,
+												  Transform transformA, Transform transformB,
+												  Vector3f v, Vector3f wWitnessOnA, Vector3f wWitnessOnB,
+												  IDebugDraw debugDraw/*, btStackAlloc* stackAlloc*/)
+	{
+		float radialmargin = 0f;
+
+		GjkEpaSolver.Results results = new GjkEpaSolver.Results();
+		if (GjkEpaSolver.collide(pConvexA, transformA,
+				pConvexB, transformB,
+				radialmargin/*,stackAlloc*/, results)) {
+			//debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
+			//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
+			wWitnessOnA.set(results.witnesses[0]);
+			wWitnessOnB.set(results.witnesses[1]);
+			return true;
+		}
+
+		return false;
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/GjkEpaSolver.java b/src/jbullet/src/javabullet/collision/narrowphase/GjkEpaSolver.java
new file mode 100644
index 0000000..989e38f
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/GjkEpaSolver.java
@@ -0,0 +1,947 @@
+/*
+ * 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.collision.narrowphase;
+
+import java.util.Arrays;
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javabullet.ObjectStackList;
+import javabullet.collision.narrowphase.GjkEpaSolver.EPA.Face;
+import javabullet.collision.narrowphase.GjkEpaSolver.GJK.He;
+import javabullet.collision.narrowphase.GjkEpaSolver.GJK.Mkv;
+import javabullet.collision.shapes.ConvexShape;
+import javabullet.linearmath.MatrixUtil;
+import javabullet.linearmath.QuaternionUtil;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Quat4f;
+import javax.vecmath.Vector3f;
+
+/*
+GJK-EPA collision solver by Nathanael Presson
+Nov.2006
+*/
+
+/**
+ * GjkEpaSolver contributed under zlib by Nathanael Presson.
+ * 
+ * @author jezek2
+ */
+public class GjkEpaSolver {
+	
+	protected static ObjectStackList<Mkv> stackMkv = new ObjectStackList<Mkv>(Mkv.class);
+	protected static ObjectStackList<He> stackHe = new ObjectStackList<He>(He.class);
+	protected static ObjectStackList<Face> stackFace = new ObjectStackList<Face>(Face.class);
+	
+	protected static void pushStack() {
+		stackMkv.push();
+		stackHe.push();
+		stackFace.push();
+	}
+	
+	protected static void popStack() {
+		stackMkv.pop();
+		stackHe.pop();
+		stackFace.pop();
+	}
+
+	public enum ResultsStatus {
+		Separated,		/* Shapes doesnt penetrate												*/ 
+		Penetrating,	/* Shapes are penetrating												*/ 
+		GJK_Failed,		/* GJK phase fail, no big issue, shapes are probably just 'touching'	*/ 
+		EPA_Failed,		/* EPA phase fail, bigger problem, need to save parameters, and debug	*/ 
+	}
+	
+	public static class Results {
+		public ResultsStatus status;
+		public final Vector3f[] witnesses/*[2]*/ = new Vector3f[] { new Vector3f(), new Vector3f() };
+		public final Vector3f normal = new Vector3f();
+		public float depth;
+		public int epa_iterations;
+		public int gjk_iterations;
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	private static final float cstInf = BulletGlobals.SIMD_INFINITY;
+	private static final float cstPi = BulletGlobals.SIMD_PI;
+	private static final float cst2Pi = BulletGlobals.SIMD_2_PI;
+	private static final int GJK_maxiterations = 128;
+	private static final int GJK_hashsize = 1 << 6;
+	private static final int GJK_hashmask = GJK_hashsize - 1;
+	private static final float GJK_insimplex_eps = 0.0001f;
+	private static final float GJK_sqinsimplex_eps = GJK_insimplex_eps * GJK_insimplex_eps;
+	private static final int EPA_maxiterations = 256;
+	private static final float EPA_inface_eps = 0.01f;
+	private static final float EPA_accuracy = 0.001f;
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	protected static class GJK {
+		protected final BulletStack stack = BulletStack.get();
+		
+		public static class Mkv {
+			public final Vector3f w = new Vector3f(); // Minkowski vertice
+			public final Vector3f r = new Vector3f(); // Ray
+			
+			public void set(Mkv m) {
+				w.set(m.w);
+				r.set(m.r);
+			}
+		}
+
+		public static class He {
+			public final Vector3f v = new Vector3f();
+			public He n;
+		}
+		
+		//public btStackAlloc sa;
+		//public Block sablock;
+		public final He[] table = new He[GJK_hashsize];
+		public final Matrix3f[] wrotations/*[2]*/ = new Matrix3f[] { new Matrix3f(), new Matrix3f() };
+		public final Vector3f[] positions/*[2]*/ = new Vector3f[] { new Vector3f(), new Vector3f() };
+		public final ConvexShape[] shapes = new ConvexShape[2];
+		public final Mkv[] simplex = new Mkv[5];
+		public final Vector3f ray = new Vector3f();
+		public /*unsigned*/ int order;
+		public /*unsigned*/ int iterations;
+		public float margin;
+		public boolean failed;
+		
+		{
+			for (int i=0; i<simplex.length; i++) simplex[i] = new Mkv();
+		}
+
+		public GJK() {
+		}
+
+		public GJK(/*StackAlloc psa,*/
+				Matrix3f wrot0, Vector3f pos0, ConvexShape shape0,
+				Matrix3f wrot1, Vector3f pos1, ConvexShape shape1) {
+			this(wrot0, pos0, shape0, wrot1, pos1, shape1, 0f);
+		}
+
+		public GJK(/*StackAlloc psa,*/
+				Matrix3f wrot0, Vector3f pos0, ConvexShape shape0,
+				Matrix3f wrot1, Vector3f pos1, ConvexShape shape1,
+				float pmargin) {
+			init(wrot0, pos0, shape0, wrot1, pos1, shape1, pmargin);
+		}
+		
+		public void init(/*StackAlloc psa,*/
+				Matrix3f wrot0, Vector3f pos0, ConvexShape shape0,
+				Matrix3f wrot1, Vector3f pos1, ConvexShape shape1,
+				float pmargin) {
+			pushStack();
+			wrotations[0].set(wrot0);
+			positions[0].set(pos0);
+			shapes[0] = shape0;
+			wrotations[1].set(wrot1);
+			positions[1].set(pos1);
+			shapes[1] = shape1;
+			//sa		=psa;
+			//sablock	=sa->beginBlock();
+			margin = pmargin;
+			failed = false;
+		}
+		
+		public void destroy() {
+			popStack();
+		}
+		
+		// vdh: very dummy hash
+		public static /*unsigned*/ int Hash(Vector3f v) {
+			int h = (int)(v.x * 15461) ^ (int)(v.y * 83003) ^ (int)(v.z * 15473);
+			return (h * 169639) & GJK_hashmask;
+		}
+
+		public Vector3f LocalSupport(Vector3f d, /*unsigned*/ int i) {
+			stack.vectors.push();
+			try {
+				Vector3f tmp = stack.vectors.get();
+				MatrixUtil.transposeTransform(tmp, d, wrotations[i]);
+
+				Vector3f tmp2 = stack.vectors.get(shapes[i].localGetSupportingVertex(tmp));
+				wrotations[i].transform(tmp2);
+				tmp2.add(positions[i]);
+
+				return stack.vectors.returning(tmp2);
+			}
+			finally {
+				stack.vectors.pop();
+			}
+		}
+		
+		public void Support(Vector3f d, Mkv v) {
+			stack.vectors.push();
+			try {
+				v.r.set(d);
+
+				Vector3f tmp1 = stack.vectors.get(LocalSupport(d, 0));
+
+				Vector3f tmp = stack.vectors.get();
+				tmp.set(d);
+				tmp.negate();
+				Vector3f tmp2 = stack.vectors.get(LocalSupport(tmp, 1));
+
+				v.w.sub(tmp1, tmp2);
+				v.w.scaleAdd(margin, d, v.w);
+			}
+			finally {
+				stack.vectors.pop();
+			}
+		}
+
+		public boolean FetchSupport() {
+			int h = Hash(ray);
+			He e = table[h];
+			while (e != null) {
+				if (e.v.equals(ray)) {
+					--order;
+					return false;
+				}
+				else {
+					e = e.n;
+				}
+			}
+			//e = (He*)sa->allocate(sizeof(He));
+			//e = new He();
+			e = stackHe.get();
+			e.v.set(ray);
+			e.n = table[h];
+			table[h] = e;
+			Support(ray, simplex[++order]);
+			return (ray.dot(simplex[order].w) > 0);
+		}
+
+		public boolean SolveSimplex2(Vector3f ao, Vector3f ab) {
+			stack.vectors.push();
+			try {
+				if (ab.dot(ao) >= 0) {
+					Vector3f cabo = stack.vectors.get();
+					cabo.cross(ab, ao);
+					if (cabo.lengthSquared() > GJK_sqinsimplex_eps) {
+						ray.cross(cabo, ab);
+					}
+					else {
+						return true;
+					}
+				}
+				else {
+					order = 0;
+					simplex[0].set(simplex[1]);
+					ray.set(ao);
+				}
+				return (false);
+			}
+			finally {
+				stack.vectors.pop();
+			}
+		}
+
+		public boolean SolveSimplex3(Vector3f ao, Vector3f ab, Vector3f ac)
+		{
+			stack.vectors.push();
+			try {
+				Vector3f tmp = stack.vectors.get();
+				tmp.cross(ab, ac);
+				return (SolveSimplex3a(ao,ab,ac,tmp));
+			}
+			finally {
+				stack.vectors.pop();
+			}
+		}
+		
+		public boolean SolveSimplex3a(Vector3f ao, Vector3f ab, Vector3f ac, Vector3f cabc) {
+			stack.vectors.push();
+			try {
+				// TODO: optimize
+
+				Vector3f tmp = stack.vectors.get();
+				tmp.cross(cabc, ab);
+
+				Vector3f tmp2 = stack.vectors.get();
+				tmp2.cross(cabc, ac);
+
+				if (tmp.dot(ao) < -GJK_insimplex_eps) {
+					order = 1;
+					simplex[0].set(simplex[1]);
+					simplex[1].set(simplex[2]);
+					return SolveSimplex2(ao, ab);
+				}
+				else if (tmp2.dot(ao) > +GJK_insimplex_eps) {
+					order = 1;
+					simplex[1].set(simplex[2]);
+					return SolveSimplex2(ao, ac);
+				}
+				else {
+					float d = cabc.dot(ao);
+					if (Math.abs(d) > GJK_insimplex_eps) {
+						if (d > 0) {
+							ray.set(cabc);
+						}
+						else {
+							ray.negate(cabc);
+
+							Mkv swapTmp = new Mkv();
+							swapTmp.set(simplex[0]);
+							simplex[0].set(simplex[1]);
+							simplex[1].set(swapTmp);
+						}
+						return false;
+					}
+					else {
+						return true;
+					}
+				}
+			}
+			finally {
+				stack.vectors.pop();
+			}
+		}
+		
+		public boolean SolveSimplex4(Vector3f ao, Vector3f ab, Vector3f ac, Vector3f ad) {
+			stack.vectors.push();
+			try {
+				// TODO: optimize
+
+				Vector3f crs = stack.vectors.get();
+
+				Vector3f tmp = stack.vectors.get();
+				tmp.cross(ab, ac);
+
+				Vector3f tmp2 = stack.vectors.get();
+				tmp2.cross(ac, ad);
+
+				Vector3f tmp3 = stack.vectors.get();
+				tmp3.cross(ad, ab);
+
+				if (tmp.dot(ao) > GJK_insimplex_eps) {
+					crs.set(tmp);
+					order = 2;
+					simplex[0].set(simplex[1]);
+					simplex[1].set(simplex[2]);
+					simplex[2].set(simplex[3]);
+					return SolveSimplex3a(ao, ab, ac, crs);
+				}
+				else if (tmp2.dot(ao) > GJK_insimplex_eps) {
+					crs.set(tmp2);
+					order = 2;
+					simplex[2].set(simplex[3]);
+					return SolveSimplex3a(ao, ac, ad, crs);
+				}
+				else if (tmp3.dot(ao) > GJK_insimplex_eps) {
+					crs.set(tmp3);
+					order = 2;
+					simplex[1].set(simplex[0]);
+					simplex[0].set(simplex[2]);
+					simplex[2].set(simplex[3]);
+					return SolveSimplex3a(ao, ad, ab, crs);
+				}
+				else {
+					return (true);
+				}
+			}
+			finally {
+				stack.vectors.pop();
+			}
+		}
+		
+		public boolean SearchOrigin() {
+			stack.vectors.push();
+			try {
+				return SearchOrigin(stack.vectors.get(1f, 0f, 0f));
+			}
+			finally {
+				stack.vectors.pop();
+			}
+		}
+		
+		public boolean SearchOrigin(Vector3f initray) {
+			stack.vectors.push();
+			try {
+				Vector3f tmp1 = stack.vectors.get();
+				Vector3f tmp2 = stack.vectors.get();
+				Vector3f tmp3 = stack.vectors.get();
+				Vector3f tmp4 = stack.vectors.get();
+
+				iterations = 0;
+				order = -1;
+				failed = false;
+				ray.set(initray);
+				ray.normalize();
+
+				Arrays.fill(table, null);
+
+				FetchSupport();
+				ray.negate(simplex[0].w);
+				for (; iterations < GJK_maxiterations; ++iterations) {
+					float rl = ray.length();
+					ray.scale(1f / (rl > 0f ? rl : 1f));
+					if (FetchSupport()) {
+						boolean found = false;
+						switch (order) {
+							case 1: {
+								tmp1.negate(simplex[1].w);
+								tmp2.sub(simplex[0].w, simplex[1].w);
+								found = SolveSimplex2(tmp1, tmp2);
+								break;
+							}
+							case 2: {
+								tmp1.negate(simplex[2].w);
+								tmp2.sub(simplex[1].w, simplex[2].w);
+								tmp3.sub(simplex[0].w, simplex[2].w);
+								found = SolveSimplex3(tmp1, tmp2, tmp3);
+								break;
+							}
+							case 3: {
+								tmp1.negate(simplex[3].w);
+								tmp2.sub(simplex[2].w, simplex[3].w);
+								tmp3.sub(simplex[1].w, simplex[3].w);
+								tmp4.sub(simplex[0].w, simplex[3].w);
+								found = SolveSimplex4(tmp1, tmp2, tmp3, tmp4);
+								break;
+							}
+						}
+						if (found) {
+							return true;
+						}
+					}
+					else {
+						return false;
+					}
+				}
+				failed = true;
+				return false;
+			}
+			finally {
+				stack.vectors.pop();
+			}
+		}
+		
+		public boolean EncloseOrigin() {
+			stack.pushCommonMath();
+			stack.quats.push();
+			try {
+				Vector3f tmp = stack.vectors.get();
+				Vector3f tmp1 = stack.vectors.get();
+				Vector3f tmp2 = stack.vectors.get();
+
+				switch (order) {
+					// Point
+					case 0:
+						break;
+					// Line
+					case 1: {
+						Vector3f ab = stack.vectors.get();
+						ab.sub(simplex[1].w, simplex[0].w);
+
+						Vector3f[] b = new Vector3f[] { stack.vectors.get(1f, 0f, 0f), stack.vectors.get(0f, 1f, 0f), stack.vectors.get(0, 0, 1f) };
+						b[0].cross(ab, b[0]);
+						b[1].cross(ab, b[1]);
+						b[2].cross(ab, b[2]);
+
+						float m[] = new float[] { b[0].lengthSquared(), b[1].lengthSquared(), b[2].lengthSquared() };
+
+						Quat4f tmpQuat = stack.quats.get();
+						tmp.normalize(ab);
+						QuaternionUtil.setRotation(tmpQuat, tmp, cst2Pi / 3f);
+
+						Matrix3f r = stack.matrices.get();
+						MatrixUtil.setRotation(r, tmpQuat);
+
+						Vector3f w = stack.vectors.get(b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]);
+
+						tmp.normalize(w);
+						Support(tmp, simplex[4]); r.transform(w);
+						tmp.normalize(w);
+						Support(tmp, simplex[2]); r.transform(w);
+						tmp.normalize(w);
+						Support(tmp, simplex[3]); r.transform(w);
+						order = 4;
+						return (true);
+					}
+					// Triangle
+					case 2: {
+						tmp1.sub(simplex[1].w, simplex[0].w);
+						tmp2.sub(simplex[2].w, simplex[0].w);
+						Vector3f n = stack.vectors.get();
+						n.cross(tmp1, tmp2);
+						n.normalize();
+
+						Support(n, simplex[3]);
+
+						tmp.negate(n);
+						Support(tmp, simplex[4]);
+						order = 4;
+						return (true);
+					}
+					// Tetrahedron
+					case 3:
+						return (true);
+					// Hexahedron
+					case 4:
+						return (true);
+				}
+				return (false);
+			}
+			finally {
+				stack.popCommonMath();
+				stack.quats.pop();
+			}
+		}
+		
+	}
+
+	////////////////////////////////////////////////////////////////////////////
+	
+	protected static class EPA {
+		protected final BulletStack stack = BulletStack.get();
+		
+		public static class Face {
+			public final GJK.Mkv[] v = new GJK.Mkv[3];
+			public final Face[] f = new Face[3];
+			public final int[] e = new int[3];
+			public final Vector3f n = new Vector3f();
+			public float d;
+			public int mark;
+			public Face prev;
+			public Face next;
+		}
+		
+		public GJK gjk;
+		//public btStackAlloc* sa;
+		public Face root;
+		public int nfaces;
+		public int iterations;
+		public final Vector3f[][] features = new Vector3f[2][3];
+		public final Vector3f[] nearest/*[2]*/ = new Vector3f[] { new Vector3f(), new Vector3f() };
+		public final Vector3f normal = new Vector3f();
+		public float depth;
+		public boolean failed;
+		
+		{
+			for (int i=0; i<features.length; i++) {
+				for (int j=0; j<features[i].length; j++) {
+					features[i][j] = new Vector3f();
+				}
+			}
+		}
+
+		public EPA(GJK pgjk) {
+			gjk = pgjk;
+			//sa = pgjk->sa;
+		}
+		
+		public Vector3f GetCoordinates(Face face) {
+			stack.vectors.push();
+			try {
+				Vector3f tmp = stack.vectors.get();
+				Vector3f tmp1 = stack.vectors.get();
+				Vector3f tmp2 = stack.vectors.get();
+
+				Vector3f o = stack.vectors.get();
+				o.scale(-face.d, face.n);
+
+				float[] a = stack.floatArrays.getFixed(3);
+
+				tmp1.sub(face.v[0].w, o);
+				tmp2.sub(face.v[1].w, o);
+				tmp.cross(tmp1, tmp2);
+				a[0] = tmp.length();
+
+				tmp1.sub(face.v[1].w, o);
+				tmp2.sub(face.v[2].w, o);
+				tmp.cross(tmp1, tmp2);
+				a[1] = tmp.length();
+
+				tmp1.sub(face.v[2].w, o);
+				tmp2.sub(face.v[0].w, o);
+				tmp.cross(tmp1, tmp2);
+				a[2] = tmp.length();
+
+				float sm = a[0] + a[1] + a[2];
+
+				Vector3f result = stack.vectors.get(a[1], a[2], a[0]);
+				result.scale(1f / (sm > 0f ? sm : 1f));
+				
+				stack.floatArrays.release(a);
+				
+				return stack.vectors.returning(result);
+			}
+			finally {
+				stack.vectors.pop();
+			}
+		}
+		
+		public Face FindBest() {
+			Face bf = null;
+			if (root != null) {
+				Face cf = root;
+				float bd = cstInf;
+				do {
+					if (cf.d < bd) {
+						bd = cf.d;
+						bf = cf;
+					}
+				}
+				while (null != (cf = cf.next));
+			}
+			return bf;
+		}
+
+		public boolean Set(Face f, GJK.Mkv a, GJK.Mkv b, GJK.Mkv c) {
+			stack.vectors.push();
+			try {
+				Vector3f tmp1 = stack.vectors.get();
+				Vector3f tmp2 = stack.vectors.get();
+				Vector3f tmp3 = stack.vectors.get();
+
+				Vector3f nrm = stack.vectors.get();
+				tmp1.sub(b.w, a.w);
+				tmp2.sub(c.w, a.w);
+				nrm.cross(tmp1, tmp2);
+
+				float len = nrm.length();
+
+				tmp1.cross(a.w, b.w);
+				tmp2.cross(b.w, c.w);
+				tmp3.cross(c.w, a.w);
+
+				boolean valid = (tmp1.dot(nrm) >= -EPA_inface_eps) &&
+						(tmp2.dot(nrm) >= -EPA_inface_eps) &&
+						(tmp3.dot(nrm) >= -EPA_inface_eps);
+
+				f.v[0] = a;
+				f.v[1] = b;
+				f.v[2] = c;
+				f.mark = 0;
+				f.n.scale(1f / (len > 0f ? len : cstInf), nrm);
+				f.d = Math.max(0, -f.n.dot(a.w));
+				return valid;
+			}
+			finally {
+				stack.vectors.pop();
+			}
+		}
+		
+		public Face NewFace(GJK.Mkv a, GJK.Mkv b, GJK.Mkv c) {
+			//Face pf = new Face();
+			Face pf = stackFace.get();
+			if (Set(pf, a, b, c)) {
+				if (root != null) {
+					root.prev = pf;
+				}
+				pf.prev = null;
+				pf.next = root;
+				root = pf;
+				++nfaces;
+			}
+			else {
+				pf.prev = pf.next = null;
+			}
+			return (pf);
+		}
+	
+		public void Detach(Face face) {
+			if (face.prev != null || face.next != null) {
+				--nfaces;
+				if (face == root) {
+					root = face.next;
+					root.prev = null;
+				}
+				else {
+					if (face.next == null) {
+						face.prev.next = null;
+					}
+					else {
+						face.prev.next = face.next;
+						face.next.prev = face.prev;
+					}
+				}
+				face.prev = face.next = null;
+			}
+		}
+
+		public void Link(Face f0, int e0, Face f1, int e1) {
+			f0.f[e0] = f1; f1.e[e1] = e0;
+			f1.f[e1] = f0; f0.e[e0] = e1;
+		}
+
+		public Mkv Support(Vector3f w) {
+			//Mkv v = new Mkv();
+			Mkv v = stackMkv.get();
+			gjk.Support(w, v);
+			return v;
+		}
+
+		private static int[] mod3 = new int[] { 0, 1, 2, 0, 1 };
+		
+		public int BuildHorizon(int markid, GJK.Mkv w, Face f, int e, Face[] cf, Face[] ff) {
+			int ne = 0;
+			if (f.mark != markid) {
+				int e1 = mod3[e + 1];
+				if ((f.n.dot(w.w) + f.d) > 0) {
+					Face nf = NewFace(f.v[e1], f.v[e], w);
+					Link(nf, 0, f, e);
+					if (cf[0] != null) {
+						Link(cf[0], 1, nf, 2);
+					}
+					else {
+						ff[0] = nf;
+					}
+					cf[0] = nf;
+					ne = 1;
+				}
+				else {
+					int e2 = mod3[e + 2];
+					Detach(f);
+					f.mark = markid;
+					ne += BuildHorizon(markid, w, f.f[e1], f.e[e1], cf, ff);
+					ne += BuildHorizon(markid, w, f.f[e2], f.e[e2], cf, ff);
+				}
+			}
+			return (ne);
+		}
+
+		public float EvaluatePD() {
+			return EvaluatePD(EPA_accuracy);
+		}
+		
+		private static final int[][] tetrahedron_fidx/*[4][3]*/ = new int[][] {{2,1,0},{3,0,1},{3,1,2},{3,2,0}};
+		private static final int[][] tetrahedron_eidx/*[6][4]*/ = new int[][] {{0,0,2,1},{0,1,1,1},{0,2,3,1},{1,0,3,2},{2,0,1,2},{3,0,2,2}};
+		
+		private static final int[][] hexahedron_fidx/*[6][3]*/ = new int[][] {{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}};
+		private static final int[][] hexahedron_eidx/*[9][4]*/ = new int[][] {{0,0,4,0},{0,1,2,1},{0,2,1,2},{1,1,5,2},{1,0,2,0},{2,2,3,2},{3,1,5,0},{3,0,4,2},{5,1,4,1}};
+		
+		public float EvaluatePD(float accuracy) {
+			stack.vectors.push();
+			pushStack();
+			try {
+				Vector3f tmp = stack.vectors.get();
+
+				//btBlock* sablock = sa->beginBlock();
+				Face bestface = null;
+				int markid = 1;
+				depth = -cstInf;
+				normal.set(0f, 0f, 0f);
+				root = null;
+				nfaces = 0;
+				iterations = 0;
+				failed = false;
+				/* Prepare hull		*/
+				if (gjk.EncloseOrigin()) {
+					//const U* pfidx = 0;
+					int[][] pfidx_ptr = null;
+					int pfidx_index = 0;
+
+					int nfidx = 0;
+					//const U* peidx = 0;
+					int[][] peidx_ptr = null;
+					int peidx_index = 0;
+
+					int neidx = 0;
+					GJK.Mkv[] basemkv = new GJK.Mkv[5];
+					Face[] basefaces = new Face[6];
+					switch (gjk.order) {
+						// Tetrahedron
+						case 3:
+							 {
+								//pfidx=(const U*)fidx;
+								pfidx_ptr = tetrahedron_fidx;
+								pfidx_index = 0;
+
+								nfidx = 4;
+
+								//peidx=(const U*)eidx;
+								peidx_ptr = tetrahedron_eidx;
+								peidx_index = 0;
+
+								neidx = 6;
+							}
+							break;
+						// Hexahedron
+						case 4:
+							 {
+								//pfidx=(const U*)fidx;
+								pfidx_ptr = hexahedron_fidx;
+								pfidx_index = 0;
+
+								nfidx = 6;
+
+								//peidx=(const U*)eidx;
+								peidx_ptr = hexahedron_eidx;
+								peidx_index = 0;
+
+								neidx = 9;
+							}
+							break;
+					}
+					int i;
+
+					for (i = 0; i <= gjk.order; ++i) {
+						basemkv[i] = new GJK.Mkv();
+						basemkv[i].set(gjk.simplex[i]);
+					}
+					for (i = 0; i < nfidx; ++i, pfidx_index++) {
+						basefaces[i] = NewFace(basemkv[pfidx_ptr[pfidx_index][0]], basemkv[pfidx_ptr[pfidx_index][1]], basemkv[pfidx_ptr[pfidx_index][2]]);
+					}
+					for (i = 0; i < neidx; ++i, peidx_index++) {
+						Link(basefaces[peidx_ptr[peidx_index][0]], peidx_ptr[peidx_index][1], basefaces[peidx_ptr[peidx_index][2]], peidx_ptr[peidx_index][3]);
+					}
+				}
+				if (0 == nfaces) {
+					//sa->endBlock(sablock);
+					return (depth);
+				}
+				/* Expand hull		*/
+				for (; iterations < EPA_maxiterations; ++iterations) {
+					Face bf = FindBest();
+					if (bf != null) {
+						tmp.negate(bf.n);
+						GJK.Mkv w = Support(tmp);
+						float d = bf.n.dot(w.w) + bf.d;
+						bestface = bf;
+						if (d < -accuracy) {
+							Face[] cf = new Face[]{null};
+							Face[] ff = new Face[]{null};
+							int nf = 0;
+							Detach(bf);
+							bf.mark = ++markid;
+							for (int i = 0; i < 3; ++i) {
+								nf += BuildHorizon(markid, w, bf.f[i], bf.e[i], cf, ff);
+							}
+							if (nf <= 2) {
+								break;
+							}
+							Link(cf[0], 1, ff[0], 2);
+						}
+						else {
+							break;
+						}
+					}
+					else {
+						break;
+					}
+				}
+				/* Extract contact	*/
+				if (bestface != null) {
+					Vector3f b = stack.vectors.get(GetCoordinates(bestface));
+					normal.set(bestface.n);
+					depth = Math.max(0, bestface.d);
+					for (int i = 0; i < 2; ++i) {
+						float s = i != 0 ? -1f : 1f;
+						for (int j = 0; j < 3; ++j) {
+							tmp.scale(s, bestface.v[j].r);
+							features[i][j].set(gjk.LocalSupport(tmp, i));
+						}
+					}
+
+					Vector3f tmp1 = stack.vectors.get();
+					Vector3f tmp2 = stack.vectors.get();
+					Vector3f tmp3 = stack.vectors.get();
+
+					tmp1.scale(b.x, features[0][0]);
+					tmp2.scale(b.y, features[0][1]);
+					tmp3.scale(b.z, features[0][2]);
+					VectorUtil.add(nearest[0], tmp1, tmp2, tmp3);
+
+					tmp1.scale(b.x, features[1][0]);
+					tmp2.scale(b.y, features[1][1]);
+					tmp3.scale(b.z, features[1][2]);
+					VectorUtil.add(nearest[1], tmp1, tmp2, tmp3);
+				}
+				else {
+					failed = true;
+				}
+				//sa->endBlock(sablock);
+				return (depth);
+			}
+			finally {
+				stack.vectors.pop();
+				popStack();
+			}
+		}
+		
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	private static GJK gjk = new GJK();
+	
+	public static boolean collide(ConvexShape shape0, Transform wtrs0,
+			ConvexShape shape1, Transform wtrs1,
+			float radialmargin/*,
+			btStackAlloc* stackAlloc*/,
+			Results results) {
+		
+		// Initialize
+		results.witnesses[0].set(0f, 0f, 0f);
+		results.witnesses[1].set(0f, 0f, 0f);
+		results.normal.set(0f, 0f, 0f);
+		results.depth = 0;
+		results.status = ResultsStatus.Separated;
+		results.epa_iterations = 0;
+		results.gjk_iterations = 0;
+		/* Use GJK to locate origin		*/
+		gjk.init(/*stackAlloc,*/
+				wtrs0.basis, wtrs0.origin, shape0,
+				wtrs1.basis, wtrs1.origin, shape1,
+				radialmargin + EPA_accuracy);
+		try {
+			boolean collide = gjk.SearchOrigin();
+			results.gjk_iterations = gjk.iterations + 1;
+			if (collide) {
+				/* Then EPA for penetration depth	*/
+				EPA epa = new EPA(gjk);
+				float pd = epa.EvaluatePD();
+				results.epa_iterations = epa.iterations + 1;
+				if (pd > 0) {
+					results.status = ResultsStatus.Penetrating;
+					results.normal.set(epa.normal);
+					results.depth = pd;
+					results.witnesses[0].set(epa.nearest[0]);
+					results.witnesses[1].set(epa.nearest[1]);
+					return (true);
+				}
+				else {
+					if (epa.failed) {
+						results.status = ResultsStatus.EPA_Failed;
+					}
+				}
+			}
+			else {
+				if (gjk.failed) {
+					results.status = ResultsStatus.GJK_Failed;
+				}
+			}
+			return (false);
+		}
+		finally {
+			gjk.destroy();
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/GjkPairDetector.java b/src/jbullet/src/javabullet/collision/narrowphase/GjkPairDetector.java
new file mode 100644
index 0000000..a70a2cd
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/GjkPairDetector.java
@@ -0,0 +1,337 @@
+/*
+ * 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.collision.narrowphase;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javabullet.collision.shapes.ConvexShape;
+import javabullet.linearmath.IDebugDraw;
+import javabullet.linearmath.MatrixUtil;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * GjkPairDetector uses GJK to implement the DiscreteCollisionDetectorInterface.
+ * 
+ * @author jezek2
+ */
+public class GjkPairDetector implements DiscreteCollisionDetectorInterface {
+
+	protected final BulletStack stack = BulletStack.get();
+	
+	// must be above the machine epsilon
+	private static final float REL_ERROR2 = 1.0e-6f;
+	
+	private final Vector3f cachedSeparatingAxis = new Vector3f(0f, 0f, 1f);
+	private ConvexPenetrationDepthSolver penetrationDepthSolver;
+	private SimplexSolverInterface simplexSolver;
+	private ConvexShape minkowskiA;
+	private ConvexShape minkowskiB;
+	private boolean ignoreMargin = false;
+	
+	//some debugging to fix degeneracy problems
+	public int lastUsedMethod = -1;
+	public int curIter;
+	public int degenerateSimplex;
+	public int catchDegeneracies = 1;
+	
+	public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, SimplexSolverInterface simplexSolver, ConvexPenetrationDepthSolver penetrationDepthSolver) {
+		this.penetrationDepthSolver = penetrationDepthSolver;
+		this.simplexSolver = simplexSolver;
+		this.minkowskiA = objectA;
+		this.minkowskiB = objectB;
+	}
+	
+	public void getClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw) {
+		stack.pushCommonMath();
+		try {
+			Vector3f tmp = stack.vectors.get();
+
+			float distance = 0f;
+			Vector3f normalInB = stack.vectors.get(0f, 0f, 0f);
+			Vector3f pointOnA = stack.vectors.get(), pointOnB = stack.vectors.get();
+			Transform localTransA = stack.transforms.get(input.transformA);
+			Transform localTransB = stack.transforms.get(input.transformB);
+			Vector3f positionOffset = stack.vectors.get();
+			positionOffset.add(localTransA.origin, localTransB.origin);
+			positionOffset.scale(0.5f);
+			localTransA.origin.sub(positionOffset);
+			localTransB.origin.sub(positionOffset);
+
+			float marginA = minkowskiA.getMargin();
+			float marginB = minkowskiB.getMargin();
+
+			BulletGlobals.gNumGjkChecks++;
+
+			// for CCD we don't use margins
+			if (ignoreMargin) {
+				marginA = 0f;
+				marginB = 0f;
+			}
+
+			curIter = 0;
+			int gGjkMaxIter = 1000; // this is to catch invalid input, perhaps check for #NaN?
+			cachedSeparatingAxis.set(0f, 1f, 0f);
+
+			boolean isValid = false;
+			boolean checkSimplex = false;
+			boolean checkPenetration = true;
+			degenerateSimplex = 0;
+
+			lastUsedMethod = -1;
+
+			{
+				float squaredDistance = BulletGlobals.SIMD_INFINITY;
+				float delta = 0f;
+
+				float margin = marginA + marginB;
+
+				simplexSolver.reset();
+
+				for (;;) //while (true)
+				{
+					Vector3f seperatingAxisInA = stack.vectors.get();
+					seperatingAxisInA.negate(cachedSeparatingAxis);
+					MatrixUtil.transposeTransform(seperatingAxisInA, seperatingAxisInA, input.transformA.basis);
+
+					Vector3f seperatingAxisInB = stack.vectors.get();
+					seperatingAxisInB.set(cachedSeparatingAxis);
+					MatrixUtil.transposeTransform(seperatingAxisInB, seperatingAxisInB, input.transformB.basis);
+
+					Vector3f pInA = stack.vectors.get(minkowskiA.localGetSupportingVertexWithoutMargin(seperatingAxisInA));
+					Vector3f qInB = stack.vectors.get(minkowskiB.localGetSupportingVertexWithoutMargin(seperatingAxisInB));
+
+					Vector3f pWorld = stack.vectors.get(pInA);
+					localTransA.transform(pWorld);
+
+					Vector3f qWorld = stack.vectors.get(qInB);
+					localTransB.transform(qWorld);
+
+					Vector3f w = stack.vectors.get();
+					w.sub(pWorld, qWorld);
+
+					delta = cachedSeparatingAxis.dot(w);
+
+					// potential exit, they don't overlap
+					if ((delta > 0f) && (delta * delta > squaredDistance * input.maximumDistanceSquared)) {
+						checkPenetration = false;
+						break;
+					}
+
+					// exit 0: the new point is already in the simplex, or we didn't come any closer
+					if (simplexSolver.inSimplex(w)) {
+						degenerateSimplex = 1;
+						checkSimplex = true;
+						break;
+					}
+					// are we getting any closer ?
+					float f0 = squaredDistance - delta;
+					float f1 = squaredDistance * REL_ERROR2;
+
+					if (f0 <= f1) {
+						if (f0 <= 0f) {
+							degenerateSimplex = 2;
+						}
+						checkSimplex = true;
+						break;
+					}
+					// add current vertex to simplex
+					simplexSolver.addVertex(w, pWorld, qWorld);
+
+					// calculate the closest point to the origin (update vector v)
+					if (!simplexSolver.closest(cachedSeparatingAxis)) {
+						degenerateSimplex = 3;
+						checkSimplex = true;
+						break;
+					}
+
+					float previousSquaredDistance = squaredDistance;
+					squaredDistance = cachedSeparatingAxis.lengthSquared();
+
+					// redundant m_simplexSolver->compute_points(pointOnA, pointOnB);
+
+					// are we getting any closer ?
+					if (previousSquaredDistance - squaredDistance <= BulletGlobals.FLT_EPSILON * previousSquaredDistance) {
+						simplexSolver.backup_closest(cachedSeparatingAxis);
+						checkSimplex = true;
+						break;
+					}
+
+					// degeneracy, this is typically due to invalid/uninitialized worldtransforms for a CollisionObject   
+					if (curIter++ > gGjkMaxIter) {
+						//#if defined(DEBUG) || defined (_DEBUG)   
+						if (BulletGlobals.DEBUG) {
+							System.err.printf("btGjkPairDetector maxIter exceeded:%i\n", curIter);
+							System.err.printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n",
+									cachedSeparatingAxis.x,
+									cachedSeparatingAxis.y,
+									cachedSeparatingAxis.z,
+									squaredDistance,
+									minkowskiA.getShapeType(),
+									minkowskiB.getShapeType());
+						}
+						//#endif   
+						break;
+
+					}
+
+					boolean check = (!simplexSolver.fullSimplex());
+					//bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex());
+
+					if (!check) {
+						// do we need this backup_closest here ?
+						simplexSolver.backup_closest(cachedSeparatingAxis);
+						break;
+					}
+				}
+
+				if (checkSimplex) {
+					simplexSolver.compute_points(pointOnA, pointOnB);
+					normalInB.sub(pointOnA, pointOnB);
+					float lenSqr = cachedSeparatingAxis.lengthSquared();
+					// valid normal
+					if (lenSqr < 0.0001f) {
+						degenerateSimplex = 5;
+					}
+					if (lenSqr > BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) {
+						float rlen = 1f / (float) Math.sqrt(lenSqr);
+						normalInB.scale(rlen); // normalize
+						float s = (float) Math.sqrt(squaredDistance);
+
+						assert (s > 0f);
+
+						tmp.scale((marginA / s), cachedSeparatingAxis);
+						pointOnA.sub(tmp);
+
+						tmp.scale((marginB / s), cachedSeparatingAxis);
+						pointOnB.add(tmp);
+
+						distance = ((1f / rlen) - margin);
+						isValid = true;
+
+						lastUsedMethod = 1;
+					}
+					else {
+						lastUsedMethod = 2;
+					}
+				}
+
+				boolean catchDegeneratePenetrationCase =
+						(catchDegeneracies != 0 && penetrationDepthSolver != null && degenerateSimplex != 0 && ((distance + margin) < 0.01f));
+
+				//if (checkPenetration && !isValid)
+				if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) {
+					// penetration case
+
+					// if there is no way to handle penetrations, bail out
+					if (penetrationDepthSolver != null) {
+						// Penetration depth case.
+						Vector3f tmpPointOnA = stack.vectors.get(), tmpPointOnB = stack.vectors.get();
+
+						BulletGlobals.gNumDeepPenetrationChecks++;
+
+						boolean isValid2 = penetrationDepthSolver.calcPenDepth(
+								simplexSolver,
+								minkowskiA, minkowskiB,
+								localTransA, localTransB,
+								cachedSeparatingAxis, tmpPointOnA, tmpPointOnB,
+								debugDraw/*,input.stackAlloc*/);
+
+						if (isValid2) {
+							Vector3f tmpNormalInB = stack.vectors.get();
+							tmpNormalInB.sub(tmpPointOnB, tmpPointOnA);
+
+							float lenSqr = tmpNormalInB.lengthSquared();
+							if (lenSqr > (BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
+								tmpNormalInB.scale(1f / (float) Math.sqrt(lenSqr));
+								tmp.sub(tmpPointOnA, tmpPointOnB);
+								float distance2 = -tmp.length();
+								// only replace valid penetrations when the result is deeper (check)
+								if (!isValid || (distance2 < distance)) {
+									distance = distance2;
+									pointOnA.set(tmpPointOnA);
+									pointOnB.set(tmpPointOnB);
+									normalInB.set(tmpNormalInB);
+									isValid = true;
+									lastUsedMethod = 3;
+								}
+								else {
+
+								}
+							}
+							else {
+								//isValid = false;
+								lastUsedMethod = 4;
+							}
+						}
+						else {
+							lastUsedMethod = 5;
+						}
+
+					}
+				}
+			}
+
+			if (isValid) {
+				//#ifdef __SPU__
+				//		//spu_printf("distance\n");
+				//#endif //__CELLOS_LV2__
+
+				tmp.add(pointOnB, positionOffset);
+				output.addContactPoint(
+						normalInB,
+						tmp,
+						distance);
+			//printf("gjk add:%f",distance);
+			}
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	public void setMinkowskiA(ConvexShape minkA) {
+		minkowskiA = minkA;
+	}
+
+	public void setMinkowskiB(ConvexShape minkB) {
+		minkowskiB = minkB;
+	}
+
+	public void setCachedSeperatingAxis(Vector3f seperatingAxis) {
+		cachedSeparatingAxis.set(seperatingAxis);
+	}
+
+	public void setPenetrationDepthSolver(ConvexPenetrationDepthSolver penetrationDepthSolver) {
+		this.penetrationDepthSolver = penetrationDepthSolver;
+	}
+
+	/**
+	 * Don't use setIgnoreMargin, it's for Bullet's internal use.
+	 */
+	public void setIgnoreMargin(boolean ignoreMargin) {
+		this.ignoreMargin = ignoreMargin;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/ManifoldPoint.java b/src/jbullet/src/javabullet/collision/narrowphase/ManifoldPoint.java
new file mode 100644
index 0000000..beb5a83
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/ManifoldPoint.java
@@ -0,0 +1,98 @@
+/*
+ * 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.collision.narrowphase;
+
+import javax.vecmath.Vector3f;
+
+/**
+ * ManifoldContactPoint collects and maintains persistent contactpoints.
+ * Used to improve stability and performance of rigidbody dynamics response.
+ * 
+ * @author jezek2
+ */
+public class ManifoldPoint {
+
+	public final Vector3f localPointA = new Vector3f();
+	public final Vector3f localPointB = new Vector3f();
+	public final Vector3f positionWorldOnB = new Vector3f();
+	///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity
+	public final Vector3f positionWorldOnA = new Vector3f();
+	public final Vector3f normalWorldOnB = new Vector3f();
+	
+	public float distance1;
+	public float combinedFriction;
+	public float combinedRestitution;
+	
+	public Object userPersistentData;
+	public int lifeTime; //lifetime of the contactpoint in frames
+
+	public ManifoldPoint() {
+	}
+	
+	public ManifoldPoint(Vector3f pointA, Vector3f pointB, Vector3f normal, float distance) {
+		init(pointA, pointB, normal, distance);
+	}
+
+	public void init(Vector3f pointA, Vector3f pointB, Vector3f normal, float distance) {
+		this.localPointA.set(pointA);
+		this.localPointB.set(pointB);
+		this.normalWorldOnB.set(normal);
+		this.distance1 = distance;
+	}
+
+	public float getDistance() {
+		return distance1;
+	}
+
+	public int getLifeTime() {
+		return lifeTime;
+	}
+	
+	public void set(ManifoldPoint p) {
+		localPointA.set(p.localPointA);
+		localPointB.set(p.localPointB);
+		positionWorldOnA.set(p.positionWorldOnA);
+		positionWorldOnB.set(p.positionWorldOnB);
+		normalWorldOnB.set(p.normalWorldOnB);
+		distance1 = p.distance1;
+		combinedFriction = p.combinedFriction;
+		combinedRestitution = p.combinedRestitution;
+		userPersistentData = p.userPersistentData;
+		lifeTime = p.lifeTime;
+	}
+	
+	public Vector3f getPositionWorldOnA() {
+		return positionWorldOnA;
+		//return m_positionWorldOnB + m_normalWorldOnB * m_distance1;
+	}
+
+	public Vector3f getPositionWorldOnB() {
+		return positionWorldOnB;
+	}
+
+	public void setDistance(float dist) {
+		distance1 = dist;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/PersistentManifold.java b/src/jbullet/src/javabullet/collision/narrowphase/PersistentManifold.java
new file mode 100644
index 0000000..93dfb4b
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/PersistentManifold.java
@@ -0,0 +1,373 @@
+/*
+ * 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.collision.narrowphase;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+import javax.vecmath.Vector4f;
+
+
+/**
+ * btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping in the broadphase.
+ * Those contact points are created by the collision narrow phase.
+ * The cache can be empty, or hold 1,2,3 or 4 points. Some collision algorithms (GJK) might only add one point at a time.
+ * updates/refreshes old contact points, and throw them away if necessary (distance becomes too large)
+ * reduces the cache to 4 points, when more then 4 points are added, using following rules:
+ * the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points
+ * note that some pairs of objects might have more then one contact manifold.
+ * 
+ * @author jezek2
+ */
+public class PersistentManifold {
+
+	protected final BulletStack stack = BulletStack.get();
+	
+	public static final int MANIFOLD_CACHE_SIZE = 4;
+	
+	private final ManifoldPoint[] pointCache = new ManifoldPoint[MANIFOLD_CACHE_SIZE];
+	/// this two body pointers can point to the physics rigidbody class.
+	/// void* will allow any rigidbody class
+	private Object body0;
+	private Object body1;
+	private int cachedPoints;
+	
+	public int index1a;
+	
+	{
+		for (int i=0; i<pointCache.length; i++) pointCache[i] = new ManifoldPoint();
+	}
+
+	public PersistentManifold() {
+	}
+
+	public PersistentManifold(Object body0, Object body1, int bla) {
+		init(body0, body1, bla);
+	}
+
+	public void init(Object body0, Object body1, int bla) {
+		this.body0 = body0;
+		this.body1 = body1;
+		cachedPoints = 0;
+		index1a = 0;
+	}
+
+	/// sort cached points so most isolated points come first
+	private int sortCachedPoints(ManifoldPoint pt) {
+		stack.vectors.push();
+		stack.vectors4.push();
+		try {
+			//calculate 4 possible cases areas, and take biggest area
+			//also need to keep 'deepest'
+
+			int maxPenetrationIndex = -1;
+	//#define KEEP_DEEPEST_POINT 1
+	//#ifdef KEEP_DEEPEST_POINT
+			float maxPenetration = pt.getDistance();
+			for (int i = 0; i < 4; i++) {
+				if (pointCache[i].getDistance() < maxPenetration) {
+					maxPenetrationIndex = i;
+					maxPenetration = pointCache[i].getDistance();
+				}
+			}
+	//#endif //KEEP_DEEPEST_POINT
+
+			float res0 = 0f, res1 = 0f, res2 = 0f, res3 = 0f;
+			if (maxPenetrationIndex != 0) {
+				Vector3f a0 = stack.vectors.get(pt.localPointA);
+				a0.sub(pointCache[1].localPointA);
+
+				Vector3f b0 = stack.vectors.get(pointCache[3].localPointA);
+				b0.sub(pointCache[2].localPointA);
+
+				Vector3f cross = stack.vectors.get();
+				cross.cross(a0, b0);
+
+				res0 = cross.lengthSquared();
+			}
+
+			if (maxPenetrationIndex != 1) {
+				Vector3f a1 = stack.vectors.get(pt.localPointA);
+				a1.sub(pointCache[0].localPointA);
+
+				Vector3f b1 = stack.vectors.get(pointCache[3].localPointA);
+				b1.sub(pointCache[2].localPointA);
+
+				Vector3f cross = stack.vectors.get();
+				cross.cross(a1, b1);
+				res1 = cross.lengthSquared();
+			}
+
+			if (maxPenetrationIndex != 2) {
+				Vector3f a2 = stack.vectors.get(pt.localPointA);
+				a2.sub(pointCache[0].localPointA);
+
+				Vector3f b2 = stack.vectors.get(pointCache[3].localPointA);
+				b2.sub(pointCache[1].localPointA);
+
+				Vector3f cross = stack.vectors.get();
+				cross.cross(a2, b2);
+
+				res2 = cross.lengthSquared();
+			}
+
+			if (maxPenetrationIndex != 3) {
+				Vector3f a3 = stack.vectors.get(pt.localPointA);
+				a3.sub(pointCache[0].localPointA);
+
+				Vector3f b3 = stack.vectors.get(pointCache[2].localPointA);
+				b3.sub(pointCache[1].localPointA);
+
+				Vector3f cross = stack.vectors.get();
+				cross.cross(a3, b3);
+				res3 = cross.lengthSquared();
+			}
+
+			Vector4f maxvec = stack.vectors4.get(res0, res1, res2, res3);
+			int biggestarea = VectorUtil.closestAxis4(maxvec);
+			return biggestarea;
+		}
+		finally {
+			stack.vectors.pop();
+			stack.vectors4.pop();
+		}
+	}
+
+	//private int findContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt);
+
+	public Object getBody0() {
+		return body0;
+	}
+
+	public Object getBody1() {
+		return body1;
+	}
+
+	public void setBodies(Object body0, Object body1) {
+		this.body0 = body0;
+		this.body1 = body1;
+	}
+	
+	public void clearUserCache(ManifoldPoint pt) {
+		Object oldPtr = pt.userPersistentData;
+		if (oldPtr != null) {
+//#ifdef DEBUG_PERSISTENCY
+//			int i;
+//			int occurance = 0;
+//			for (i = 0; i < cachedPoints; i++) {
+//				if (pointCache[i].userPersistentData == oldPtr) {
+//					occurance++;
+//					if (occurance > 1) {
+//						throw new InternalError();
+//					}
+//				}
+//			}
+//			assert (occurance <= 0);
+//#endif //DEBUG_PERSISTENCY
+
+			if (pt.userPersistentData != null && BulletGlobals.gContactDestroyedCallback != null) {
+				BulletGlobals.gContactDestroyedCallback.invoke(pt.userPersistentData);
+				pt.userPersistentData = null;
+			}
+
+//#ifdef DEBUG_PERSISTENCY
+//			DebugPersistency();
+//#endif
+		}
+	}
+
+	public int getNumContacts() {
+		return cachedPoints;
+	}
+
+	public ManifoldPoint getContactPoint(int index) {
+		return pointCache[index];
+	}
+
+	// todo: get this margin from the current physics / collision environment
+	public float getContactBreakingThreshold() {
+		return BulletGlobals.gContactBreakingThreshold;
+	}
+
+	public int getCacheEntry(ManifoldPoint newPoint) {
+		stack.vectors.push();
+		try {
+			float shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold();
+			int size = getNumContacts();
+			int nearestPoint = -1;
+			Vector3f diffA = stack.vectors.get();
+			for (int i = 0; i < size; i++) {
+				ManifoldPoint mp = pointCache[i];
+
+				diffA.sub(mp.localPointA, newPoint.localPointA);
+
+				float distToManiPoint = diffA.dot(diffA);
+				if (distToManiPoint < shortestDist) {
+					shortestDist = distToManiPoint;
+					nearestPoint = i;
+				}
+			}
+			return nearestPoint;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void addManifoldPoint(ManifoldPoint newPoint) {
+		assert (validContactDistance(newPoint));
+
+		int insertIndex = getNumContacts();
+		if (insertIndex == MANIFOLD_CACHE_SIZE) {
+			//#if MANIFOLD_CACHE_SIZE >= 4
+			if (MANIFOLD_CACHE_SIZE >= 4) {
+				//sort cache so best points come first, based on area
+				insertIndex = sortCachedPoints(newPoint);
+			}
+			else {
+				//#else
+				insertIndex = 0;
+			}
+			//#endif
+		}
+		else {
+			cachedPoints++;
+		}
+		replaceContactPoint(newPoint, insertIndex);
+	}
+
+	public void removeContactPoint(int index) {
+		clearUserCache(pointCache[index]);
+
+		int lastUsedIndex = getNumContacts() - 1;
+//		m_pointCache[index] = m_pointCache[lastUsedIndex];
+		if (index != lastUsedIndex) {
+			// TODO: possible bug
+			pointCache[index].set(pointCache[lastUsedIndex]);
+			//get rid of duplicated userPersistentData pointer
+			pointCache[lastUsedIndex].userPersistentData = null;
+			pointCache[lastUsedIndex].lifeTime = 0;
+		}
+
+		assert (pointCache[lastUsedIndex].userPersistentData == null);
+		cachedPoints--;
+	}
+
+	public void replaceContactPoint(ManifoldPoint newPoint, int insertIndex) {
+		assert (validContactDistance(newPoint));
+
+//#define MAINTAIN_PERSISTENCY 1
+//#ifdef MAINTAIN_PERSISTENCY
+		int lifeTime = pointCache[insertIndex].getLifeTime();
+		assert (lifeTime >= 0);
+		Object cache = pointCache[insertIndex].userPersistentData;
+
+		pointCache[insertIndex].set(newPoint);
+
+		pointCache[insertIndex].userPersistentData = cache;
+		pointCache[insertIndex].lifeTime = lifeTime;
+//#else
+//		clearUserCache(m_pointCache[insertIndex]);
+//		m_pointCache[insertIndex] = newPoint;
+//#endif
+	}
+
+	private boolean validContactDistance(ManifoldPoint pt) {
+		return pt.distance1 <= getContactBreakingThreshold();
+	}
+
+	/// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
+	public void refreshContactPoints(Transform trA, Transform trB) {
+		stack.vectors.push();
+		try {
+			Vector3f tmp = stack.vectors.get();
+			int i;
+	//#ifdef DEBUG_PERSISTENCY
+	//	printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n",
+	//		trA.getOrigin().getX(),
+	//		trA.getOrigin().getY(),
+	//		trA.getOrigin().getZ(),
+	//		trB.getOrigin().getX(),
+	//		trB.getOrigin().getY(),
+	//		trB.getOrigin().getZ());
+	//#endif //DEBUG_PERSISTENCY
+			// first refresh worldspace positions and distance
+			for (i = getNumContacts() - 1; i >= 0; i--) {
+				ManifoldPoint manifoldPoint = pointCache[i];
+
+				manifoldPoint.positionWorldOnA.set(manifoldPoint.localPointA);
+				trA.transform(manifoldPoint.positionWorldOnA);
+
+				manifoldPoint.positionWorldOnB.set(manifoldPoint.localPointB);
+				trB.transform(manifoldPoint.positionWorldOnB);
+
+				tmp.set(manifoldPoint.positionWorldOnA);
+				tmp.sub(manifoldPoint.positionWorldOnB);
+				manifoldPoint.distance1 = tmp.dot(manifoldPoint.normalWorldOnB);
+
+				manifoldPoint.lifeTime++;
+			}
+
+			// then 
+			float distance2d;
+			Vector3f projectedDifference = stack.vectors.get(), projectedPoint = stack.vectors.get();
+
+			for (i = getNumContacts() - 1; i >= 0; i--) {
+
+				ManifoldPoint manifoldPoint = pointCache[i];
+				// contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
+				if (!validContactDistance(manifoldPoint)) {
+					removeContactPoint(i);
+				}
+				else {
+					// contact also becomes invalid when relative movement orthogonal to normal exceeds margin
+					tmp.scale(manifoldPoint.distance1, manifoldPoint.normalWorldOnB);
+					projectedPoint.sub(manifoldPoint.positionWorldOnA, tmp);
+					projectedDifference.sub(manifoldPoint.positionWorldOnB, projectedPoint);
+					distance2d = projectedDifference.dot(projectedDifference);
+					if (distance2d > getContactBreakingThreshold() * getContactBreakingThreshold()) {
+						removeContactPoint(i);
+					}
+				}
+			}
+	//#ifdef DEBUG_PERSISTENCY
+	//	DebugPersistency();
+	//#endif //
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void clearManifold() {
+		int i;
+		for (i = 0; i < cachedPoints; i++) {
+			clearUserCache(pointCache[i]);
+		}
+		cachedPoints = 0;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/PointCollector.java b/src/jbullet/src/javabullet/collision/narrowphase/PointCollector.java
new file mode 100644
index 0000000..a754515
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/PointCollector.java
@@ -0,0 +1,54 @@
+/*
+ * 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.collision.narrowphase;
+
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class PointCollector implements DiscreteCollisionDetectorInterface.Result {
+
+	public final Vector3f normalOnBInWorld = new Vector3f();
+	public final Vector3f pointInWorld = new Vector3f();
+	public float distance = 1e30f; // negative means penetration
+
+	public boolean hasResult = false;
+	
+	public void setShapeIdentifiers(int partId0, int index0, int partId1, int index1) {
+		// ??
+	}
+
+	public void addContactPoint(Vector3f normalOnBInWorld, Vector3f pointInWorld, float depth) {
+		if (depth < distance) {
+			hasResult = true;
+			this.normalOnBInWorld.set(normalOnBInWorld);
+			this.pointInWorld.set(pointInWorld);
+			// negative means penetration
+			distance = depth;
+		}
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/SimplexSolverInterface.java b/src/jbullet/src/javabullet/collision/narrowphase/SimplexSolverInterface.java
new file mode 100644
index 0000000..13c1a54
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/SimplexSolverInterface.java
@@ -0,0 +1,59 @@
+/*
+ * 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.collision.narrowphase;
+
+import javax.vecmath.Vector3f;
+
+/**
+ * SimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices
+ * Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on
+ * voronoi regions or barycentric coordinates.
+ * 
+ * @author jezek2
+ */
+public interface SimplexSolverInterface {
+
+	public void reset();
+
+	public void addVertex(Vector3f w, Vector3f p, Vector3f q);
+	
+	public boolean closest(Vector3f v);
+
+	public float maxVertex();
+
+	public boolean fullSimplex();
+
+	public int getSimplex(Vector3f[] pBuf, Vector3f[] qBuf, Vector3f[] yBuf);
+
+	public boolean inSimplex(Vector3f w);
+	
+	public void backup_closest(Vector3f v);
+
+	public boolean emptySimplex();
+
+	public void compute_points(Vector3f p1, Vector3f p2);
+
+	public int numVertices();
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/SubsimplexConvexCast.java b/src/jbullet/src/javabullet/collision/narrowphase/SubsimplexConvexCast.java
new file mode 100644
index 0000000..17a7aa8
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/SubsimplexConvexCast.java
@@ -0,0 +1,169 @@
+/*
+ * 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.collision.narrowphase;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javabullet.collision.shapes.ConvexShape;
+import javabullet.collision.shapes.MinkowskiSumShape;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * SubsimplexConvexCast implements Gino van den Bergens' paper
+ * "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
+ * GJK based Ray Cast, optimized version
+ * Objects should not start in overlap, otherwise results are not defined.
+ * 
+ * @author jezek2
+ */
+public class SubsimplexConvexCast implements ConvexCast {
+
+	protected final BulletStack stack = BulletStack.get();
+	
+	// Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
+	// See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
+	//#ifdef BT_USE_DOUBLE_PRECISION
+	//#define MAX_ITERATIONS 64
+	//#else
+	//#define MAX_ITERATIONS 32
+	//#endif
+	
+	private static final int MAX_ITERATIONS = 32;
+	
+	private SimplexSolverInterface simplexSolver;
+	private ConvexShape convexA;
+	private ConvexShape convexB;
+
+	public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, SimplexSolverInterface simplexSolver) {
+		this.convexA = shapeA;
+		this.convexB = shapeB;
+		this.simplexSolver = simplexSolver;
+	}
+	
+	public boolean calcTimeOfImpact(Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result) {
+		stack.pushCommonMath();
+		try {
+			MinkowskiSumShape combi = new MinkowskiSumShape(convexA, convexB);
+			MinkowskiSumShape convex = combi;
+
+			Transform rayFromLocalA = stack.transforms.get();
+			Transform rayToLocalA = stack.transforms.get();
+
+			rayFromLocalA.inverse(fromA);
+			rayFromLocalA.mul(fromB);
+
+			rayToLocalA.inverse(toA);
+			rayToLocalA.mul(toB);
+
+			simplexSolver.reset();
+
+			convex.setTransformB(stack.transforms.get(rayFromLocalA.basis));
+
+			//btScalar radius = btScalar(0.01);
+
+			float lambda = 0f;
+			// todo: need to verify this:
+			// because of minkowski difference, we need the inverse direction
+
+			Vector3f s = stack.vectors.get();
+			s.negate(rayFromLocalA.origin);
+
+			//Vector3f r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin());
+			Vector3f r = stack.vectors.get();
+			r.sub(rayToLocalA.origin, rayFromLocalA.origin);
+			r.negate();
+
+			Vector3f x = stack.vectors.get(s);
+			Vector3f v = stack.vectors.get();
+			Vector3f arbitraryPoint = stack.vectors.get(convex.localGetSupportingVertex(r));
+
+			v.sub(x, arbitraryPoint);
+
+			int maxIter = MAX_ITERATIONS;
+
+			Vector3f n = stack.vectors.get(0f, 0f, 0f);
+			boolean hasResult = false;
+			Vector3f c = stack.vectors.get();
+
+			float lastLambda = lambda;
+
+			float dist2 = v.lengthSquared();
+			//#ifdef BT_USE_DOUBLE_PRECISION
+			//	btScalar epsilon = btScalar(0.0001);
+			//#else
+			float epsilon = 0.0001f;
+			//#endif
+			Vector3f w = stack.vectors.get(), p = stack.vectors.get();
+			float VdotR;
+
+			while ((dist2 > epsilon) && (maxIter--) != 0) {
+				p.set(convex.localGetSupportingVertex(v));
+				w.sub(x, p);
+
+				float VdotW = v.dot(w);
+
+				if (VdotW > 0f) {
+					VdotR = v.dot(r);
+
+					if (VdotR >= -(BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
+						return false;
+					}
+					else {
+						lambda = lambda - VdotW / VdotR;
+						x.scaleAdd(lambda, r, s);
+						simplexSolver.reset();
+						// check next line
+						w.sub(x, p);
+						lastLambda = lambda;
+						n.set(v);
+						hasResult = true;
+					}
+				}
+				simplexSolver.addVertex(w, x, p);
+				if (simplexSolver.closest(v)) {
+					dist2 = v.lengthSquared();
+					hasResult = true;
+					//printf("V=%f , %f, %f\n",v[0],v[1],v[2]);
+					//printf("DIST2=%f\n",dist2);
+					//printf("numverts = %i\n",m_simplexSolver->numVertices());
+				}
+				else {
+					dist2 = 0f;
+				}
+			}
+
+			//int numiter = MAX_ITERATIONS - maxIter;
+			//	printf("number of iterations: %d", numiter);
+			result.fraction = lambda;
+			result.normal.set(n);
+
+			return true;
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/TriangleConvexcastCallback.java b/src/jbullet/src/javabullet/collision/narrowphase/TriangleConvexcastCallback.java
new file mode 100644
index 0000000..430d3a5
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/TriangleConvexcastCallback.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.collision.narrowphase;
+
+import javabullet.collision.narrowphase.ConvexCast.CastResult;
+import javabullet.collision.shapes.ConvexShape;
+import javabullet.collision.shapes.TriangleCallback;
+import javabullet.collision.shapes.TriangleShape;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public abstract class TriangleConvexcastCallback implements TriangleCallback {
+
+	public ConvexShape convexShape;
+	public final Transform convexShapeFrom = new Transform();
+	public final Transform convexShapeTo = new Transform();
+	public final Transform triangleToWorld = new Transform();
+	public float hitFraction;
+
+	public TriangleConvexcastCallback(ConvexShape convexShape, Transform convexShapeFrom, Transform convexShapeTo, Transform triangleToWorld) {
+		this.convexShape = convexShape;
+		this.convexShapeFrom.set(convexShapeFrom);
+		this.convexShapeTo.set(convexShapeTo);
+		this.triangleToWorld.set(triangleToWorld);
+		this.hitFraction = 1f;
+	}
+	
+	public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) {
+		TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);
+
+		VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
+
+		//#define  USE_SUBSIMPLEX_CONVEX_CAST 1
+		//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+		// TODO: implement ContinuousConvexCollision
+		SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(convexShape, triangleShape, simplexSolver);
+		//#else
+		// //btGjkConvexCast	convexCaster(m_convexShape,&triangleShape,&simplexSolver);
+		//btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,NULL);
+		//#endif //#USE_SUBSIMPLEX_CONVEX_CAST
+
+		CastResult castResult = new CastResult();
+		castResult.fraction = 1f;
+		if (convexCaster.calcTimeOfImpact(convexShapeFrom, convexShapeTo, triangleToWorld, triangleToWorld, castResult)) {
+			// add hit
+			if (castResult.normal.lengthSquared() > 0.0001f) {
+				if (castResult.fraction < hitFraction) {
+
+					//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
+					// rotate normal into worldspace
+					convexShapeFrom.basis.transform(castResult.normal);
+					//#endif //USE_SUBSIMPLEX_CONVEX_CAST
+					castResult.normal.normalize();
+
+					reportHit(castResult.normal,
+							castResult.hitPoint,
+							castResult.fraction,
+							partId,
+							triangleIndex);
+				}
+			}
+		}
+	}
+
+	public abstract float reportHit(Vector3f hitNormalLocal, Vector3f hitPointLocal, float hitFraction, int partId, int triangleIndex);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/TriangleRaycastCallback.java b/src/jbullet/src/javabullet/collision/narrowphase/TriangleRaycastCallback.java
new file mode 100644
index 0000000..a5ccf60
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/TriangleRaycastCallback.java
@@ -0,0 +1,128 @@
+/*
+ * 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.collision.narrowphase;
+
+import javabullet.BulletStack;
+import javabullet.collision.shapes.TriangleCallback;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public abstract class TriangleRaycastCallback implements TriangleCallback {
+	
+	protected final BulletStack stack = BulletStack.get();
+
+	public final Vector3f from = new Vector3f();
+	public final Vector3f to = new Vector3f();
+
+	public float hitFraction;
+
+	public TriangleRaycastCallback(Vector3f from, Vector3f to) {
+		this.from.set(from);
+		this.to.set(to);
+		this.hitFraction = 1f;
+	}
+	
+	public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) {
+		stack.vectors.push();
+		try {
+			Vector3f vert0 = triangle[0];
+			Vector3f vert1 = triangle[1];
+			Vector3f vert2 = triangle[2];
+
+			Vector3f v10 = stack.vectors.get();
+			v10.sub(vert1, vert0);
+
+			Vector3f v20 = stack.vectors.get();
+			v20.sub(vert2, vert0);
+
+			Vector3f triangleNormal = stack.vectors.get();
+			triangleNormal.cross(v10, v20);
+
+			float dist = vert0.dot(triangleNormal);
+			float dist_a = triangleNormal.dot(from);
+			dist_a -= dist;
+			float dist_b = triangleNormal.dot(to);
+			dist_b -= dist;
+
+			if (dist_a * dist_b >= 0f) {
+				return; // same sign
+			}
+
+			float proj_length = dist_a - dist_b;
+			float distance = (dist_a) / (proj_length);
+			// Now we have the intersection point on the plane, we'll see if it's inside the triangle
+			// Add an epsilon as a tolerance for the raycast,
+			// in case the ray hits exacly on the edge of the triangle.
+			// It must be scaled for the triangle size.
+
+			if (distance < hitFraction) {
+				float edge_tolerance = triangleNormal.lengthSquared();
+				edge_tolerance *= -0.0001f;
+				Vector3f point = new Vector3f();
+				VectorUtil.setInterpolate3(point, from, to, distance);
+				{
+					Vector3f v0p = stack.vectors.get();
+					v0p.sub(vert0, point);
+					Vector3f v1p = stack.vectors.get();
+					v1p.sub(vert1, point);
+					Vector3f cp0 = stack.vectors.get();
+					cp0.cross(v0p, v1p);
+
+					if (cp0.dot(triangleNormal) >= edge_tolerance) {
+						Vector3f v2p = stack.vectors.get();
+						v2p.sub(vert2, point);
+						Vector3f cp1 = stack.vectors.get();
+						cp1.cross(v1p, v2p);
+						if (cp1.dot(triangleNormal) >= edge_tolerance) {
+							Vector3f cp2 = stack.vectors.get();
+							cp2.cross(v2p, v0p);
+
+							if (cp2.dot(triangleNormal) >= edge_tolerance) {
+
+								if (dist_a > 0f) {
+									hitFraction = reportHit(triangleNormal, distance, partId, triangleIndex);
+								}
+								else {
+									Vector3f tmp = stack.vectors.get();
+									tmp.negate(triangleNormal);
+									hitFraction = reportHit(tmp, distance, partId, triangleIndex);
+								}
+							}
+						}
+					}
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public abstract float reportHit(Vector3f hitNormalLocal, float hitFraction, int partId, int triangleIndex );
+
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/VoronoiSimplexSolver.java b/src/jbullet/src/javabullet/collision/narrowphase/VoronoiSimplexSolver.java
new file mode 100644
index 0000000..ec504e4
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/VoronoiSimplexSolver.java
@@ -0,0 +1,739 @@
+/*
+ * 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.collision.narrowphase;
+
+import javabullet.BulletPool;
+import javabullet.BulletStack;
+import javabullet.ObjectPool;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class VoronoiSimplexSolver implements SimplexSolverInterface {
+
+	protected final BulletStack stack = BulletStack.get();
+	protected final ObjectPool<SubSimplexClosestResult> subsimplexResultsPool = BulletPool.get(SubSimplexClosestResult.class);
+	
+	private static final int VORONOI_SIMPLEX_MAX_VERTS = 5;
+	
+	private static final int VERTA = 0;
+	private static final int VERTB = 1;
+	private static final int VERTC = 2;
+	private static final int VERTD = 3;
+
+	public int numVertices;
+
+	public final Vector3f[] simplexVectorW = new Vector3f[VORONOI_SIMPLEX_MAX_VERTS];
+	public final Vector3f[] simplexPointsP = new Vector3f[VORONOI_SIMPLEX_MAX_VERTS];
+	public final Vector3f[] simplexPointsQ = new Vector3f[VORONOI_SIMPLEX_MAX_VERTS];
+
+	public final Vector3f cachedP1 = new Vector3f();
+	public final Vector3f cachedP2 = new Vector3f();
+	public final Vector3f cachedV = new Vector3f();
+	public final Vector3f lastW = new Vector3f();
+	public boolean cachedValidClosest;
+
+	public final SubSimplexClosestResult cachedBC = new SubSimplexClosestResult();
+
+	public boolean needsUpdate;
+	
+	{
+		for (int i=0; i<VORONOI_SIMPLEX_MAX_VERTS; i++) {
+			simplexVectorW[i] = new Vector3f();
+			simplexPointsP[i] = new Vector3f();
+			simplexPointsQ[i] = new Vector3f();
+		}
+	}
+
+	public void removeVertex(int index) {
+		assert(numVertices>0);
+		numVertices--;
+		simplexVectorW[index].set(simplexVectorW[numVertices]);
+		simplexPointsP[index].set(simplexPointsP[numVertices]);
+		simplexPointsQ[index].set(simplexPointsQ[numVertices]);
+	}
+	
+	public void	reduceVertices(UsageBitfield usedVerts) {
+		if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
+			removeVertex(3);
+
+		if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
+			removeVertex(2);
+
+		if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
+			removeVertex(1);
+
+		if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
+			removeVertex(0);
+	}
+	
+	public boolean updateClosestVectorAndPoints() {
+		stack.vectors.push();
+		try {
+			if (needsUpdate)
+			{
+				Vector3f tmp = stack.vectors.get();
+				Vector3f tmp1 = stack.vectors.get();
+				Vector3f tmp2 = stack.vectors.get();
+				Vector3f tmp3 = stack.vectors.get();
+				Vector3f tmp4 = stack.vectors.get();
+
+				cachedBC.reset();
+
+				needsUpdate = false;
+
+				switch (numVertices())
+				{
+				case 0:
+						cachedValidClosest = false;
+						break;
+				case 1:
+					{
+						cachedP1.set(simplexPointsP[0]);
+						cachedP2.set(simplexPointsQ[0]);
+						cachedV.sub(cachedP1, cachedP2); //== m_simplexVectorW[0]
+						cachedBC.reset();
+						cachedBC.setBarycentricCoordinates(1f, 0f, 0f, 0f);
+						cachedValidClosest = cachedBC.isValid();
+						break;
+					}
+				case 2:
+					{
+						//closest point origin from line segment
+						Vector3f from = simplexVectorW[0];
+						Vector3f to = simplexVectorW[1];
+						Vector3f nearest = stack.vectors.get();
+
+						Vector3f p = stack.vectors.get(0f, 0f, 0f);
+						Vector3f diff = stack.vectors.get();
+						diff.sub(p, from);
+
+						Vector3f v = stack.vectors.get();
+						v.sub(to, from);
+
+						float t = v.dot(diff);
+
+						if (t > 0) {
+							float dotVV = v.dot(v);
+							if (t < dotVV) {
+								t /= dotVV;
+								tmp.scale(t, v);
+								diff.sub(tmp);
+								cachedBC.usedVertices.usedVertexA = true;
+								cachedBC.usedVertices.usedVertexB = true;
+							} else {
+								t = 1;
+								diff.sub(v);
+								// reduce to 1 point
+								cachedBC.usedVertices.usedVertexB = true;
+							}
+						} else
+						{
+							t = 0;
+							//reduce to 1 point
+							cachedBC.usedVertices.usedVertexA = true;
+						}
+						cachedBC.setBarycentricCoordinates(1f-t, t, 0f, 0f);
+						tmp.scale(t, v);
+						nearest.add(from, tmp);
+
+						tmp.sub(simplexPointsP[1], simplexPointsP[0]);
+						tmp.scale(t);
+						cachedP1.add(simplexPointsP[0], tmp);
+
+						tmp.sub(simplexPointsQ[1], simplexPointsQ[0]);
+						tmp.scale(t);
+						cachedP2.add(simplexPointsQ[0], tmp);
+
+						cachedV.sub(cachedP1, cachedP2);
+
+						reduceVertices(cachedBC.usedVertices);
+
+						cachedValidClosest = cachedBC.isValid();
+						break;
+					}
+				case 3: 
+					{ 
+						// closest point origin from triangle 
+						Vector3f p = stack.vectors.get(0f, 0f, 0f);
+
+						Vector3f a = simplexVectorW[0]; 
+						Vector3f b = simplexVectorW[1]; 
+						Vector3f c = simplexVectorW[2]; 
+
+						closestPtPointTriangle(p,a,b,c,cachedBC);
+
+						tmp1.scale(cachedBC.barycentricCoords[0], simplexPointsP[0]);
+						tmp2.scale(cachedBC.barycentricCoords[1], simplexPointsP[1]);
+						tmp3.scale(cachedBC.barycentricCoords[2], simplexPointsP[2]);
+						VectorUtil.add(cachedP1, tmp1, tmp2, tmp3);
+
+						tmp1.scale(cachedBC.barycentricCoords[0], simplexPointsQ[0]);
+						tmp2.scale(cachedBC.barycentricCoords[1], simplexPointsQ[1]);
+						tmp3.scale(cachedBC.barycentricCoords[2], simplexPointsQ[2]);
+						VectorUtil.add(cachedP2, tmp1, tmp2, tmp3);
+
+						cachedV.sub(cachedP1, cachedP2);
+
+						reduceVertices(cachedBC.usedVertices);
+						cachedValidClosest = cachedBC.isValid(); 
+
+						break; 
+					}
+				case 4:
+					{
+						Vector3f p = stack.vectors.get(0f, 0f, 0f);
+
+						Vector3f a = simplexVectorW[0];
+						Vector3f b = simplexVectorW[1];
+						Vector3f c = simplexVectorW[2];
+						Vector3f d = simplexVectorW[3];
+
+						boolean hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,cachedBC);
+
+						if (hasSeperation)
+						{
+							tmp1.scale(cachedBC.barycentricCoords[0], simplexPointsP[0]);
+							tmp2.scale(cachedBC.barycentricCoords[1], simplexPointsP[1]);
+							tmp3.scale(cachedBC.barycentricCoords[2], simplexPointsP[2]);
+							tmp4.scale(cachedBC.barycentricCoords[3], simplexPointsP[3]);
+							VectorUtil.add(cachedP1, tmp1, tmp2, tmp3, tmp4);
+
+							tmp1.scale(cachedBC.barycentricCoords[0], simplexPointsQ[0]);
+							tmp2.scale(cachedBC.barycentricCoords[1], simplexPointsQ[1]);
+							tmp3.scale(cachedBC.barycentricCoords[2], simplexPointsQ[2]);
+							tmp4.scale(cachedBC.barycentricCoords[3], simplexPointsQ[3]);
+							VectorUtil.add(cachedP2, tmp1, tmp2, tmp3, tmp4);
+
+							cachedV.sub(cachedP1, cachedP2);
+							reduceVertices (cachedBC.usedVertices);
+						} else
+						{
+		//					printf("sub distance got penetration\n");
+
+							if (cachedBC.degenerate)
+							{
+								cachedValidClosest = false;
+							} else
+							{
+								cachedValidClosest = true;
+								//degenerate case == false, penetration = true + zero
+								cachedV.set(0f, 0f, 0f);
+							}
+							break;
+						}
+
+						cachedValidClosest = cachedBC.isValid();
+
+						//closest point origin from tetrahedron
+						break;
+					}
+				default:
+					{
+						cachedValidClosest = false;
+					}
+				}
+			}
+
+			return cachedValidClosest;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public boolean closestPtPointTriangle(Vector3f p, Vector3f a, Vector3f b, Vector3f c, SubSimplexClosestResult result) {
+		stack.vectors.push();
+		try {
+			result.usedVertices.reset();
+
+			// Check if P in vertex region outside A
+			Vector3f ab = stack.vectors.get();
+			ab.sub(b, a);
+
+			Vector3f ac = stack.vectors.get();
+			ac.sub(c, a);
+
+			Vector3f ap = stack.vectors.get();
+			ap.sub(p, a);
+
+			float d1 = ab.dot(ap);
+			float d2 = ac.dot(ap);
+
+			if (d1 <= 0f && d2 <= 0f) 
+			{
+				result.closestPointOnSimplex.set(a);
+				result.usedVertices.usedVertexA = true;
+				result.setBarycentricCoordinates(1f, 0f, 0f, 0f);
+				return true; // a; // barycentric coordinates (1,0,0)
+			}
+
+			// Check if P in vertex region outside B
+			Vector3f bp = stack.vectors.get();
+			bp.sub(p, b);
+
+			float d3 = ab.dot(bp);
+			float d4 = ac.dot(bp);
+
+			if (d3 >= 0f && d4 <= d3) 
+			{
+				result.closestPointOnSimplex.set(b);
+				result.usedVertices.usedVertexB = true;
+				result.setBarycentricCoordinates(0, 1f, 0f, 0f);
+
+				return true; // b; // barycentric coordinates (0,1,0)
+			}
+
+			// Check if P in edge region of AB, if so return projection of P onto AB
+			float vc = d1*d4 - d3*d2;
+			if (vc <= 0f && d1 >= 0f && d3 <= 0f) {
+				float v = d1 / (d1 - d3);
+				result.closestPointOnSimplex.scaleAdd(v, ab, a);
+				result.usedVertices.usedVertexA = true;
+				result.usedVertices.usedVertexB = true;
+				result.setBarycentricCoordinates(1f-v, v, 0f, 0f);
+				return true;
+				//return a + v * ab; // barycentric coordinates (1-v,v,0)
+			}
+
+			// Check if P in vertex region outside C
+			Vector3f cp = stack.vectors.get();
+			cp.sub(p, c);
+
+			float d5 = ab.dot(cp);
+			float d6 = ac.dot(cp);
+
+			if (d6 >= 0f && d5 <= d6) 
+			{
+				result.closestPointOnSimplex.set(c);
+				result.usedVertices.usedVertexC = true;
+				result.setBarycentricCoordinates(0f, 0f, 1f, 0f);
+				return true;//c; // barycentric coordinates (0,0,1)
+			}
+
+			// Check if P in edge region of AC, if so return projection of P onto AC
+			float vb = d5*d2 - d1*d6;
+			if (vb <= 0f && d2 >= 0f && d6 <= 0f) {
+				float w = d2 / (d2 - d6);
+				result.closestPointOnSimplex.scaleAdd(w, ac, a);
+				result.usedVertices.usedVertexA = true;
+				result.usedVertices.usedVertexC = true;
+				result.setBarycentricCoordinates(1f-w, 0f, w, 0f);
+				return true;
+				//return a + w * ac; // barycentric coordinates (1-w,0,w)
+			}
+
+			// Check if P in edge region of BC, if so return projection of P onto BC
+			float va = d3*d6 - d5*d4;
+			if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f) {
+				float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+
+				Vector3f tmp = stack.vectors.get();
+				tmp.sub(c, b);
+				result.closestPointOnSimplex.scaleAdd(w, tmp, b);
+
+				result.usedVertices.usedVertexB = true;
+				result.usedVertices.usedVertexC = true;
+				result.setBarycentricCoordinates(0, 1f-w, w, 0f);
+				return true;		
+			   // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
+			}
+
+			// P inside face region. Compute Q through its barycentric coordinates (u,v,w)
+			float denom = 1f / (va + vb + vc);
+			float v = vb * denom;
+			float w = vc * denom;
+
+			Vector3f tmp1 = stack.vectors.get();
+			Vector3f tmp2 = stack.vectors.get();
+
+			tmp1.scale(v, ab);
+			tmp2.scale(w, ac);
+			VectorUtil.add(result.closestPointOnSimplex, a, tmp1, tmp2);
+			result.usedVertices.usedVertexA = true;
+			result.usedVertices.usedVertexB = true;
+			result.usedVertices.usedVertexC = true;
+			result.setBarycentricCoordinates(1f-v-w, v, w, 0f);
+
+			return true;
+		//	return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	/// Test if point p and d lie on opposite sides of plane through abc
+	public /*static*/ int pointOutsideOfPlane(Vector3f p, Vector3f a, Vector3f b, Vector3f c, Vector3f d)
+	{
+		stack.vectors.push();
+		try {
+			Vector3f tmp = stack.vectors.get();
+
+			Vector3f normal = stack.vectors.get();
+			normal.sub(b, a);
+			tmp.sub(c, a);
+			normal.cross(normal, tmp);
+
+			tmp.sub(p, a);
+			float signp = tmp.dot(normal); // [AP AB AC]
+
+			tmp.sub(d, a);
+			float signd = tmp.dot(normal); // [AD AB AC]
+
+		//#ifdef CATCH_DEGENERATE_TETRAHEDRON
+	//	#ifdef BT_USE_DOUBLE_PRECISION
+	//	if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
+	//		{
+	//			return -1;
+	//		}
+	//	#else
+			if (signd * signd < ((1e-4f) * (1e-4f)))
+			{
+		//		printf("affine dependent/degenerate\n");//
+				return -1;
+			}
+		//#endif
+
+		//#endif
+			// Points on opposite sides if expression signs are opposite
+			return (signp * signd < 0f)? 1 : 0;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public boolean closestPtPointTetrahedron(Vector3f p, Vector3f a, Vector3f b, Vector3f c, Vector3f d, SubSimplexClosestResult finalResult) {
+		stack.vectors.push();
+		SubSimplexClosestResult tempResult = subsimplexResultsPool.get();
+		tempResult.reset();
+		try {
+			Vector3f tmp = stack.vectors.get();
+			Vector3f tmp1 = stack.vectors.get();
+			Vector3f tmp2 = stack.vectors.get();
+
+			// Start out assuming point inside all halfspaces, so closest to itself
+			finalResult.closestPointOnSimplex.set(p);
+			finalResult.usedVertices.reset();
+			finalResult.usedVertices.usedVertexA = true;
+			finalResult.usedVertices.usedVertexB = true;
+			finalResult.usedVertices.usedVertexC = true;
+			finalResult.usedVertices.usedVertexD = true;
+
+			int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
+			int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
+			int	pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
+			int	pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
+
+		   if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
+		   {
+			   finalResult.degenerate = true;
+			   return false;
+		   }
+
+		   if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0)
+			 {
+				 return false;
+			 }
+
+
+			float bestSqDist = Float.MAX_VALUE;
+			// If point outside face abc then compute closest point on abc
+			if (pointOutsideABC != 0) 
+			{
+				closestPtPointTriangle(p, a, b, c,tempResult);
+				Vector3f q = stack.vectors.get(tempResult.closestPointOnSimplex);
+
+				tmp.sub(q, p);
+				float sqDist = tmp.dot(tmp);
+				// Update best closest point if (squared) distance is less than current best
+				if (sqDist < bestSqDist) {
+					bestSqDist = sqDist;
+					finalResult.closestPointOnSimplex.set(q);
+					//convert result bitmask!
+					finalResult.usedVertices.reset();
+					finalResult.usedVertices.usedVertexA = tempResult.usedVertices.usedVertexA;
+					finalResult.usedVertices.usedVertexB = tempResult.usedVertices.usedVertexB;
+					finalResult.usedVertices.usedVertexC = tempResult.usedVertices.usedVertexC;
+					finalResult.setBarycentricCoordinates(
+							tempResult.barycentricCoords[VERTA],
+							tempResult.barycentricCoords[VERTB],
+							tempResult.barycentricCoords[VERTC],
+							0
+					);
+
+				}
+			}
+
+
+			// Repeat test for face acd
+			if (pointOutsideACD != 0) 
+			{
+				closestPtPointTriangle(p, a, c, d,tempResult);
+				Vector3f q = stack.vectors.get(tempResult.closestPointOnSimplex);
+				//convert result bitmask!
+
+				tmp.sub(q, p);
+				float sqDist = tmp.dot(tmp);
+				if (sqDist < bestSqDist) 
+				{
+					bestSqDist = sqDist;
+					finalResult.closestPointOnSimplex.set(q);
+					finalResult.usedVertices.reset();
+					finalResult.usedVertices.usedVertexA = tempResult.usedVertices.usedVertexA;
+
+					finalResult.usedVertices.usedVertexC = tempResult.usedVertices.usedVertexB;
+					finalResult.usedVertices.usedVertexD = tempResult.usedVertices.usedVertexC;
+					finalResult.setBarycentricCoordinates(
+							tempResult.barycentricCoords[VERTA],
+							0,
+							tempResult.barycentricCoords[VERTB],
+							tempResult.barycentricCoords[VERTC]
+					);
+
+				}
+			}
+			// Repeat test for face adb
+
+
+			if (pointOutsideADB != 0)
+			{
+				closestPtPointTriangle(p, a, d, b,tempResult);
+				Vector3f q = stack.vectors.get(tempResult.closestPointOnSimplex);
+				//convert result bitmask!
+
+				tmp.sub(q, p);
+				float sqDist = tmp.dot(tmp);
+				if (sqDist < bestSqDist) 
+				{
+					bestSqDist = sqDist;
+					finalResult.closestPointOnSimplex.set(q);
+					finalResult.usedVertices.reset();
+					finalResult.usedVertices.usedVertexA = tempResult.usedVertices.usedVertexA;
+					finalResult.usedVertices.usedVertexB = tempResult.usedVertices.usedVertexC;
+
+					finalResult.usedVertices.usedVertexD = tempResult.usedVertices.usedVertexB;
+					finalResult.setBarycentricCoordinates(
+							tempResult.barycentricCoords[VERTA],
+							tempResult.barycentricCoords[VERTC],
+							0,
+							tempResult.barycentricCoords[VERTB]
+					);
+
+				}
+			}
+			// Repeat test for face bdc
+
+
+			if (pointOutsideBDC != 0)
+			{
+				closestPtPointTriangle(p, b, d, c,tempResult);
+				Vector3f q = stack.vectors.get(tempResult.closestPointOnSimplex);
+				//convert result bitmask!
+				tmp.sub(q, p);
+				float sqDist = tmp.dot(tmp);
+				if (sqDist < bestSqDist) 
+				{
+					bestSqDist = sqDist;
+					finalResult.closestPointOnSimplex.set(q);
+					finalResult.usedVertices.reset();
+					//
+					finalResult.usedVertices.usedVertexB = tempResult.usedVertices.usedVertexA;
+					finalResult.usedVertices.usedVertexC = tempResult.usedVertices.usedVertexC;
+					finalResult.usedVertices.usedVertexD = tempResult.usedVertices.usedVertexB;
+
+					finalResult.setBarycentricCoordinates(
+							0,
+							tempResult.barycentricCoords[VERTA],
+							tempResult.barycentricCoords[VERTC],
+							tempResult.barycentricCoords[VERTB]
+					);
+
+				}
+			}
+
+			//help! we ended up full !
+
+			if (finalResult.usedVertices.usedVertexA &&
+				finalResult.usedVertices.usedVertexB &&
+				finalResult.usedVertices.usedVertexC &&
+				finalResult.usedVertices.usedVertexD) 
+			{
+				return true;
+			}
+
+			return true;
+		}
+		finally {
+			stack.vectors.pop();
+			subsimplexResultsPool.release(tempResult);
+		}
+	}
+	
+	/**
+	 * Clear the simplex, remove all the vertices.
+	 */
+	public void reset() {
+		cachedValidClosest = false;
+		numVertices = 0;
+		needsUpdate = true;
+		lastW.set(1e30f, 1e30f, 1e30f);
+		cachedBC.reset();
+	}
+
+	public void addVertex(Vector3f w, Vector3f p, Vector3f q) {
+		lastW.set(w);
+		needsUpdate = true;
+
+		simplexVectorW[numVertices].set(w);
+		simplexPointsP[numVertices].set(p);
+		simplexPointsQ[numVertices].set(q);
+
+		numVertices++;
+	}
+
+	/**
+	 * Return/calculate the closest vertex.
+	 */
+	public boolean closest(Vector3f v) {
+		boolean succes = updateClosestVectorAndPoints();
+		v.set(cachedV);
+		return succes;
+	}
+
+	public float maxVertex() {
+		int i, numverts = numVertices();
+		float maxV = 0f;
+		for (i = 0; i < numverts; i++) {
+			float curLen2 = simplexVectorW[i].lengthSquared();
+			if (maxV < curLen2) {
+				maxV = curLen2;
+			}
+		}
+		return maxV;
+	}
+
+	public boolean fullSimplex() {
+		return (numVertices == 4);
+	}
+
+	public int getSimplex(Vector3f[] pBuf, Vector3f[] qBuf, Vector3f[] yBuf) {
+		for (int i = 0; i < numVertices(); i++) {
+			yBuf[i].set(simplexVectorW[i]);
+			pBuf[i].set(simplexPointsP[i]);
+			qBuf[i].set(simplexPointsQ[i]);
+		}
+		return numVertices();
+	}
+
+	public boolean inSimplex(Vector3f w) {
+		boolean found = false;
+		int i, numverts = numVertices();
+		//btScalar maxV = btScalar(0.);
+
+		//w is in the current (reduced) simplex
+		for (i = 0; i < numverts; i++) {
+			if (simplexVectorW[i].equals(w)) {
+				found = true;
+			}
+		}
+
+		//check in case lastW is already removed
+		if (w.equals(lastW)) {
+			return true;
+		}
+
+		return found;
+	}
+
+	public void backup_closest(Vector3f v) {
+		v.set(cachedV);
+	}
+
+	public boolean emptySimplex() {
+		return (numVertices() == 0);
+	}
+
+	public void compute_points(Vector3f p1, Vector3f p2) {
+		updateClosestVectorAndPoints();
+		p1.set(cachedP1);
+		p2.set(cachedP2);
+	}
+
+	public int numVertices() {
+		return numVertices;
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	public static class UsageBitfield {
+		public boolean usedVertexA;
+		public boolean usedVertexB;
+		public boolean usedVertexC;
+		public boolean usedVertexD;
+		
+		public void reset() {
+			usedVertexA = false;
+			usedVertexB = false;
+			usedVertexC = false;
+			usedVertexD = false;
+		}
+	}
+	
+	public static class SubSimplexClosestResult {
+		public final Vector3f closestPointOnSimplex = new Vector3f();
+		//MASK for m_usedVertices
+		//stores the simplex vertex-usage, using the MASK, 
+		// if m_usedVertices & MASK then the related vertex is used
+		public final UsageBitfield usedVertices = new UsageBitfield();
+		public final float[] barycentricCoords = new float[4];
+		public boolean degenerate;
+		
+		public void reset() {
+			degenerate = false;
+			setBarycentricCoordinates(0f, 0f, 0f, 0f);
+			usedVertices.reset();
+		}
+
+		public boolean isValid() {
+			boolean valid = (barycentricCoords[0] >= 0f) &&
+					(barycentricCoords[1] >= 0f) &&
+					(barycentricCoords[2] >= 0f) &&
+					(barycentricCoords[3] >= 0f);
+			return valid;
+		}
+
+		public void setBarycentricCoordinates(float a, float b, float c, float d) {
+			barycentricCoords[0] = a;
+			barycentricCoords[1] = b;
+			barycentricCoords[2] = c;
+			barycentricCoords[3] = d;
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/narrowphase/package-info.java b/src/jbullet/src/javabullet/collision/narrowphase/package-info.java
new file mode 100644
index 0000000..cafa61a
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/narrowphase/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.
+ */
+
+/**
+ * Narrow-phase collision.
+ */
+package javabullet.collision.narrowphase;
+
diff --git a/src/jbullet/src/javabullet/collision/shapes/BoxShape.java b/src/jbullet/src/javabullet/collision/shapes/BoxShape.java
new file mode 100644
index 0000000..68dd46d
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/BoxShape.java
@@ -0,0 +1,400 @@
+/*
+ * 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.collision.shapes;
+
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.linearmath.MatrixUtil;
+import javabullet.linearmath.ScalarUtil;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Vector3f;
+import javax.vecmath.Vector4f;
+
+/**
+ * BoxShape implements both a feature based (vertex/edge/plane) and implicit (getSupportingVertex) Box.
+ * 
+ * @author jezek2
+ */
+public class BoxShape extends PolyhedralConvexShape {
+
+	public BoxShape(Vector3f boxHalfExtents) {
+		Vector3f margin = new Vector3f(getMargin(), getMargin(), getMargin());
+		VectorUtil.mul(implicitShapeDimensions, boxHalfExtents, localScaling);
+		implicitShapeDimensions.sub(margin);
+	}
+
+	public Vector3f getHalfExtentsWithMargin() {
+		stack.vectors.push();
+		try {
+			Vector3f halfExtents = stack.vectors.get(getHalfExtentsWithoutMargin());
+			Vector3f margin = stack.vectors.get(getMargin(), getMargin(), getMargin());
+			halfExtents.add(margin);
+			return stack.vectors.returning(halfExtents);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public Vector3f getHalfExtentsWithoutMargin() {
+		return implicitShapeDimensions; // changed in Bullet 2.63: assume the scaling and margin are included
+	}
+
+	@Override
+	public BroadphaseNativeType getShapeType() {
+		return BroadphaseNativeType.BOX_SHAPE_PROXYTYPE;
+	}
+
+	@Override
+	public Vector3f localGetSupportingVertex(Vector3f vec) {
+		stack.vectors.push();
+		try {
+			Vector3f halfExtents = stack.vectors.get(getHalfExtentsWithoutMargin());
+			Vector3f margin = stack.vectors.get(getMargin(), getMargin(), getMargin());
+			halfExtents.add(margin);
+
+			return stack.vectors.returning(stack.vectors.get(
+					ScalarUtil.fsel(vec.x, halfExtents.x, -halfExtents.x),
+					ScalarUtil.fsel(vec.y, halfExtents.y, -halfExtents.y),
+					ScalarUtil.fsel(vec.z, halfExtents.z, -halfExtents.z)));
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) {
+		stack.vectors.push();
+		try {
+			Vector3f halfExtents = stack.vectors.get(getHalfExtentsWithoutMargin());
+
+			return stack.vectors.returning(stack.vectors.get(
+					ScalarUtil.fsel(vec.x, halfExtents.x, -halfExtents.x),
+					ScalarUtil.fsel(vec.y, halfExtents.y, -halfExtents.y),
+					ScalarUtil.fsel(vec.z, halfExtents.z, -halfExtents.z)));
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
+		stack.vectors.push();
+		try {
+			Vector3f halfExtents = stack.vectors.get(getHalfExtentsWithoutMargin());
+
+			for (int i = 0; i < numVectors; i++) {
+				Vector3f vec = vectors[i];
+				supportVerticesOut[i].set(ScalarUtil.fsel(vec.x, halfExtents.x, -halfExtents.x),
+						ScalarUtil.fsel(vec.y, halfExtents.y, -halfExtents.y),
+						ScalarUtil.fsel(vec.z, halfExtents.z, -halfExtents.z));
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void setMargin(float margin) {
+		stack.vectors.push();
+		try {
+			// correct the implicitShapeDimensions for the margin
+			Vector3f oldMargin = stack.vectors.get(getMargin(), getMargin(), getMargin());
+			Vector3f implicitShapeDimensionsWithMargin = stack.vectors.get();
+			implicitShapeDimensionsWithMargin.add(implicitShapeDimensions, oldMargin);
+
+			super.setMargin(margin);
+			Vector3f newMargin = stack.vectors.get(getMargin(), getMargin(), getMargin());
+			implicitShapeDimensions.sub(implicitShapeDimensionsWithMargin, newMargin);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void setLocalScaling(Vector3f scaling) {
+		stack.vectors.push();
+		try {
+			Vector3f oldMargin = stack.vectors.get(getMargin(), getMargin(), getMargin());
+			Vector3f implicitShapeDimensionsWithMargin = stack.vectors.get();
+			implicitShapeDimensionsWithMargin.add(implicitShapeDimensions, oldMargin);
+			Vector3f unScaledImplicitShapeDimensionsWithMargin = stack.vectors.get();
+			VectorUtil.div(unScaledImplicitShapeDimensionsWithMargin, implicitShapeDimensionsWithMargin, localScaling);
+
+			super.setLocalScaling(scaling);
+
+			VectorUtil.mul(implicitShapeDimensions, unScaledImplicitShapeDimensionsWithMargin, localScaling);
+			implicitShapeDimensions.sub(oldMargin);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) {
+		stack.pushCommonMath();
+		try {
+			Vector3f halfExtents = getHalfExtentsWithoutMargin();
+
+			Matrix3f abs_b = stack.matrices.get(t.basis);
+			MatrixUtil.absolute(abs_b);
+
+			Vector3f tmp = stack.vectors.get();
+
+			Vector3f center = stack.vectors.get(t.origin);
+			Vector3f extent = stack.vectors.get();
+			abs_b.getRow(0, tmp);
+			extent.x = tmp.dot(halfExtents);
+			abs_b.getRow(1, tmp);
+			extent.y = tmp.dot(halfExtents);
+			abs_b.getRow(2, tmp);
+			extent.z = tmp.dot(halfExtents);
+
+			extent.add(stack.vectors.get(getMargin(), getMargin(), getMargin()));
+
+			aabbMin.sub(center, extent);
+			aabbMax.add(center, extent);
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	@Override
+	public void calculateLocalInertia(float mass, Vector3f inertia) {
+		stack.vectors.push();
+		try {
+			//btScalar margin = btScalar(0.);
+			Vector3f halfExtents = stack.vectors.get(getHalfExtentsWithMargin());
+
+			float lx = 2f * halfExtents.x;
+			float ly = 2f * halfExtents.y;
+			float lz = 2f * halfExtents.z;
+
+			inertia.set(mass / 12f * (ly * ly + lz * lz),
+					mass / 12f * (lx * lx + lz * lz),
+					mass / 12f * (lx * lx + ly * ly));
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void getPlane(Vector3f planeNormal, Vector3f planeSupport, int i) {
+		stack.vectors.push();
+		stack.vectors4.push();
+		try {
+			// this plane might not be aligned...
+			Vector4f plane = stack.vectors4.get();
+			getPlaneEquation(plane, i);
+			planeNormal.set(plane.x, plane.y, plane.z);
+			Vector3f tmp = stack.vectors.get();
+			tmp.negate(planeNormal);
+			planeSupport.set(localGetSupportingVertex(tmp));
+		}
+		finally {
+			stack.vectors.pop();
+			stack.vectors4.pop();
+		}
+	}
+
+	@Override
+	public int getNumPlanes() {
+		return 6;
+	}
+
+	@Override
+	public int getNumVertices() {
+		return 8;
+	}
+
+	@Override
+	public int getNumEdges() {
+		return 12;
+	}
+
+	@Override
+	public void getVertex(int i, Vector3f vtx) {
+		// JAVA NOTE: against stack usage, but safe with code below
+		Vector3f halfExtents = getHalfExtentsWithoutMargin();
+
+		vtx.set(halfExtents.x * (1 - (i & 1)) - halfExtents.x * (i & 1),
+				halfExtents.y * (1 - ((i & 2) >> 1)) - halfExtents.y * ((i & 2) >> 1),
+				halfExtents.z * (1 - ((i & 4) >> 2)) - halfExtents.z * ((i & 4) >> 2));
+	}
+	
+	public void getPlaneEquation(Vector4f plane, int i) {
+		// JAVA NOTE: against stack usage, but safe with code below
+		Vector3f halfExtents = getHalfExtentsWithoutMargin();
+
+		switch (i) {
+			case 0:
+				plane.set(1f, 0f, 0f, -halfExtents.x);
+				break;
+			case 1:
+				plane.set(-1f, 0f, 0f, -halfExtents.x);
+				break;
+			case 2:
+				plane.set(0f, 1f, 0f, -halfExtents.y);
+				break;
+			case 3:
+				plane.set(0f, -1f, 0f, -halfExtents.y);
+				break;
+			case 4:
+				plane.set(0f, 0f, 1f, -halfExtents.z);
+				break;
+			case 5:
+				plane.set(0f, 0f, -1f, -halfExtents.z);
+				break;
+			default:
+				assert (false);
+		}
+	}
+
+	@Override
+	public void getEdge(int i, Vector3f pa, Vector3f pb) {
+		int edgeVert0 = 0;
+		int edgeVert1 = 0;
+
+		switch (i) {
+			case 0:
+				edgeVert0 = 0;
+				edgeVert1 = 1;
+				break;
+			case 1:
+				edgeVert0 = 0;
+				edgeVert1 = 2;
+				break;
+			case 2:
+				edgeVert0 = 1;
+				edgeVert1 = 3;
+
+				break;
+			case 3:
+				edgeVert0 = 2;
+				edgeVert1 = 3;
+				break;
+			case 4:
+				edgeVert0 = 0;
+				edgeVert1 = 4;
+				break;
+			case 5:
+				edgeVert0 = 1;
+				edgeVert1 = 5;
+
+				break;
+			case 6:
+				edgeVert0 = 2;
+				edgeVert1 = 6;
+				break;
+			case 7:
+				edgeVert0 = 3;
+				edgeVert1 = 7;
+				break;
+			case 8:
+				edgeVert0 = 4;
+				edgeVert1 = 5;
+				break;
+			case 9:
+				edgeVert0 = 4;
+				edgeVert1 = 6;
+				break;
+			case 10:
+				edgeVert0 = 5;
+				edgeVert1 = 7;
+				break;
+			case 11:
+				edgeVert0 = 6;
+				edgeVert1 = 7;
+				break;
+			default:
+				assert (false);
+		}
+
+		getVertex(edgeVert0, pa);
+		getVertex(edgeVert1, pb);
+	}
+
+	@Override
+	public boolean isInside(Vector3f pt, float tolerance) {
+		// JAVA NOTE: against stack usage, but safe with code below
+		Vector3f halfExtents = getHalfExtentsWithoutMargin();
+
+		//btScalar minDist = 2*tolerance;
+
+		boolean result =
+				(pt.x <= (halfExtents.x + tolerance)) &&
+				(pt.x >= (-halfExtents.x - tolerance)) &&
+				(pt.y <= (halfExtents.y + tolerance)) &&
+				(pt.y >= (-halfExtents.y - tolerance)) &&
+				(pt.z <= (halfExtents.z + tolerance)) &&
+				(pt.z >= (-halfExtents.z - tolerance));
+
+		return result;
+	}
+
+	@Override
+	public String getName() {
+		return "Box";
+	}
+
+	@Override
+	public int getNumPreferredPenetrationDirections() {
+		return 6;
+	}
+
+	@Override
+	public void getPreferredPenetrationDirection(int index, Vector3f penetrationVector) {
+		switch (index) {
+			case 0:
+				penetrationVector.set(1f, 0f, 0f);
+				break;
+			case 1:
+				penetrationVector.set(-1f, 0f, 0f);
+				break;
+			case 2:
+				penetrationVector.set(0f, 1f, 0f);
+				break;
+			case 3:
+				penetrationVector.set(0f, -1f, 0f);
+				break;
+			case 4:
+				penetrationVector.set(0f, 0f, 1f);
+				break;
+			case 5:
+				penetrationVector.set(0f, 0f, -1f);
+				break;
+			default:
+				assert (false);
+		}
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/BvhSubtreeInfo.java b/src/jbullet/src/javabullet/collision/shapes/BvhSubtreeInfo.java
new file mode 100644
index 0000000..fb82dbc
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/BvhSubtreeInfo.java
@@ -0,0 +1,48 @@
+/*
+ * 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.collision.shapes;
+
+/**
+ * BvhSubtreeInfo provides info to gather a subtree of limited size.
+ * 
+ * @author jezek2
+ */
+public class BvhSubtreeInfo {
+	
+	public final /*unsigned*/ short[] quantizedAabbMin = new short[3];
+	public final /*unsigned*/ short[] quantizedAabbMax = new short[3];
+	// points to the root of the subtree
+	public int rootNodeIndex;
+	public int subtreeSize;
+
+	public void setAabbFromQuantizeNode(QuantizedBvhNodes quantizedNodes, int nodeId) {
+		quantizedAabbMin[0] = (short)quantizedNodes.getQuantizedAabbMin(nodeId, 0);
+		quantizedAabbMin[1] = (short)quantizedNodes.getQuantizedAabbMin(nodeId, 1);
+		quantizedAabbMin[2] = (short)quantizedNodes.getQuantizedAabbMin(nodeId, 2);
+		quantizedAabbMax[0] = (short)quantizedNodes.getQuantizedAabbMax(nodeId, 0);
+		quantizedAabbMax[1] = (short)quantizedNodes.getQuantizedAabbMax(nodeId, 1);
+		quantizedAabbMax[2] = (short)quantizedNodes.getQuantizedAabbMax(nodeId, 2);
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/BvhTriangleMeshShape.java b/src/jbullet/src/javabullet/collision/shapes/BvhTriangleMeshShape.java
new file mode 100644
index 0000000..42878bc
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/BvhTriangleMeshShape.java
@@ -0,0 +1,276 @@
+/*
+ * 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.collision.shapes;
+
+import java.nio.ByteBuffer;
+import javabullet.BulletGlobals;
+import javabullet.BulletPool;
+import javabullet.ObjectPool;
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.collision.narrowphase.TriangleConvexcastCallback;
+import javabullet.collision.narrowphase.TriangleRaycastCallback;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ * Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization.
+ * Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
+ * 
+ * @author jezek2
+ */
+public class BvhTriangleMeshShape extends TriangleMeshShape {
+
+	private OptimizedBvh bvh;
+	private boolean useQuantizedAabbCompression;
+	private boolean ownsBvh;
+	
+	private ObjectPool<MyNodeOverlapCallback> myNodeCallbacks = BulletPool.get(MyNodeOverlapCallback.class);
+	
+	public BvhTriangleMeshShape() {
+		super(null);
+		this.bvh = null;
+		this.ownsBvh = false;
+	}
+
+	public BvhTriangleMeshShape(StridingMeshInterface meshInterface, boolean useQuantizedAabbCompression) {
+		this(meshInterface, useQuantizedAabbCompression, true);
+	}
+	
+	public BvhTriangleMeshShape(StridingMeshInterface meshInterface, boolean useQuantizedAabbCompression, boolean buildBvh) {
+		super(meshInterface);
+		this.bvh = null;
+		this.useQuantizedAabbCompression = useQuantizedAabbCompression;
+		this.ownsBvh = false;
+
+		// construct bvh from meshInterface
+		//#ifndef DISABLE_BVH
+
+		Vector3f bvhAabbMin = new Vector3f(), bvhAabbMax = new Vector3f();
+		meshInterface.calculateAabbBruteForce(bvhAabbMin, bvhAabbMax);
+
+		if (buildBvh) {
+			bvh = new OptimizedBvh();
+			bvh.build(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax);
+			ownsBvh = true;
+		}
+
+		// JAVA NOTE: moved from TriangleMeshShape
+		recalcLocalAabb();
+		//#endif //DISABLE_BVH
+	}
+
+	/**
+	 * Optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb.
+	 */
+	public BvhTriangleMeshShape(StridingMeshInterface meshInterface, boolean useQuantizedAabbCompression, Vector3f bvhAabbMin, Vector3f bvhAabbMax) {
+		this(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, true);
+	}
+	
+	/**
+	 * Optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb.
+	 */
+	public BvhTriangleMeshShape(StridingMeshInterface meshInterface, boolean useQuantizedAabbCompression, Vector3f bvhAabbMin, Vector3f bvhAabbMax, boolean buildBvh) {
+		super(meshInterface);
+
+		this.bvh = null;
+		this.useQuantizedAabbCompression = useQuantizedAabbCompression;
+		this.ownsBvh = false;
+
+		// construct bvh from meshInterface
+		//#ifndef DISABLE_BVH
+
+		if (buildBvh) {
+			bvh = new OptimizedBvh();
+
+			bvh.build(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax);
+			ownsBvh = true;
+		}
+
+		// JAVA NOTE: moved from TriangleMeshShape
+		recalcLocalAabb();
+		//#endif //DISABLE_BVH
+	}
+	
+	@Override
+	public BroadphaseNativeType getShapeType() {
+		return BroadphaseNativeType.TRIANGLE_MESH_SHAPE_PROXYTYPE;
+	}
+
+	public void performRaycast(TriangleRaycastCallback callback, Vector3f raySource, Vector3f rayTarget) {
+		MyNodeOverlapCallback myNodeCallback = myNodeCallbacks.get();
+		myNodeCallback.init(callback, meshInterface);
+
+		bvh.reportRayOverlappingNodex(myNodeCallback, raySource, rayTarget);
+		
+		myNodeCallbacks.release(myNodeCallback);
+	}
+	
+	public void performConvexcast(TriangleConvexcastCallback callback, Vector3f raySource, Vector3f rayTarget, Vector3f aabbMin, Vector3f aabbMax) {
+		MyNodeOverlapCallback myNodeCallback = myNodeCallbacks.get();
+		myNodeCallback.init(callback, meshInterface);
+
+		bvh.reportBoxCastOverlappingNodex(myNodeCallback, raySource, rayTarget, aabbMin, aabbMax);
+
+		myNodeCallbacks.release(myNodeCallback);
+	}
+
+	/**
+	 * Perform bvh tree traversal and report overlapping triangles to 'callback'.
+	 */
+	@Override
+	public void processAllTriangles(TriangleCallback callback, Vector3f aabbMin, Vector3f aabbMax) {
+		//#ifdef DISABLE_BVH
+		// // brute force traverse all triangles
+		//btTriangleMeshShape::processAllTriangles(callback,aabbMin,aabbMax);
+		//#else
+
+		// first get all the nodes
+		MyNodeOverlapCallback myNodeCallback = myNodeCallbacks.get();
+		myNodeCallback.init(callback, meshInterface);
+
+		bvh.reportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax);
+
+		myNodeCallbacks.release(myNodeCallback);
+		//#endif//DISABLE_BVH
+	}
+	
+	public void refitTree() {
+		bvh.refit(meshInterface);
+
+		recalcLocalAabb();
+	}
+
+	/**
+	 * For a fast incremental refit of parts of the tree. Note: the entire AABB of the tree will become more conservative, it never shrinks.
+	 */
+	public void partialRefitTree(Vector3f aabbMin, Vector3f aabbMax) {
+		bvh.refitPartial(meshInterface,aabbMin,aabbMax );
+
+		VectorUtil.setMin(localAabbMin, aabbMin);
+		VectorUtil.setMax(localAabbMax, aabbMax);
+	}
+
+	@Override
+	public String getName() {
+		return "BVHTRIANGLEMESH";
+	}
+	
+	@Override
+	public void setLocalScaling(Vector3f scaling) {
+		stack.vectors.push();
+		try {
+			Vector3f tmp = stack.vectors.get();
+			tmp.sub(getLocalScaling(), scaling);
+
+			if (tmp.lengthSquared() > BulletGlobals.SIMD_EPSILON) {
+				super.setLocalScaling(scaling);
+				/*
+				if (ownsBvh)
+				{
+				m_bvh->~btOptimizedBvh();
+				btAlignedFree(m_bvh);
+				}
+				*/
+				///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work
+				bvh = new OptimizedBvh();
+				// rebuild the bvh...
+				bvh.build(meshInterface, useQuantizedAabbCompression, localAabbMin, localAabbMax);
+
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public OptimizedBvh getOptimizedBvh() {
+		return bvh;
+	}
+
+	public void setOptimizedBvh(OptimizedBvh bvh) {
+		assert (this.bvh == null);
+		assert (!ownsBvh);
+
+		this.bvh = bvh;
+		ownsBvh = false;
+	}
+
+	public boolean usesQuantizedAabbCompression() {
+		return useQuantizedAabbCompression;
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	protected static class MyNodeOverlapCallback implements NodeOverlapCallback {
+		public StridingMeshInterface meshInterface;
+		public TriangleCallback callback;
+
+		private Vector3f[] triangle/*[3]*/ = new Vector3f[] { new Vector3f(), new Vector3f(), new Vector3f() };
+		private VertexData data = new VertexData();
+
+		public MyNodeOverlapCallback() {
+		}
+		
+		public void init(TriangleCallback callback, StridingMeshInterface meshInterface) {
+			this.meshInterface = meshInterface;
+			this.callback = callback;
+		}
+
+		public void processNode(int nodeSubPart, int nodeTriangleIndex) {
+			meshInterface.getLockedReadOnlyVertexIndexBase(data, nodeSubPart);
+
+			//int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride);
+			ByteBuffer gfxbase_ptr = data.indexbase;
+			int gfxbase_index = (nodeTriangleIndex * data.indexstride);
+			assert (data.indicestype == ScalarType.PHY_INTEGER || data.indicestype == ScalarType.PHY_SHORT);
+
+			Vector3f meshScaling = meshInterface.getScaling();
+			for (int j = 2; j >= 0; j--) {
+				int graphicsindex;
+				if (data.indicestype == ScalarType.PHY_SHORT) {
+					graphicsindex = gfxbase_ptr.getShort(gfxbase_index + j * 2) & 0xFFFF;
+				}
+				else {
+					graphicsindex = gfxbase_ptr.getInt(gfxbase_index + j * 4);
+				}
+
+				//float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
+				ByteBuffer graphicsbase_ptr = data.vertexbase;
+				int graphicsbase_index = graphicsindex * data.stride;
+
+				triangle[j].set(
+						graphicsbase_ptr.getFloat(graphicsbase_index + 4 * 0) * meshScaling.x,
+						graphicsbase_ptr.getFloat(graphicsbase_index + 4 * 1) * meshScaling.y,
+						graphicsbase_ptr.getFloat(graphicsbase_index + 4 * 2) * meshScaling.z);
+			}
+
+			/* Perform ray vs. triangle collision here */
+			callback.processTriangle(triangle, nodeSubPart, nodeTriangleIndex);
+			meshInterface.unLockReadOnlyVertexBase(nodeSubPart);
+
+			data.unref();
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/CapsuleShape.java b/src/jbullet/src/javabullet/collision/shapes/CapsuleShape.java
new file mode 100644
index 0000000..674ac5d
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/CapsuleShape.java
@@ -0,0 +1,160 @@
+/*
+ * 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.collision.shapes;
+
+import javabullet.BulletGlobals;
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ * CapsuleShape represents a capsule around the Y axis.
+ * A more general solution that can represent capsules is the MultiSphereShape.
+ * 
+ * @author jezek2
+ */
+public class CapsuleShape extends ConvexInternalShape {
+	
+	public CapsuleShape(float radius, float height) {
+		implicitShapeDimensions.set(radius, 0.5f * height, radius);
+	}
+
+	@Override
+	public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec0) {
+		stack.vectors.push();
+		try {
+			Vector3f supVec = stack.vectors.get(0f, 0f, 0f);
+
+			float maxDot = -1e30f;
+
+			Vector3f vec = stack.vectors.get(vec0);
+			float lenSqr = vec.lengthSquared();
+			if (lenSqr < 0.0001f) {
+				vec.set(1f, 0f, 0f);
+			}
+			else {
+				float rlen = 1f / (float) Math.sqrt(lenSqr);
+				vec.scale(rlen);
+			}
+
+			Vector3f vtx = stack.vectors.get();
+			float newDot;
+
+			float radius = getRadius();
+
+			Vector3f tmp1 = stack.vectors.get();
+			Vector3f tmp2 = stack.vectors.get();
+
+			{
+				Vector3f pos = stack.vectors.get(0f, getHalfHeight(), 0f);
+				VectorUtil.mul(tmp1, vec, localScaling);
+				tmp1.scale(radius);
+				tmp2.scale(getMargin(), vec);
+				vtx.add(pos, tmp1);
+				vtx.sub(tmp2);
+				newDot = vec.dot(vtx);
+				if (newDot > maxDot) {
+					maxDot = newDot;
+					supVec.set(vtx);
+				}
+			}
+			{
+				Vector3f pos = stack.vectors.get(0f, -getHalfHeight(), 0f);
+				VectorUtil.mul(tmp1, vec, localScaling);
+				tmp1.scale(radius);
+				tmp2.scale(getMargin(), vec);
+				vtx.add(pos, tmp1);
+				vtx.sub(tmp2);
+				newDot = vec.dot(vtx);
+				if (newDot > maxDot) {
+					maxDot = newDot;
+					supVec.set(vtx);
+				}
+			}
+
+			return stack.vectors.returning(supVec);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
+		// TODO: implement
+		throw new UnsupportedOperationException("Not supported yet.");
+	}
+
+	@Override
+	public void calculateLocalInertia(float mass, Vector3f inertia) {
+		stack.pushCommonMath();
+		try {
+			// as an approximation, take the inertia of the box that bounds the spheres
+
+			Transform ident = stack.transforms.get();
+			ident.setIdentity();
+
+			float radius = getRadius();
+
+			Vector3f halfExtents = stack.vectors.get(radius, radius + getHalfHeight(), radius);
+
+			float margin = BulletGlobals.CONVEX_DISTANCE_MARGIN;
+
+			float lx = 2f * (halfExtents.x + margin);
+			float ly = 2f * (halfExtents.y + margin);
+			float lz = 2f * (halfExtents.z + margin);
+			float x2 = lx * lx;
+			float y2 = ly * ly;
+			float z2 = lz * lz;
+			float scaledmass = mass * 0.08333333f;
+
+			inertia.x = scaledmass * (y2 + z2);
+			inertia.y = scaledmass * (x2 + z2);
+			inertia.z = scaledmass * (x2 + y2);
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	@Override
+	public BroadphaseNativeType getShapeType() {
+		return BroadphaseNativeType.CAPSULE_SHAPE_PROXYTYPE;
+	}
+
+	@Override
+	public String getName() {
+		return "CapsuleShape";
+	}
+	
+	public float getRadius() {
+		return implicitShapeDimensions.x;
+	}
+
+	public float getHalfHeight() {
+		return implicitShapeDimensions.y;
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/CollisionShape.java b/src/jbullet/src/javabullet/collision/shapes/CollisionShape.java
new file mode 100644
index 0000000..87145f4
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/CollisionShape.java
@@ -0,0 +1,171 @@
+/*
+ * 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.collision.shapes;
+
+import javabullet.BulletStack;
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * CollisionShape provides interface for collision shapes that can be shared among CollisionObjects.
+ * 
+ * @author jezek2
+ */
+public abstract class CollisionShape {
+
+	protected final BulletStack stack = BulletStack.get();
+	
+	///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t.
+	public abstract void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax);
+
+	public void getBoundingSphere(Vector3f center, float[] radius) {
+		stack.pushCommonMath();
+		try {
+			Vector3f tmp = stack.vectors.get();
+
+			Transform tr = stack.transforms.get();
+			tr.setIdentity();
+			Vector3f aabbMin = stack.vectors.get(), aabbMax = stack.vectors.get();
+
+			getAabb(tr, aabbMin, aabbMax);
+
+			tmp.sub(aabbMax, aabbMin);
+			radius[0] = tmp.length() * 0.5f;
+
+			tmp.add(aabbMin, aabbMax);
+			center.scale(0.5f, tmp);
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations.
+	public float getAngularMotionDisc() {
+		stack.vectors.push();
+		try {
+			Vector3f center = stack.vectors.get();
+			float[] disc = new float[1]; // TODO: stack
+			getBoundingSphere(center, disc);
+			disc[0] += center.length();
+			return disc[0];
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
+	///result is conservative
+	public void calculateTemporalAabb(Transform curTrans, Vector3f linvel, Vector3f angvel, float timeStep, Vector3f temporalAabbMin, Vector3f temporalAabbMax) {
+		stack.vectors.push();
+		try {
+			//start with static aabb
+			getAabb(curTrans, temporalAabbMin, temporalAabbMax);
+
+			float temporalAabbMaxx = temporalAabbMax.x;
+			float temporalAabbMaxy = temporalAabbMax.y;
+			float temporalAabbMaxz = temporalAabbMax.z;
+			float temporalAabbMinx = temporalAabbMin.x;
+			float temporalAabbMiny = temporalAabbMin.y;
+			float temporalAabbMinz = temporalAabbMin.z;
+
+			// add linear motion
+			Vector3f linMotion = stack.vectors.get(linvel);
+			linMotion.scale(timeStep);
+
+			//todo: simd would have a vector max/min operation, instead of per-element access
+			if (linMotion.x > 0f) {
+				temporalAabbMaxx += linMotion.x;
+			}
+			else {
+				temporalAabbMinx += linMotion.x;
+			}
+			if (linMotion.y > 0f) {
+				temporalAabbMaxy += linMotion.y;
+			}
+			else {
+				temporalAabbMiny += linMotion.y;
+			}
+			if (linMotion.z > 0f) {
+				temporalAabbMaxz += linMotion.z;
+			}
+			else {
+				temporalAabbMinz += linMotion.z;
+			}
+
+			//add conservative angular motion
+			float angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
+			Vector3f angularMotion3d = stack.vectors.get(angularMotion, angularMotion, angularMotion);
+			temporalAabbMin.set(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz);
+			temporalAabbMax.set(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz);
+
+			temporalAabbMin.sub(angularMotion3d);
+			temporalAabbMax.add(angularMotion3d);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+//#ifndef __SPU__
+	public boolean isPolyhedral() {
+		return getShapeType().isPolyhedral();
+	}
+
+	public boolean isConvex() {
+		return getShapeType().isConvex();
+	}
+
+	public boolean isConcave() {
+		return getShapeType().isConcave();
+	}
+
+	public boolean isCompound() {
+		return getShapeType().isCompound();
+	}
+
+	///isInfinite is used to catch simulation error (aabb check)
+	public boolean isInfinite() {
+		return getShapeType().isInfinite();
+	}
+
+	public abstract BroadphaseNativeType getShapeType();
+
+	public abstract void setLocalScaling(Vector3f scaling);
+	
+	// TODO: returns const
+	public abstract Vector3f getLocalScaling();
+
+	public abstract void calculateLocalInertia(float mass, Vector3f inertia);
+
+
+//debugging support
+	public abstract String getName();
+//#endif //__SPU__
+	public abstract void setMargin(float margin);
+
+	public abstract float getMargin();
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/CompoundShape.java b/src/jbullet/src/javabullet/collision/shapes/CompoundShape.java
new file mode 100644
index 0000000..8531fdf
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/CompoundShape.java
@@ -0,0 +1,212 @@
+/*
+ * 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.collision.shapes;
+
+import java.util.ArrayList;
+import java.util.List;
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.linearmath.MatrixUtil;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Vector3f;
+
+/**
+ * CompoundShape allows to store multiple other CollisionShapes.
+ * This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape.
+ * 
+ * @author jezek2
+ */
+public class CompoundShape extends CollisionShape {
+
+	private final List<CompoundShapeChild> children = new ArrayList<CompoundShapeChild>();
+	private final Vector3f localAabbMin = new Vector3f(1e30f, 1e30f, 1e30f);
+	private final Vector3f localAabbMax = new Vector3f(-1e30f, -1e30f, -1e30f);
+
+	private OptimizedBvh aabbTree = null;
+
+	private float collisionMargin = 0f;
+	protected final Vector3f localScaling = new Vector3f(1f, 1f, 1f);
+
+	public void addChildShape(Transform localTransform, CollisionShape shape) {
+		stack.vectors.push();
+		try {
+			//m_childTransforms.push_back(localTransform);
+			//m_childShapes.push_back(shape);
+			CompoundShapeChild child = new CompoundShapeChild();
+			child.transform.set(localTransform);
+			child.childShape = shape;
+			child.childShapeType = shape.getShapeType();
+			child.childMargin = shape.getMargin();
+
+			children.add(child);
+
+			// extend the local aabbMin/aabbMax
+			Vector3f _localAabbMin = stack.vectors.get(), _localAabbMax = stack.vectors.get();
+			shape.getAabb(localTransform, _localAabbMin, _localAabbMax);
+
+			// JAVA NOTE: rewritten
+	//		for (int i=0;i<3;i++)
+	//		{
+	//			if (this.localAabbMin[i] > _localAabbMin[i])
+	//			{
+	//				this.localAabbMin[i] = _localAabbMin[i];
+	//			}
+	//			if (this.localAabbMax[i] < _localAabbMax[i])
+	//			{
+	//				this.localAabbMax[i] = _localAabbMax[i];
+	//			}
+	//		}
+			VectorUtil.setMin(this.localAabbMin, _localAabbMin);
+			VectorUtil.setMax(this.localAabbMax, _localAabbMax);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public int getNumChildShapes() {
+		return children.size();
+	}
+
+	public CollisionShape getChildShape(int index) {
+		return children.get(index).childShape;
+	}
+
+	public Transform getChildTransform(int index) {
+		return children.get(index).transform;
+	}
+
+	public List<CompoundShapeChild> getChildList() {
+		return children;
+	}
+
+	/**
+	 * getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version.
+	 */
+	@Override
+	public void getAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax) {
+		stack.pushCommonMath();
+		try {
+			Vector3f localHalfExtents = stack.vectors.get();
+			localHalfExtents.sub(localAabbMax, localAabbMin);
+			localHalfExtents.scale(0.5f);
+
+			Vector3f localCenter = stack.vectors.get();
+			localCenter.add(localAabbMax, localAabbMin);
+			localCenter.scale(0.5f);
+
+			Matrix3f abs_b = stack.matrices.get(trans.basis);
+			MatrixUtil.absolute(abs_b);
+
+			Vector3f center = stack.vectors.get(localCenter);
+			trans.transform(center);
+
+			Vector3f tmp = stack.vectors.get();
+
+			Vector3f extent = stack.vectors.get();
+			abs_b.getRow(0, tmp);
+			extent.x = tmp.dot(localHalfExtents);
+			abs_b.getRow(1, tmp);
+			extent.y = tmp.dot(localHalfExtents);
+			abs_b.getRow(2, tmp);
+			extent.z = tmp.dot(localHalfExtents);
+
+			extent.x += getMargin();
+			extent.y += getMargin();
+			extent.z += getMargin();
+
+			aabbMin.sub(center, extent);
+			aabbMax.add(center, extent);
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	@Override
+	public void setLocalScaling(Vector3f scaling) {
+		localScaling.set(scaling);
+	}
+
+	@Override
+	public Vector3f getLocalScaling() {
+		return localScaling;
+	}
+
+	@Override
+	public void calculateLocalInertia(float mass, Vector3f inertia) {
+		stack.pushCommonMath();
+		try {
+			// approximation: take the inertia from the aabb for now
+			Transform ident = stack.transforms.get();
+			ident.setIdentity();
+			Vector3f aabbMin = stack.vectors.get(), aabbMax = stack.vectors.get();
+			getAabb(ident, aabbMin, aabbMax);
+
+			Vector3f halfExtents = stack.vectors.get();
+			halfExtents.sub(aabbMax, aabbMin);
+			halfExtents.scale(0.5f);
+
+			float lx = 2f * halfExtents.x;
+			float ly = 2f * halfExtents.y;
+			float lz = 2f * halfExtents.z;
+
+			inertia.x = (mass / 12f) * (ly * ly + lz * lz);
+			inertia.y = (mass / 12f) * (lx * lx + lz * lz);
+			inertia.z = (mass / 12f) * (lx * lx + ly * ly);
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+	
+	@Override
+	public BroadphaseNativeType getShapeType() {
+		return BroadphaseNativeType.COMPOUND_SHAPE_PROXYTYPE;
+	}
+
+	@Override
+	public void setMargin(float margin) {
+		collisionMargin = margin;
+	}
+
+	@Override
+	public float getMargin() {
+		return collisionMargin;
+	}
+
+	@Override
+	public String getName() {
+		return "Compound";
+	}
+
+	// this is optional, but should make collision queries faster, by culling non-overlapping nodes
+	// void	createAabbTreeFromChildren();
+	
+	public OptimizedBvh getAabbTree() {
+		return aabbTree;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/CompoundShapeChild.java b/src/jbullet/src/javabullet/collision/shapes/CompoundShapeChild.java
new file mode 100644
index 0000000..0f320b0
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/CompoundShapeChild.java
@@ -0,0 +1,40 @@
+/*
+ * 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.collision.shapes;
+
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.linearmath.Transform;
+
+/**
+ *
+ * @author jezek2
+ */
+public class CompoundShapeChild {
+	
+	public final Transform transform = new Transform();
+	public CollisionShape childShape;
+	public BroadphaseNativeType childShapeType;
+	public float childMargin;
+
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/ConcaveShape.java b/src/jbullet/src/javabullet/collision/shapes/ConcaveShape.java
new file mode 100644
index 0000000..846b78d
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/ConcaveShape.java
@@ -0,0 +1,48 @@
+/*
+ * 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.collision.shapes;
+
+import javax.vecmath.Vector3f;
+
+/**
+ * Concave shape proves an interface concave shapes that can produce triangles that overlapping a given AABB.
+ * Static triangle mesh, infinite plane, height field/landscapes are example that implement this interface.
+ * 
+ * @author jezek2
+ */
+public abstract class ConcaveShape extends CollisionShape {
+
+	protected float collisionMargin = 0f;
+
+	public abstract void processAllTriangles(TriangleCallback callback, Vector3f aabbMin, Vector3f aabbMax);
+
+	public float getMargin() {
+		return collisionMargin;
+	}
+
+	public void setMargin(float margin) {
+		this.collisionMargin = margin;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/ConvexHullShape.java b/src/jbullet/src/javabullet/collision/shapes/ConvexHullShape.java
new file mode 100644
index 0000000..8636802
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/ConvexHullShape.java
@@ -0,0 +1,223 @@
+/*
+ * 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.collision.shapes;
+
+import java.util.ArrayList;
+import java.util.List;
+import javabullet.BulletGlobals;
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ * ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices).
+ * No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices.
+ * On modern hardware, due to cache coherency this isn't that bad. Complex algorithms tend to trash the cashe
+ * (memory is much slower then the cpu).
+ * 
+ * @author jezek2
+ */
+public class ConvexHullShape extends PolyhedralConvexShape {
+
+	private final List<Vector3f> points = new ArrayList<Vector3f>();
+	
+	/**
+	 * TODO: This constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive float (x,y,z), the striding defines the number of bytes between each point, in memory.
+	 * It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint.
+	 * ConvexHullShape make an internal copy of the points.
+	 */
+	// TODO: make better constuctors (ByteBuffer, etc.)
+	public ConvexHullShape(List<Vector3f> points) {
+		// JAVA NOTE: rewritten
+		
+		for (int i=0; i<points.size(); i++) {
+			this.points.add(new Vector3f(points.get(i)));
+		}
+		
+		recalcLocalAabb();
+	}
+	
+	public void addPoint(Vector3f point) {
+		points.add(new Vector3f(point));
+		recalcLocalAabb();
+	}
+
+	public List<Vector3f> getPoints() {
+		return points;
+	}
+
+	public int getNumPoints() {
+		return points.size();
+	}
+
+	@Override
+	public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec0) {
+		stack.vectors.push();
+		try {
+			Vector3f supVec = stack.vectors.get(0f, 0f, 0f);
+			float newDot, maxDot = -1e30f;
+
+			Vector3f vec = stack.vectors.get(vec0);
+			float lenSqr = vec.lengthSquared();
+			if (lenSqr < 0.0001f) {
+				vec.set(1f, 0f, 0f);
+			}
+			else {
+				float rlen = 1f / (float) Math.sqrt(lenSqr);
+				vec.scale(rlen);
+			}
+
+
+			Vector3f vtx = stack.vectors.get();
+			for (int i = 0; i < points.size(); i++) {
+				VectorUtil.mul(vtx, points.get(i), localScaling);
+
+				newDot = vec.dot(vtx);
+				if (newDot > maxDot) {
+					maxDot = newDot;
+					supVec.set(vtx);
+				}
+			}
+			return stack.vectors.returning(supVec);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
+		stack.vectors.push();
+		try {
+			float newDot;
+
+			// JAVA NOTE: rewritten as code used W coord for temporary usage in Vector3
+			// TODO: optimize it
+			float[] wcoords = new float[numVectors];
+
+			// use 'w' component of supportVerticesOut?
+			{
+				for (int i = 0; i < numVectors; i++) {
+					//supportVerticesOut[i][3] = btScalar(-1e30);
+					wcoords[i] = -1e30f;
+				}
+			}
+			Vector3f vtx = stack.vectors.get();
+			for (int i = 0; i < points.size(); i++) {
+				VectorUtil.mul(vtx, points.get(i), localScaling);
+
+				for (int j = 0; j < numVectors; j++) {
+					Vector3f vec = vectors[j];
+
+					newDot = vec.dot(vtx);
+					//if (newDot > supportVerticesOut[j][3])
+					if (newDot > wcoords[j]) {
+						// WARNING: don't swap next lines, the w component would get overwritten!
+						supportVerticesOut[j].set(vtx);
+						//supportVerticesOut[j][3] = newDot;
+						wcoords[j] = newDot;
+					}
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public Vector3f localGetSupportingVertex(Vector3f vec) {
+		stack.vectors.push();
+		try {
+			Vector3f supVertex = stack.vectors.get(localGetSupportingVertexWithoutMargin(vec));
+
+			if (getMargin() != 0f) {
+				Vector3f vecnorm = stack.vectors.get(vec);
+				if (vecnorm.lengthSquared() < (BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
+					vecnorm.set(-1f, -1f, -1f);
+				}
+				vecnorm.normalize();
+				supVertex.scaleAdd(getMargin(), vecnorm, supVertex);
+			}
+			return stack.vectors.returning(supVertex);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	/**
+	 * Currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection.
+	 * Please note that you can debug-draw ConvexHullShape with the Raytracer Demo.
+	 */
+	@Override
+	public int getNumVertices() {
+		return points.size();
+	}
+
+	@Override
+	public int getNumEdges() {
+		return points.size();
+	}
+
+	@Override
+	public void getEdge(int i, Vector3f pa, Vector3f pb) {
+		int index0 = i % points.size();
+		int index1 = (i + 1) % points.size();
+		VectorUtil.mul(pa, points.get(index0), localScaling);
+		VectorUtil.mul(pb, points.get(index1), localScaling);
+	}
+
+	@Override
+	public void getVertex(int i, Vector3f vtx) {
+		VectorUtil.mul(vtx, points.get(i), localScaling);
+	}
+
+	@Override
+	public int getNumPlanes() {
+		return 0;
+	}
+
+	@Override
+	public void getPlane(Vector3f planeNormal, Vector3f planeSupport, int i) {
+		assert false;
+	}
+
+	@Override
+	public boolean isInside(Vector3f pt, float tolerance) {
+		assert false;
+		return false;
+	}
+
+	@Override
+	public BroadphaseNativeType getShapeType() {
+		return BroadphaseNativeType.CONVEX_HULL_SHAPE_PROXYTYPE;
+	}
+
+	@Override
+	public String getName() {
+		return "Convex";
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/ConvexInternalShape.java b/src/jbullet/src/javabullet/collision/shapes/ConvexInternalShape.java
new file mode 100644
index 0000000..0f7836c
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/ConvexInternalShape.java
@@ -0,0 +1,131 @@
+/*
+ * 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.collision.shapes;
+
+import javabullet.BulletGlobals;
+import javabullet.linearmath.MatrixUtil;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public abstract class ConvexInternalShape extends ConvexShape {
+
+	//local scaling. collisionMargin is not scaled !
+	protected final Vector3f localScaling = new Vector3f(1f, 1f, 1f);
+	protected final Vector3f implicitShapeDimensions = new Vector3f();
+	protected float collisionMargin = BulletGlobals.CONVEX_DISTANCE_MARGIN;
+
+	/**
+	 * getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version.
+	 */
+	@Override
+	public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) {
+		getAabbSlow(t, aabbMin, aabbMax);
+	}
+	
+	@Override
+	public void getAabbSlow(Transform trans, Vector3f minAabb, Vector3f maxAabb) {
+		stack.vectors.push();
+		try {
+			float margin = getMargin();
+			for (int i=0;i<3;i++)
+			{
+				Vector3f vec = stack.vectors.get(0f, 0f, 0f);
+				VectorUtil.setCoord(vec, i, 1f);
+
+				Vector3f tmp1 = stack.vectors.get();
+				MatrixUtil.transposeTransform(tmp1, vec, trans.basis);
+				Vector3f sv = stack.vectors.get(localGetSupportingVertex(tmp1));
+
+				Vector3f tmp2 = stack.vectors.get(sv);
+				trans.transform(tmp2);
+
+				VectorUtil.setCoord(maxAabb, i, VectorUtil.getCoord(tmp2, i) + margin);
+
+				VectorUtil.setCoord(vec, i, -1f);
+				
+				MatrixUtil.transposeTransform(tmp1, vec, trans.basis);
+				tmp2.set(localGetSupportingVertex(tmp1));
+				trans.transform(tmp2);
+
+				VectorUtil.setCoord(minAabb, i, VectorUtil.getCoord(tmp2, i) - margin);
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public Vector3f localGetSupportingVertex(Vector3f vec) {
+		stack.vectors.push();
+		try {
+			Vector3f supVertex = stack.vectors.get(localGetSupportingVertexWithoutMargin(vec));
+
+			if (getMargin() != 0f) {
+				Vector3f vecnorm = stack.vectors.get(vec);
+				if (vecnorm.lengthSquared() < (BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) {
+					vecnorm.set(-1f, -1f, -1f);
+				}
+				vecnorm.normalize();
+				supVertex.scaleAdd(getMargin(), vecnorm, supVertex);
+			}
+			return stack.vectors.returning(supVertex);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public void setLocalScaling(Vector3f scaling) {
+		this.localScaling.set(scaling);
+	}
+	
+	public Vector3f getLocalScaling() {
+		return localScaling;
+	}
+
+	public float getMargin() {
+		return collisionMargin;
+	}
+
+	public void setMargin(float margin) {
+		this.collisionMargin = margin;
+	}
+
+	@Override
+	public int getNumPreferredPenetrationDirections() {
+		return 0;
+	}
+
+	@Override
+	public void getPreferredPenetrationDirection(int index, Vector3f penetrationVector) {
+		throw new InternalError();
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/ConvexShape.java b/src/jbullet/src/javabullet/collision/shapes/ConvexShape.java
new file mode 100644
index 0000000..f1352ea
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/ConvexShape.java
@@ -0,0 +1,61 @@
+/*
+ * 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.collision.shapes;
+
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * ConvexShape is an abstract shape interface.
+ * It describes general convex shapes using the localGetSupportingVertex interface
+ * used in combination with GJK or btConvexCast
+ * 
+ * @author jezek2
+ */
+public abstract class ConvexShape extends CollisionShape {
+
+	public abstract Vector3f localGetSupportingVertex(Vector3f vec);
+
+	//#ifndef __SPU__
+	public abstract Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec);
+
+	//notice that the vectors should be unit length
+	public abstract void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors);
+	//#endif
+	
+	public abstract void getAabbSlow(Transform t, Vector3f aabbMin, Vector3f aabbMax);
+
+	public abstract void setLocalScaling(Vector3f scaling);
+
+	public abstract Vector3f getLocalScaling();
+
+	public abstract void setMargin(float margin);
+
+	public abstract float getMargin();
+
+	public abstract int getNumPreferredPenetrationDirections();
+
+	public abstract void getPreferredPenetrationDirection(int index, Vector3f penetrationVector);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/CylinderShape.java b/src/jbullet/src/javabullet/collision/shapes/CylinderShape.java
new file mode 100644
index 0000000..f74dcdd
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/CylinderShape.java
@@ -0,0 +1,152 @@
+/*
+ * 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.collision.shapes;
+
+import javabullet.BulletGlobals;
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ * Implements cylinder shape interface.
+ * 
+ * @author jezek2
+ */
+public class CylinderShape extends BoxShape {
+
+	protected int upAxis;
+
+	public CylinderShape(Vector3f halfExtents) {
+		super(halfExtents);
+		upAxis = 1;
+		recalcLocalAabb();
+	}
+
+	protected CylinderShape(Vector3f halfExtents, boolean unused) {
+		super(halfExtents);
+	}
+
+	@Override
+	public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) {
+		_PolyhedralConvexShape_getAabb(t, aabbMin, aabbMax);
+	}
+
+	protected Vector3f cylinderLocalSupportX(Vector3f halfExtents, Vector3f v) {
+		return cylinderLocalSupport(halfExtents, v, 0, 1, 0, 2);
+	}
+
+	protected Vector3f cylinderLocalSupportY(Vector3f halfExtents, Vector3f v) {
+		return cylinderLocalSupport(halfExtents, v, 1, 0, 1, 2);
+	}
+
+	protected Vector3f cylinderLocalSupportZ(Vector3f halfExtents, Vector3f v) {
+		return cylinderLocalSupport(halfExtents, v, 2, 0, 2, 1);
+	}
+	
+	private Vector3f cylinderLocalSupport(Vector3f halfExtents, Vector3f v, int cylinderUpAxis, int XX, int YY, int ZZ) {
+		stack.vectors.push();
+		try {
+			//mapping depends on how cylinder local orientation is
+			// extents of the cylinder is: X,Y is for radius, and Z for height
+
+			float radius = VectorUtil.getCoord(halfExtents, XX);
+			float halfHeight = VectorUtil.getCoord(halfExtents, cylinderUpAxis);
+
+			Vector3f tmp = stack.vectors.get();
+			float d;
+
+			float s = (float) Math.sqrt(VectorUtil.getCoord(v, XX) * VectorUtil.getCoord(v, XX) + VectorUtil.getCoord(v, ZZ) * VectorUtil.getCoord(v, ZZ));
+			if (s != 0f) {
+				d = radius / s;
+				VectorUtil.setCoord(tmp, XX, VectorUtil.getCoord(v, XX) * d);
+				VectorUtil.setCoord(tmp, YY, VectorUtil.getCoord(v, YY) < 0f ? -halfHeight : halfHeight);
+				VectorUtil.setCoord(tmp, ZZ, VectorUtil.getCoord(v, ZZ) * d);
+				return stack.vectors.returning(tmp);
+			}
+			else {
+				VectorUtil.setCoord(tmp, XX, radius);
+				VectorUtil.setCoord(tmp, YY, VectorUtil.getCoord(v, YY) < 0f ? -halfHeight : halfHeight);
+				VectorUtil.setCoord(tmp, ZZ, 0f);
+				return stack.vectors.returning(tmp);
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) {
+		return cylinderLocalSupportY(getHalfExtentsWithoutMargin(), vec);
+	}
+
+	@Override
+	public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
+		for (int i = 0; i < numVectors; i++) {
+			supportVerticesOut[i].set(cylinderLocalSupportY(getHalfExtentsWithoutMargin(), vectors[i]));
+		}
+	}
+
+	@Override
+	public Vector3f localGetSupportingVertex(Vector3f vec) {
+		stack.vectors.push();
+		try {
+			Vector3f supVertex = stack.vectors.get();
+			supVertex.set(localGetSupportingVertexWithoutMargin(vec));
+
+			if (getMargin() != 0f) {
+				Vector3f vecnorm = stack.vectors.get(vec);
+				if (vecnorm.lengthSquared() < (BulletGlobals.SIMD_EPSILON * BulletGlobals.SIMD_EPSILON)) {
+					vecnorm.set(-1f, -1f, -1f);
+				}
+				vecnorm.normalize();
+				supVertex.scaleAdd(getMargin(), vecnorm, supVertex);
+			}
+			return stack.vectors.returning(supVertex);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public BroadphaseNativeType getShapeType() {
+		return BroadphaseNativeType.CYLINDER_SHAPE_PROXYTYPE;
+	}
+
+	public int getUpAxis() {
+		return upAxis;
+	}
+	
+	public float getRadius() {
+		return getHalfExtentsWithMargin().x;
+	}
+
+	@Override
+	public String getName() {
+		return "CylinderY";
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/CylinderShapeX.java b/src/jbullet/src/javabullet/collision/shapes/CylinderShapeX.java
new file mode 100644
index 0000000..1126cf1
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/CylinderShapeX.java
@@ -0,0 +1,62 @@
+/*
+ * 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.collision.shapes;
+
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class CylinderShapeX extends CylinderShape {
+
+	public CylinderShapeX(Vector3f halfExtents) {
+		super(halfExtents, false);
+		upAxis = 0;
+		recalcLocalAabb();
+	}
+
+	@Override
+	public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) {
+		return cylinderLocalSupportX(getHalfExtentsWithoutMargin(), vec);
+	}
+
+	@Override
+	public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
+		for (int i = 0; i < numVectors; i++) {
+			supportVerticesOut[i].set(cylinderLocalSupportX(getHalfExtentsWithoutMargin(), vectors[i]));
+		}
+	}
+
+	@Override
+	public float getRadius() {
+		return getHalfExtentsWithMargin().y;
+	}
+
+	@Override
+	public String getName() {
+		return "CylinderX";
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/CylinderShapeZ.java b/src/jbullet/src/javabullet/collision/shapes/CylinderShapeZ.java
new file mode 100644
index 0000000..a94812c
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/CylinderShapeZ.java
@@ -0,0 +1,62 @@
+/*
+ * 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.collision.shapes;
+
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class CylinderShapeZ extends CylinderShape {
+
+	public CylinderShapeZ(Vector3f halfExtents) {
+		super(halfExtents, false);
+		upAxis = 2;
+		recalcLocalAabb();
+	}
+
+	@Override
+	public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) {
+		return cylinderLocalSupportZ(getHalfExtentsWithoutMargin(), vec);
+	}
+
+	@Override
+	public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
+		for (int i = 0; i < numVectors; i++) {
+			supportVerticesOut[i].set(cylinderLocalSupportZ(getHalfExtentsWithoutMargin(), vectors[i]));
+		}
+	}
+
+	@Override
+	public float getRadius() {
+		return getHalfExtentsWithMargin().x;
+	}
+
+	@Override
+	public String getName() {
+		return "CylinderZ";
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/IndexedMesh.java b/src/jbullet/src/javabullet/collision/shapes/IndexedMesh.java
new file mode 100644
index 0000000..7307f66
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/IndexedMesh.java
@@ -0,0 +1,44 @@
+/*
+ * 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.collision.shapes;
+
+import java.nio.ByteBuffer;
+
+/**
+ *
+ * @author jezek2
+ */
+public class IndexedMesh {
+	
+	public int numTriangles;
+	public ByteBuffer triangleIndexBase;
+	public int triangleIndexStride;
+	public int numVertices;
+	public ByteBuffer vertexBase;
+	public int vertexStride;
+	// The index type is set when adding an indexed mesh to the
+	// TriangleIndexVertexArray, do not set it manually
+	public ScalarType indexType;
+
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/InternalTriangleIndexCallback.java b/src/jbullet/src/javabullet/collision/shapes/InternalTriangleIndexCallback.java
new file mode 100644
index 0000000..418f3eb
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/InternalTriangleIndexCallback.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.collision.shapes;
+
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public interface InternalTriangleIndexCallback {
+
+	public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/MinkowskiSumShape.java b/src/jbullet/src/javabullet/collision/shapes/MinkowskiSumShape.java
new file mode 100644
index 0000000..cb4fdf2
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/MinkowskiSumShape.java
@@ -0,0 +1,128 @@
+/*
+ * 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.collision.shapes;
+
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.linearmath.MatrixUtil;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes.
+ * 
+ * @author jezek2
+ */
+public class MinkowskiSumShape extends ConvexInternalShape {
+
+	private final Transform transA = new Transform();
+	private final Transform transB = new Transform();
+	private ConvexShape shapeA;
+	private ConvexShape shapeB;
+
+	public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB) {
+		this.shapeA = shapeA;
+		this.shapeB = shapeB;
+		this.transA.setIdentity();
+		this.transB.setIdentity();
+	}
+	
+	@Override
+	public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) {
+		stack.vectors.push();
+		try {
+			Vector3f tmp = stack.vectors.get();
+			Vector3f supVertexA = stack.vectors.get();
+			Vector3f supVertexB = stack.vectors.get();
+			
+			// btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis()));
+			MatrixUtil.transposeTransform(tmp, vec, transA.basis);
+			supVertexA.set(shapeA.localGetSupportingVertexWithoutMargin(tmp));
+			transA.transform(supVertexA);
+
+			// btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(vec*m_transB.getBasis()));
+			MatrixUtil.transposeTransform(tmp, vec, transB.basis);
+			supVertexB.set(shapeB.localGetSupportingVertexWithoutMargin(tmp));
+			transB.transform(supVertexB);
+
+			//return supVertexA + supVertexB;
+			Vector3f result = stack.vectors.get();
+			result.add(supVertexA, supVertexB);
+			return stack.vectors.returning(result);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
+		//todo: could make recursive use of batching. probably this shape is not used frequently.
+		for (int i = 0; i < numVectors; i++) {
+			supportVerticesOut[i].set(localGetSupportingVertexWithoutMargin(vectors[i]));
+		}
+	}
+
+	@Override
+	public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) {
+		throw new UnsupportedOperationException("Not supported yet.");
+	}
+
+	@Override
+	public BroadphaseNativeType getShapeType() {
+		return BroadphaseNativeType.MINKOWSKI_SUM_SHAPE_PROXYTYPE;
+	}
+
+	@Override
+	public void calculateLocalInertia(float mass, Vector3f inertia) {
+		assert (false);
+		inertia.set(0, 0, 0);
+	}
+
+	@Override
+	public String getName() {
+		return "MinkowskiSum";
+	}
+	
+	@Override
+	public float getMargin() {
+		return shapeA.getMargin() + shapeB.getMargin();
+	}
+
+	public void setTransformA(Transform transA) {
+		this.transA.set(transA);
+	}
+
+	public void setTransformB(Transform transB) {
+		this.transB.set(transB);
+	}
+
+	public void getTransformA(Transform dest) {
+		dest.set(transA);
+	}
+
+	public void getTransformB(Transform dest) {
+		dest.set(transB);
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/NodeOverlapCallback.java b/src/jbullet/src/javabullet/collision/shapes/NodeOverlapCallback.java
new file mode 100644
index 0000000..216e0d7
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/NodeOverlapCallback.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.collision.shapes;
+
+/**
+ *
+ * @author jezek2
+ */
+public interface NodeOverlapCallback {
+
+	public void processNode(int subPart, int triangleIndex);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/OptimizedBvh.java b/src/jbullet/src/javabullet/collision/shapes/OptimizedBvh.java
new file mode 100644
index 0000000..8ae7621
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/OptimizedBvh.java
@@ -0,0 +1,943 @@
+/*
+ * 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.collision.shapes;
+
+import java.nio.ByteBuffer;
+import java.util.ArrayList;
+import java.util.List;
+import javabullet.BulletStack;
+import javabullet.linearmath.AabbUtil2;
+import javabullet.linearmath.MiscUtil;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ * OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future).
+ * 
+ * @author jezek2
+ */
+public class OptimizedBvh {
+
+	protected final BulletStack stack = BulletStack.get();
+	
+	private static final boolean DEBUG_TREE_BUILDING = false;
+	private static int gStackDepth = 0;
+	private static int gMaxStackDepth = 0;
+	
+	private static int maxIterations = 0;
+	
+	// Note: currently we have 16 bytes per quantized node
+	public static final int MAX_SUBTREE_SIZE_IN_BYTES = 2048;
+
+	// 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one
+	// actually) triangles each (since the sign bit is reserved
+	public static final int MAX_NUM_PARTS_IN_BITS = 10;
+	
+	////////////////////////////////////////////////////////////////////////////
+
+	private final List<OptimizedBvhNode> leafNodes = new ArrayList<OptimizedBvhNode>();
+	private final List<OptimizedBvhNode> contiguousNodes = new ArrayList<OptimizedBvhNode>();
+
+	private QuantizedBvhNodes quantizedLeafNodes = new QuantizedBvhNodes();
+	private QuantizedBvhNodes quantizedContiguousNodes = new QuantizedBvhNodes();
+	
+	private int curNodeIndex;
+
+	// quantization data
+	private boolean useQuantization;
+	private final Vector3f bvhAabbMin = new Vector3f();
+	private final Vector3f bvhAabbMax = new Vector3f();
+	private final Vector3f bvhQuantization = new Vector3f();
+	
+	protected TraversalMode traversalMode;
+	protected final List<BvhSubtreeInfo> SubtreeHeaders = new ArrayList<BvhSubtreeInfo>();
+	// This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray
+	protected int subtreeHeaderCount;
+
+	// two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!)
+	// this might be refactored into a virtual, it is usually not calculated at run-time
+	public void setInternalNodeAabbMin(int nodeIndex, Vector3f aabbMin) {
+		if (useQuantization) {
+			quantizedContiguousNodes.setQuantizedAabbMin(nodeIndex, quantizeWithClamp(aabbMin));
+		}
+		else {
+			contiguousNodes.get(nodeIndex).aabbMinOrg.set(aabbMin);
+		}
+	}
+
+	public void setInternalNodeAabbMax(int nodeIndex, Vector3f aabbMax) {
+		if (useQuantization) {
+			quantizedContiguousNodes.setQuantizedAabbMax(nodeIndex, quantizeWithClamp(aabbMax));
+		}
+		else {
+			contiguousNodes.get(nodeIndex).aabbMaxOrg.set(aabbMax);
+		}
+	}
+	
+	public Vector3f getAabbMin(int nodeIndex) {
+		if (useQuantization) {
+			Vector3f tmp = new Vector3f();
+			unQuantize(tmp, quantizedLeafNodes.getQuantizedAabbMin(nodeIndex));
+			return tmp;
+		}
+
+		// non-quantized
+		return leafNodes.get(nodeIndex).aabbMinOrg;
+	}
+
+	public Vector3f getAabbMax(int nodeIndex) {
+		if (useQuantization) {
+			Vector3f tmp = new Vector3f();
+			unQuantize(tmp, quantizedLeafNodes.getQuantizedAabbMax(nodeIndex));
+			return tmp;
+		}
+		
+		// non-quantized
+		return leafNodes.get(nodeIndex).aabbMaxOrg;
+	}
+
+	public void setQuantizationValues(Vector3f aabbMin, Vector3f aabbMax) {
+		setQuantizationValues(aabbMin, aabbMax, 1f);
+	}
+	
+	public void setQuantizationValues(Vector3f aabbMin, Vector3f aabbMax, float quantizationMargin) {
+		stack.vectors.push();
+		try {
+			// enlarge the AABB to avoid division by zero when initializing the quantization values
+			Vector3f clampValue = stack.vectors.get(quantizationMargin,quantizationMargin,quantizationMargin);
+			bvhAabbMin.sub(aabbMin, clampValue);
+			bvhAabbMax.add(aabbMax, clampValue);
+			Vector3f aabbSize = stack.vectors.get();
+			aabbSize.sub(bvhAabbMax, bvhAabbMin);
+			bvhQuantization.set(65535f, 65535f, 65535f);
+			VectorUtil.div(bvhQuantization, bvhQuantization, aabbSize);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) {
+		if (useQuantization) {
+			quantizedContiguousNodes.setEscapeIndexOrTriangleIndex(nodeIndex, -escapeIndex);
+		}
+		else {
+			contiguousNodes.get(nodeIndex).escapeIndex = escapeIndex;
+		}
+	}
+
+	public void mergeInternalNodeAabb(int nodeIndex, Vector3f newAabbMin, Vector3f newAabbMax) {
+		if (useQuantization) {
+			long quantizedAabbMin;
+			long quantizedAabbMax;
+
+			quantizedAabbMin = quantizeWithClamp(newAabbMin);
+			quantizedAabbMax = quantizeWithClamp(newAabbMax);
+			for (int i = 0; i < 3; i++) {
+				if (quantizedContiguousNodes.getQuantizedAabbMin(nodeIndex, i) > QuantizedBvhNodes.getCoord(quantizedAabbMin, i)) {
+					quantizedContiguousNodes.setQuantizedAabbMin(nodeIndex, i, QuantizedBvhNodes.getCoord(quantizedAabbMin, i));
+				}
+
+				if (quantizedContiguousNodes.getQuantizedAabbMax(nodeIndex, i) < QuantizedBvhNodes.getCoord(quantizedAabbMax, i)) {
+					quantizedContiguousNodes.setQuantizedAabbMax(nodeIndex, i, QuantizedBvhNodes.getCoord(quantizedAabbMax, i));
+				}
+			}
+		}
+		else {
+			// non-quantized
+			VectorUtil.setMin(contiguousNodes.get(nodeIndex).aabbMinOrg, newAabbMin);
+			VectorUtil.setMax(contiguousNodes.get(nodeIndex).aabbMaxOrg, newAabbMax);
+		}
+	}
+	
+	public void swapLeafNodes(int i, int splitIndex) {
+		if (useQuantization) {
+			quantizedLeafNodes.swap(i, splitIndex);
+		}
+		else {
+			// JAVA NOTE: changing reference instead of copy
+			OptimizedBvhNode tmp = leafNodes.get(i);
+			leafNodes.set(i, leafNodes.get(splitIndex));
+			leafNodes.set(splitIndex, tmp);
+		}
+	}
+
+	public void assignInternalNodeFromLeafNode(int internalNode, int leafNodeIndex) {
+		if (useQuantization) {
+			quantizedContiguousNodes.set(internalNode, quantizedLeafNodes, leafNodeIndex);
+		}
+		else {
+			contiguousNodes.get(internalNode).set(leafNodes.get(leafNodeIndex));
+		}
+	}
+
+	private static class NodeTriangleCallback implements InternalTriangleIndexCallback {
+		public List<OptimizedBvhNode> triangleNodes;
+		
+		public NodeTriangleCallback(List<OptimizedBvhNode> triangleNodes) {
+			this.triangleNodes = triangleNodes;
+		}
+
+		private final Vector3f aabbMin = new Vector3f(), aabbMax = new Vector3f();
+		
+		public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) {
+			OptimizedBvhNode node = new OptimizedBvhNode();
+			aabbMin.set(1e30f, 1e30f, 1e30f);
+			aabbMax.set(-1e30f, -1e30f, -1e30f);
+			VectorUtil.setMin(aabbMin, triangle[0]);
+			VectorUtil.setMax(aabbMax, triangle[0]);
+			VectorUtil.setMin(aabbMin, triangle[1]);
+			VectorUtil.setMax(aabbMax, triangle[1]);
+			VectorUtil.setMin(aabbMin, triangle[2]);
+			VectorUtil.setMax(aabbMax, triangle[2]);
+
+			// with quantization?
+			node.aabbMinOrg.set(aabbMin);
+			node.aabbMaxOrg.set(aabbMax);
+
+			node.escapeIndex = -1;
+
+			// for child nodes
+			node.subPart = partId;
+			node.triangleIndex = triangleIndex;
+			triangleNodes.add(node);
+		}
+	}
+
+	private static class QuantizedNodeTriangleCallback implements InternalTriangleIndexCallback {
+		protected final BulletStack stack = BulletStack.get();
+		
+		public QuantizedBvhNodes triangleNodes;
+		public OptimizedBvh optimizedTree; // for quantization
+
+		public QuantizedNodeTriangleCallback(QuantizedBvhNodes triangleNodes, OptimizedBvh tree) {
+			this.triangleNodes = triangleNodes;
+			this.optimizedTree = tree;
+		}
+		
+		public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) {
+			// The partId and triangle index must fit in the same (positive) integer
+			assert (partId < (1 << MAX_NUM_PARTS_IN_BITS));
+			assert (triangleIndex < (1 << (31 - MAX_NUM_PARTS_IN_BITS)));
+			// negative indices are reserved for escapeIndex
+			assert (triangleIndex >= 0);
+
+			stack.vectors.push();
+			try {
+				int nodeId = triangleNodes.add();
+				Vector3f aabbMin = stack.vectors.get(), aabbMax = stack.vectors.get();
+				aabbMin.set(1e30f, 1e30f, 1e30f);
+				aabbMax.set(-1e30f, -1e30f, -1e30f);
+				VectorUtil.setMin(aabbMin, triangle[0]);
+				VectorUtil.setMax(aabbMax, triangle[0]);
+				VectorUtil.setMin(aabbMin, triangle[1]);
+				VectorUtil.setMax(aabbMax, triangle[1]);
+				VectorUtil.setMin(aabbMin, triangle[2]);
+				VectorUtil.setMax(aabbMax, triangle[2]);
+
+				// PCK: add these checks for zero dimensions of aabb
+				final float MIN_AABB_DIMENSION = 0.002f;
+				final float MIN_AABB_HALF_DIMENSION = 0.001f;
+				if (aabbMax.x - aabbMin.x < MIN_AABB_DIMENSION) {
+					aabbMax.x = (aabbMax.x + MIN_AABB_HALF_DIMENSION);
+					aabbMin.x = (aabbMin.x - MIN_AABB_HALF_DIMENSION);
+				}
+				if (aabbMax.y - aabbMin.y < MIN_AABB_DIMENSION) {
+					aabbMax.y = (aabbMax.y + MIN_AABB_HALF_DIMENSION);
+					aabbMin.y = (aabbMin.y - MIN_AABB_HALF_DIMENSION);
+				}
+				if (aabbMax.z - aabbMin.z < MIN_AABB_DIMENSION) {
+					aabbMax.z = (aabbMax.z + MIN_AABB_HALF_DIMENSION);
+					aabbMin.z = (aabbMin.z - MIN_AABB_HALF_DIMENSION);
+				}
+
+				triangleNodes.setQuantizedAabbMin(nodeId, optimizedTree.quantizeWithClamp(aabbMin));
+				triangleNodes.setQuantizedAabbMax(nodeId, optimizedTree.quantizeWithClamp(aabbMax));
+
+				triangleNodes.setEscapeIndexOrTriangleIndex(nodeId, (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex);
+			}
+			finally {
+				stack.vectors.pop();
+			}
+		}
+	}
+	
+	public void build(StridingMeshInterface triangles, boolean useQuantizedAabbCompression, Vector3f _aabbMin, Vector3f _aabbMax) {
+		stack.vectors.push();
+		try {
+			this.useQuantization = useQuantizedAabbCompression;
+
+			// NodeArray	triangleNodes;
+
+			int numLeafNodes = 0;
+
+			if (useQuantization) {
+				// initialize quantization values
+				setQuantizationValues(_aabbMin, _aabbMax);
+
+				QuantizedNodeTriangleCallback callback = new QuantizedNodeTriangleCallback(quantizedLeafNodes, this);
+
+				triangles.internalProcessAllTriangles(callback, bvhAabbMin, bvhAabbMax);
+
+				// now we have an array of leafnodes in m_leafNodes
+				numLeafNodes = quantizedLeafNodes.size();
+
+				quantizedContiguousNodes.resize(2 * numLeafNodes);
+			}
+			else {
+				NodeTriangleCallback callback = new NodeTriangleCallback(leafNodes);
+
+				Vector3f aabbMin = stack.vectors.get(-1e30f, -1e30f, -1e30f);
+				Vector3f aabbMax = stack.vectors.get(1e30f, 1e30f, 1e30f);
+
+				triangles.internalProcessAllTriangles(callback, aabbMin, aabbMax);
+
+				// now we have an array of leafnodes in m_leafNodes
+				numLeafNodes = leafNodes.size();
+
+				// TODO: check
+				//contiguousNodes.resize(2*numLeafNodes);
+				MiscUtil.resize(contiguousNodes, 2 * numLeafNodes, OptimizedBvhNode.class);
+			}
+
+			curNodeIndex = 0;
+
+			buildTree(0, numLeafNodes);
+
+			//  if the entire tree is small then subtree size, we need to create a header info for the tree
+			if (useQuantization && SubtreeHeaders.size() == 0) {
+				BvhSubtreeInfo subtree = new BvhSubtreeInfo();
+				SubtreeHeaders.add(subtree);
+
+				subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, 0);
+				subtree.rootNodeIndex = 0;
+				subtree.subtreeSize = quantizedContiguousNodes.isLeafNode(0) ? 1 : quantizedContiguousNodes.getEscapeIndex(0);
+			}
+
+			// PCK: update the copy of the size
+			subtreeHeaderCount = SubtreeHeaders.size();
+
+			// PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary
+			quantizedLeafNodes.clear();
+			leafNodes.clear();
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public void refit(StridingMeshInterface meshInterface) {
+		stack.vectors.push();
+		try {
+			if (useQuantization) {
+				// calculate new aabb
+				Vector3f aabbMin = stack.vectors.get(), aabbMax = stack.vectors.get();
+				meshInterface.calculateAabbBruteForce(aabbMin, aabbMax);
+
+				setQuantizationValues(aabbMin, aabbMax);
+
+				updateBvhNodes(meshInterface, 0, curNodeIndex, 0);
+
+				// now update all subtree headers
+
+				int i;
+				for (i = 0; i < SubtreeHeaders.size(); i++) {
+					BvhSubtreeInfo subtree = SubtreeHeaders.get(i);
+					subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, subtree.rootNodeIndex);
+				}
+
+			}
+			else {
+				// JAVA NOTE: added for testing, it's too slow for practical use
+				build(meshInterface, false, null, null);
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public void refitPartial(StridingMeshInterface meshInterface, Vector3f aabbMin, Vector3f aabbMax) {
+		throw new UnsupportedOperationException();
+//		// incrementally initialize quantization values
+//		assert (useQuantization);
+//
+//		btAssert(aabbMin.getX() > m_bvhAabbMin.getX());
+//		btAssert(aabbMin.getY() > m_bvhAabbMin.getY());
+//		btAssert(aabbMin.getZ() > m_bvhAabbMin.getZ());
+//
+//		btAssert(aabbMax.getX() < m_bvhAabbMax.getX());
+//		btAssert(aabbMax.getY() < m_bvhAabbMax.getY());
+//		btAssert(aabbMax.getZ() < m_bvhAabbMax.getZ());
+//
+//		///we should update all quantization values, using updateBvhNodes(meshInterface);
+//		///but we only update chunks that overlap the given aabb
+//
+//		unsigned short	quantizedQueryAabbMin[3];
+//		unsigned short	quantizedQueryAabbMax[3];
+//
+//		quantizeWithClamp(&quantizedQueryAabbMin[0],aabbMin);
+//		quantizeWithClamp(&quantizedQueryAabbMax[0],aabbMax);
+//
+//		int i;
+//		for (i=0;i<this->m_SubtreeHeaders.size();i++)
+//		{
+//			btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
+//
+//			//PCK: unsigned instead of bool
+//			unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
+//			if (overlap != 0)
+//			{
+//				updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i);
+//
+//				subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
+//			}
+//		}
+	}
+
+	private VertexData data = new VertexData();
+	
+	public void updateBvhNodes(StridingMeshInterface meshInterface, int firstNode, int endNode, int index) {
+		assert (useQuantization);
+
+		stack.vectors.push();
+		try {
+			int curNodeSubPart = -1;
+
+			Vector3f[] triangleVerts/*[3]*/ = new Vector3f[] { stack.vectors.get(), stack.vectors.get(), stack.vectors.get() };
+			Vector3f aabbMin = stack.vectors.get(), aabbMax = stack.vectors.get();
+			Vector3f meshScaling = meshInterface.getScaling();
+
+			for (int i = endNode - 1; i >= firstNode; i--) {
+				QuantizedBvhNodes curNodes = quantizedContiguousNodes;
+				int curNodeId = i;
+
+				if (curNodes.isLeafNode(curNodeId)) {
+					// recalc aabb from triangle data
+					int nodeSubPart = curNodes.getPartId(curNodeId);
+					int nodeTriangleIndex = curNodes.getTriangleIndex(curNodeId);
+					if (nodeSubPart != curNodeSubPart) {
+						if (curNodeSubPart >= 0) {
+							meshInterface.unLockReadOnlyVertexBase(curNodeSubPart);
+						}
+						meshInterface.getLockedReadOnlyVertexIndexBase(data, nodeSubPart);
+						assert (data.indicestype == ScalarType.PHY_INTEGER || data.indicestype == ScalarType.PHY_SHORT);
+					}
+					//triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts,
+
+					ByteBuffer gfxbase_ptr = data.indexbase;
+					int gfxbase_index = nodeTriangleIndex * data.indexstride;
+
+					for (int j = 2; j >= 0; j--) {
+						int graphicsindex;
+						if (data.indicestype == ScalarType.PHY_SHORT) {
+							graphicsindex = gfxbase_ptr.getShort(gfxbase_index + j * 2) & 0xFFFF;
+						}
+						else {
+							graphicsindex = gfxbase_ptr.getInt(gfxbase_index + j * 4);
+						}
+
+						ByteBuffer graphicsbase_ptr = data.vertexbase;
+						int graphicsbase_index = graphicsindex * data.stride;
+
+						//#ifdef DEBUG_PATCH_COLORS
+						//btVector3 mycolor = color[index&3];
+						//graphicsbase[8] = mycolor.getX();
+						//graphicsbase[9] = mycolor.getY();
+						//graphicsbase[10] = mycolor.getZ();
+						//#endif //DEBUG_PATCH_COLORS
+
+						triangleVerts[j].set(
+								graphicsbase_ptr.getFloat(graphicsbase_index + 4 * 0) * meshScaling.x,
+								graphicsbase_ptr.getFloat(graphicsbase_index + 4 * 1) * meshScaling.y,
+								graphicsbase_ptr.getFloat(graphicsbase_index + 4 * 2) * meshScaling.z);
+					}
+
+					aabbMin.set(1e30f, 1e30f, 1e30f);
+					aabbMax.set(-1e30f, -1e30f, -1e30f);
+					VectorUtil.setMin(aabbMin, triangleVerts[0]);
+					VectorUtil.setMax(aabbMax, triangleVerts[0]);
+					VectorUtil.setMin(aabbMin, triangleVerts[1]);
+					VectorUtil.setMax(aabbMax, triangleVerts[1]);
+					VectorUtil.setMin(aabbMin, triangleVerts[2]);
+					VectorUtil.setMax(aabbMax, triangleVerts[2]);
+
+					curNodes.setQuantizedAabbMin(curNodeId, quantizeWithClamp(aabbMin));
+					curNodes.setQuantizedAabbMax(curNodeId, quantizeWithClamp(aabbMax));
+				}
+				else {
+					// combine aabb from both children
+
+					//quantizedContiguousNodes
+					int leftChildNodeId = i + 1;
+
+					int rightChildNodeId = quantizedContiguousNodes.isLeafNode(leftChildNodeId) ? i + 2 : i + 1 + quantizedContiguousNodes.getEscapeIndex(leftChildNodeId);
+
+					for (int i2 = 0; i2 < 3; i2++) {
+						curNodes.setQuantizedAabbMin(curNodeId, i2, quantizedContiguousNodes.getQuantizedAabbMin(leftChildNodeId, i2));
+						if (curNodes.getQuantizedAabbMin(curNodeId, i2) > quantizedContiguousNodes.getQuantizedAabbMin(rightChildNodeId, i2)) {
+							curNodes.setQuantizedAabbMin(curNodeId, i2, quantizedContiguousNodes.getQuantizedAabbMin(rightChildNodeId, i2));
+						}
+
+						curNodes.setQuantizedAabbMax(curNodeId, i2, quantizedContiguousNodes.getQuantizedAabbMax(leftChildNodeId, i2));
+						if (curNodes.getQuantizedAabbMax(curNodeId, i2) < quantizedContiguousNodes.getQuantizedAabbMax(rightChildNodeId, i2)) {
+							curNodes.setQuantizedAabbMax(curNodeId, i2, quantizedContiguousNodes.getQuantizedAabbMax(rightChildNodeId, i2));
+						}
+					}
+				}
+			}
+
+			if (curNodeSubPart >= 0) {
+				meshInterface.unLockReadOnlyVertexBase(curNodeSubPart);
+			}
+			
+			data.unref();
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	protected void buildTree(int startIndex, int endIndex) {
+		stack.vectors.push();
+		try {
+			//#ifdef DEBUG_TREE_BUILDING
+			if (DEBUG_TREE_BUILDING) {
+				gStackDepth++;
+				if (gStackDepth > gMaxStackDepth) {
+					gMaxStackDepth = gStackDepth;
+				}
+			}
+			//#endif //DEBUG_TREE_BUILDING
+
+			int splitAxis, splitIndex, i;
+			int numIndices = endIndex - startIndex;
+			int curIndex = curNodeIndex;
+
+			assert (numIndices > 0);
+
+			if (numIndices == 1) {
+				//#ifdef DEBUG_TREE_BUILDING
+				if (DEBUG_TREE_BUILDING) {
+					gStackDepth--;
+				}
+				//#endif //DEBUG_TREE_BUILDING
+
+				assignInternalNodeFromLeafNode(curNodeIndex, startIndex);
+
+				curNodeIndex++;
+				return;
+			}
+			// calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+			splitAxis = calcSplittingAxis(startIndex, endIndex);
+
+			splitIndex = sortAndCalcSplittingIndex(startIndex, endIndex, splitAxis);
+
+			int internalNodeIndex = curNodeIndex;
+
+			setInternalNodeAabbMax(curNodeIndex, stack.vectors.get(-1e30f, -1e30f, -1e30f));
+			setInternalNodeAabbMin(curNodeIndex, stack.vectors.get(1e30f, 1e30f, 1e30f));
+
+			for (i = startIndex; i < endIndex; i++) {
+				mergeInternalNodeAabb(curNodeIndex, getAabbMin(i), getAabbMax(i));
+			}
+
+			curNodeIndex++;
+
+			//internalNode->m_escapeIndex;
+
+			int leftChildNodexIndex = curNodeIndex;
+
+			//build left child tree
+			buildTree(startIndex, splitIndex);
+
+			int rightChildNodexIndex = curNodeIndex;
+			// build right child tree
+			buildTree(splitIndex, endIndex);
+
+			//#ifdef DEBUG_TREE_BUILDING
+			if (DEBUG_TREE_BUILDING) {
+				gStackDepth--;
+			}
+			//#endif //DEBUG_TREE_BUILDING
+
+			int escapeIndex = curNodeIndex - curIndex;
+
+			if (useQuantization) {
+				// escapeIndex is the number of nodes of this subtree
+				int sizeQuantizedNode = QuantizedBvhNodes.getNodeSize();
+				int treeSizeInBytes = escapeIndex * sizeQuantizedNode;
+				if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES) {
+					updateSubtreeHeaders(leftChildNodexIndex, rightChildNodexIndex);
+				}
+			}
+
+			setInternalNodeEscapeIndex(internalNodeIndex, escapeIndex);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	protected boolean testQuantizedAabbAgainstQuantizedAabb(long aabbMin1, long aabbMax1, long aabbMin2, long aabbMax2) {
+		int aabbMin1_0 = QuantizedBvhNodes.getCoord(aabbMin1, 0);
+		int aabbMin1_1 = QuantizedBvhNodes.getCoord(aabbMin1, 1);
+		int aabbMin1_2 = QuantizedBvhNodes.getCoord(aabbMin1, 2);
+
+		int aabbMax1_0 = QuantizedBvhNodes.getCoord(aabbMax1, 0);
+		int aabbMax1_1 = QuantizedBvhNodes.getCoord(aabbMax1, 1);
+		int aabbMax1_2 = QuantizedBvhNodes.getCoord(aabbMax1, 2);
+
+		int aabbMin2_0 = QuantizedBvhNodes.getCoord(aabbMin2, 0);
+		int aabbMin2_1 = QuantizedBvhNodes.getCoord(aabbMin2, 1);
+		int aabbMin2_2 = QuantizedBvhNodes.getCoord(aabbMin2, 2);
+
+		int aabbMax2_0 = QuantizedBvhNodes.getCoord(aabbMax2, 0);
+		int aabbMax2_1 = QuantizedBvhNodes.getCoord(aabbMax2, 1);
+		int aabbMax2_2 = QuantizedBvhNodes.getCoord(aabbMax2, 2);
+
+		boolean overlap = true;
+		overlap = (aabbMin1_0 > aabbMax2_0 || aabbMax1_0 < aabbMin2_0) ? false : overlap;
+		overlap = (aabbMin1_2 > aabbMax2_2 || aabbMax1_2 < aabbMin2_2) ? false : overlap;
+		overlap = (aabbMin1_1 > aabbMax2_1 || aabbMax1_1 < aabbMin2_1) ? false : overlap;
+		return overlap;
+	}
+
+	protected void updateSubtreeHeaders(int leftChildNodexIndex, int rightChildNodexIndex) {
+		assert (useQuantization);
+
+		//btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex];
+		int leftSubTreeSize = quantizedContiguousNodes.isLeafNode(leftChildNodexIndex) ? 1 : quantizedContiguousNodes.getEscapeIndex(leftChildNodexIndex);
+		int leftSubTreeSizeInBytes = leftSubTreeSize * QuantizedBvhNodes.getNodeSize();
+
+		//btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex];
+		int rightSubTreeSize = quantizedContiguousNodes.isLeafNode(rightChildNodexIndex) ? 1 : quantizedContiguousNodes.getEscapeIndex(rightChildNodexIndex);
+		int rightSubTreeSizeInBytes = rightSubTreeSize * QuantizedBvhNodes.getNodeSize();
+
+		if (leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) {
+			BvhSubtreeInfo subtree = new BvhSubtreeInfo();
+			SubtreeHeaders.add(subtree);
+
+			subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, leftChildNodexIndex);
+			subtree.rootNodeIndex = leftChildNodexIndex;
+			subtree.subtreeSize = leftSubTreeSize;
+		}
+
+		if (rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) {
+			BvhSubtreeInfo subtree = new BvhSubtreeInfo();
+			SubtreeHeaders.add(subtree);
+
+			subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, rightChildNodexIndex);
+			subtree.rootNodeIndex = rightChildNodexIndex;
+			subtree.subtreeSize = rightSubTreeSize;
+		}
+
+		// PCK: update the copy of the size
+		subtreeHeaderCount = SubtreeHeaders.size();
+	}
+	
+	protected int sortAndCalcSplittingIndex(int startIndex, int endIndex, int splitAxis) {
+		stack.vectors.push();
+		try {
+			int i;
+			int splitIndex = startIndex;
+			int numIndices = endIndex - startIndex;
+			float splitValue;
+
+			Vector3f means = stack.vectors.get(0f, 0f, 0f);
+			Vector3f center = stack.vectors.get();
+			for (i = startIndex; i < endIndex; i++) {
+				center.add(getAabbMax(i), getAabbMin(i));
+				center.scale(0.5f);
+				means.add(center);
+			}
+			means.scale(1f / (float) numIndices);
+
+			splitValue = VectorUtil.getCoord(means, splitAxis);
+
+			//sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+			for (i = startIndex; i < endIndex; i++) {
+				//Vector3f center = new Vector3f();
+				center.add(getAabbMax(i), getAabbMin(i));
+				center.scale(0.5f);
+
+				if (VectorUtil.getCoord(center, splitAxis) > splitValue) {
+					// swap
+					swapLeafNodes(i, splitIndex);
+					splitIndex++;
+				}
+			}
+
+			// if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+			// otherwise the tree-building might fail due to stack-overflows in certain cases.
+			// unbalanced1 is unsafe: it can cause stack overflows
+			// bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+			// unbalanced2 should work too: always use center (perfect balanced trees)	
+			// bool unbalanced2 = true;
+
+			// this should be safe too:
+			int rangeBalancedIndices = numIndices / 3;
+			boolean unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices)));
+
+			if (unbalanced) {
+				splitIndex = startIndex + (numIndices >> 1);
+			}
+
+			boolean unbal = (splitIndex == startIndex) || (splitIndex == (endIndex));
+			assert (!unbal);
+
+			return splitIndex;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	protected int calcSplittingAxis(int startIndex, int endIndex) {
+		stack.vectors.push();
+		try {
+			int i;
+
+			Vector3f means = stack.vectors.get(0f, 0f, 0f);
+			Vector3f variance = stack.vectors.get(0f, 0f, 0f);
+			int numIndices = endIndex - startIndex;
+
+			Vector3f center = stack.vectors.get();
+			for (i = startIndex; i < endIndex; i++) {
+				center.add(getAabbMax(i), getAabbMin(i));
+				center.scale(0.5f);
+				means.add(center);
+			}
+			means.scale(1f / (float) numIndices);
+
+			Vector3f diff2 = stack.vectors.get();
+			for (i = startIndex; i < endIndex; i++) {
+				center.add(getAabbMax(i), getAabbMin(i));
+				center.scale(0.5f);
+				diff2.sub(center, means);
+				//diff2 = diff2 * diff2;
+				VectorUtil.mul(diff2, diff2, diff2);
+				variance.add(diff2);
+			}
+			variance.scale(1f / ((float) numIndices - 1));
+
+			return VectorUtil.maxAxis(variance);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void reportAabbOverlappingNodex(NodeOverlapCallback nodeCallback, Vector3f aabbMin, Vector3f aabbMax) {
+		// either choose recursive traversal (walkTree) or stackless (walkStacklessTree)
+
+		if (useQuantization) {
+			// quantize query AABB
+			long quantizedQueryAabbMin;
+			long quantizedQueryAabbMax;
+			quantizedQueryAabbMin = quantizeWithClamp(aabbMin);
+			quantizedQueryAabbMax = quantizeWithClamp(aabbMax);
+
+			// TODO
+			/*
+			switch (traversalMode) {
+				case TRAVERSAL_STACKLESS:
+					walkStacklessQuantizedTree(nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax, 0, curNodeIndex);
+					break;
+					
+				case TRAVERSAL_STACKLESS_CACHE_FRIENDLY:
+					walkStacklessQuantizedTreeCacheFriendly(nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax);
+					break;
+					
+				case TRAVERSAL_RECURSIVE:*/
+					walkRecursiveQuantizedTreeAgainstQueryAabb(quantizedContiguousNodes, 0, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax);
+					/*break;
+					
+				default:
+					assert (false); // unsupported
+			}*/
+		}
+		else {
+			walkStacklessTree(nodeCallback, aabbMin, aabbMax);
+		}
+	}
+	
+	protected void walkStacklessTree(NodeOverlapCallback nodeCallback, Vector3f aabbMin, Vector3f aabbMax) {
+		assert (!useQuantization);
+
+		// JAVA NOTE: rewritten
+		OptimizedBvhNode rootNode = null;//contiguousNodes.get(0);
+		int rootNode_index = 0;
+
+		int escapeIndex, curIndex = 0;
+		int walkIterations = 0;
+		boolean isLeafNode;
+		//PCK: unsigned instead of bool
+		//unsigned aabbOverlap;
+		boolean aabbOverlap;
+
+		while (curIndex < curNodeIndex) {
+			// catch bugs in tree data
+			assert (walkIterations < curNodeIndex);
+
+			walkIterations++;
+
+			rootNode = contiguousNodes.get(rootNode_index);
+
+			aabbOverlap = AabbUtil2.testAabbAgainstAabb2(aabbMin, aabbMax, rootNode.aabbMinOrg, rootNode.aabbMaxOrg);
+			isLeafNode = (rootNode.escapeIndex == -1);
+
+			// PCK: unsigned instead of bool
+			if (isLeafNode && (aabbOverlap/* != 0*/)) {
+				nodeCallback.processNode(rootNode.subPart, rootNode.triangleIndex);
+			}
+
+			rootNode = null;
+
+			//PCK: unsigned instead of bool
+			if ((aabbOverlap/* != 0*/) || isLeafNode) {
+				rootNode_index++;
+				curIndex++;
+			}
+			else {
+				escapeIndex = /*rootNode*/ contiguousNodes.get(rootNode_index).escapeIndex;
+				rootNode_index += escapeIndex;
+				curIndex += escapeIndex;
+			}
+		}
+		if (maxIterations < walkIterations) {
+			maxIterations = walkIterations;
+		}
+	}
+
+	protected void walkRecursiveQuantizedTreeAgainstQueryAabb(QuantizedBvhNodes currentNodes, int currentNodeId, NodeOverlapCallback nodeCallback, long quantizedQueryAabbMin, long quantizedQueryAabbMax) {
+		assert (useQuantization);
+
+		boolean isLeafNode;
+		boolean aabbOverlap;
+
+		aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, currentNodes.getQuantizedAabbMin(currentNodeId), currentNodes.getQuantizedAabbMax(currentNodeId));
+		isLeafNode = currentNodes.isLeafNode(currentNodeId);
+
+		if (aabbOverlap) {
+			if (isLeafNode) {
+				nodeCallback.processNode(currentNodes.getPartId(currentNodeId), currentNodes.getTriangleIndex(currentNodeId));
+			}
+			else {
+				// process left and right children
+				int leftChildNodeId = currentNodeId + 1;
+				walkRecursiveQuantizedTreeAgainstQueryAabb(currentNodes, leftChildNodeId, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax);
+
+				int rightChildNodeId = currentNodes.isLeafNode(leftChildNodeId) ? leftChildNodeId + 1 : leftChildNodeId + currentNodes.getEscapeIndex(leftChildNodeId);
+				walkRecursiveQuantizedTreeAgainstQueryAabb(currentNodes, rightChildNodeId, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax);
+			}
+		}
+	}
+	
+	public void reportRayOverlappingNodex(NodeOverlapCallback nodeCallback, Vector3f raySource, Vector3f rayTarget) {
+		stack.vectors.push();
+		try {
+			boolean fast_path = useQuantization && traversalMode == TraversalMode.TRAVERSAL_STACKLESS;
+			if (fast_path) {
+				throw new UnsupportedOperationException();
+				//walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0), 0, m_curNodeIndex);
+			}
+			else {
+				/* Otherwise fallback to AABB overlap test */
+				Vector3f aabbMin = stack.vectors.get(raySource);
+				Vector3f aabbMax = stack.vectors.get(raySource);
+				VectorUtil.setMin(aabbMin, rayTarget);
+				VectorUtil.setMax(aabbMax, rayTarget);
+				reportAabbOverlappingNodex(nodeCallback, aabbMin, aabbMax);
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void reportBoxCastOverlappingNodex(NodeOverlapCallback nodeCallback, Vector3f raySource, Vector3f rayTarget, Vector3f aabbMin, Vector3f aabbMax) {
+		stack.vectors.push();
+		try {
+			boolean fast_path = useQuantization && traversalMode == TraversalMode.TRAVERSAL_STACKLESS;
+			if (fast_path) {
+				throw new UnsupportedOperationException();
+				//walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex);
+			}
+			else {
+				/* Slow path:
+				Construct the bounding box for the entire box cast and send that down the tree */
+				Vector3f qaabbMin = stack.vectors.get(raySource);
+				Vector3f qaabbMax = stack.vectors.get(raySource);
+				VectorUtil.setMin(qaabbMin, rayTarget);
+				VectorUtil.setMax(qaabbMax, rayTarget);
+				qaabbMin.add(aabbMin);
+				qaabbMax.add(aabbMax);
+				reportAabbOverlappingNodex(nodeCallback, qaabbMin, qaabbMax);
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public long quantizeWithClamp(Vector3f point) {
+		assert (useQuantization);
+
+		stack.vectors.push();
+		try {
+			Vector3f clampedPoint = stack.vectors.get(point);
+			VectorUtil.setMax(clampedPoint, bvhAabbMin);
+			VectorUtil.setMin(clampedPoint, bvhAabbMax);
+
+			Vector3f v = stack.vectors.get();
+			v.sub(clampedPoint, bvhAabbMin);
+			VectorUtil.mul(v, v, bvhQuantization);
+
+			int out0 = (int)(v.x + 0.5f) & 0xFFFF;
+			int out1 = (int)(v.y + 0.5f) & 0xFFFF;
+			int out2 = (int)(v.z + 0.5f) & 0xFFFF;
+			
+			return ((long)out0) | (((long)out1) << 16) | (((long)out2) << 32);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+	public void unQuantize(Vector3f vecOut, long vecIn) {
+		int vecIn0 = (int)((vecIn & 0x00000000FFFFL));
+		int vecIn1 = (int)((vecIn & 0x0000FFFF0000L) >>> 16);
+		int vecIn2 = (int)((vecIn & 0xFFFF00000000L) >>> 32);
+
+		vecOut.x = (float)vecIn0 / (bvhQuantization.x);
+		vecOut.y = (float)vecIn1 / (bvhQuantization.y);
+		vecOut.z = (float)vecIn2 / (bvhQuantization.z);
+
+		vecOut.add(bvhAabbMin);
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/OptimizedBvhNode.java b/src/jbullet/src/javabullet/collision/shapes/OptimizedBvhNode.java
new file mode 100644
index 0000000..b493588
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/OptimizedBvhNode.java
@@ -0,0 +1,53 @@
+/*
+ * 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.collision.shapes;
+
+import javax.vecmath.Vector3f;
+
+/**
+ * OptimizedBvhNode contains both internal and leaf node information.
+ * Total node size is 44 bytes / node. You can use the compressed version of 16 bytes.
+ * 
+ * @author jezek2
+ */
+public class OptimizedBvhNode {
+	
+	public final Vector3f aabbMinOrg = new Vector3f();
+	public final Vector3f aabbMaxOrg = new Vector3f();
+
+	public int escapeIndex;
+
+	// for child nodes
+	public int subPart;
+	public int triangleIndex;
+	
+	public void set(OptimizedBvhNode n) {
+		aabbMinOrg.set(n.aabbMinOrg);
+		aabbMaxOrg.set(n.aabbMaxOrg);
+		escapeIndex = n.escapeIndex;
+		subPart = n.subPart;
+		triangleIndex = n.triangleIndex;
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/PolyhedralConvexShape.java b/src/jbullet/src/javabullet/collision/shapes/PolyhedralConvexShape.java
new file mode 100644
index 0000000..4396be1
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/PolyhedralConvexShape.java
@@ -0,0 +1,248 @@
+/*
+ * 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.collision.shapes;
+
+import javabullet.linearmath.MatrixUtil;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Vector3f;
+
+/**
+ * PolyhedralConvexShape is an interface class for feature based (vertex/edge/face) convex shapes.
+ * 
+ * @author jezek2
+ */
+public abstract class PolyhedralConvexShape extends ConvexInternalShape {
+
+	protected final Vector3f localAabbMin = new Vector3f(1f, 1f, 1f);
+	protected final Vector3f localAabbMax = new Vector3f(-1f, -1f, -1f);
+	protected boolean isLocalAabbValid = false;
+
+//	/** optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp */
+//	public Hull optionalHull = null;
+	
+	@Override
+	public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec0) {
+		stack.vectors.push();
+		try {
+			int i;
+			Vector3f supVec = stack.vectors.get(0f, 0f, 0f);
+
+			float maxDot = -1e30f;
+
+			Vector3f vec = stack.vectors.get(vec0);
+			float lenSqr = vec.lengthSquared();
+			if (lenSqr < 0.0001f) {
+				vec.set(1f, 0f, 0f);
+			}
+			else {
+				float rlen = 1f / (float) Math.sqrt(lenSqr);
+				vec.scale(rlen);
+			}
+
+			Vector3f vtx = stack.vectors.get();
+			float newDot;
+
+			for (i = 0; i < getNumVertices(); i++) {
+				getVertex(i, vtx);
+				newDot = vec.dot(vtx);
+				if (newDot > maxDot) {
+					maxDot = newDot;
+					supVec = vtx;
+				}
+			}
+
+			return stack.vectors.returning(supVec);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
+		stack.vectors.push();
+		try {
+			int i;
+
+			Vector3f vtx = stack.vectors.get();
+			float newDot;
+
+			// JAVA NOTE: rewritten as code used W coord for temporary usage in Vector3
+			// TODO: optimize it
+			float[] wcoords = new float[numVectors];
+
+			for (i = 0; i < numVectors; i++) {
+				// TODO: used w in vector3:
+				//supportVerticesOut[i].w = -1e30f;
+				wcoords[i] = -1e30f;
+			}
+
+			for (int j = 0; j < numVectors; j++) {
+				Vector3f vec = vectors[j];
+
+				for (i = 0; i < getNumVertices(); i++) {
+					getVertex(i, vtx);
+					newDot = vec.dot(vtx);
+					//if (newDot > supportVerticesOut[j].w)
+					if (newDot > wcoords[j]) {
+						//WARNING: don't swap next lines, the w component would get overwritten!
+						supportVerticesOut[j].set(vtx);
+						//supportVerticesOut[j].w = newDot;
+						wcoords[j] = newDot;
+					}
+				}
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void calculateLocalInertia(float mass, Vector3f inertia) {
+		stack.pushCommonMath();
+		try {
+			// not yet, return box inertia
+
+			float margin = getMargin();
+
+			Transform ident = stack.transforms.get();
+			ident.setIdentity();
+			Vector3f aabbMin = stack.vectors.get(), aabbMax = stack.vectors.get();
+			getAabb(ident, aabbMin, aabbMax);
+
+			Vector3f halfExtents = stack.vectors.get();
+			halfExtents.sub(aabbMax, aabbMin);
+			halfExtents.scale(0.5f);
+
+			float lx = 2f * (halfExtents.x + margin);
+			float ly = 2f * (halfExtents.y + margin);
+			float lz = 2f * (halfExtents.z + margin);
+			float x2 = lx * lx;
+			float y2 = ly * ly;
+			float z2 = lz * lz;
+			float scaledmass = mass * 0.08333333f;
+
+			inertia.set(y2 + z2, x2 + z2, x2 + y2);
+			inertia.scale(scaledmass);
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	private void getNonvirtualAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax, float margin) {
+		stack.pushCommonMath();
+		try {
+			// lazy evaluation of local aabb
+			assert (isLocalAabbValid);
+
+			assert (localAabbMin.x <= localAabbMax.x);
+			assert (localAabbMin.y <= localAabbMax.y);
+			assert (localAabbMin.z <= localAabbMax.z);
+
+			Vector3f localHalfExtents = stack.vectors.get();
+			localHalfExtents.sub(localAabbMax, localAabbMin);
+			localHalfExtents.scale(0.5f);
+
+			Vector3f localCenter = stack.vectors.get();
+			localCenter.add(localAabbMax, localAabbMin);
+			localCenter.scale(0.5f);
+
+			Matrix3f abs_b = stack.matrices.get(trans.basis);
+			MatrixUtil.absolute(abs_b);
+
+			Vector3f center = stack.vectors.get(localCenter);
+			trans.transform(center);
+
+			Vector3f extent = stack.vectors.get();
+			Vector3f tmp = stack.vectors.get();
+
+			abs_b.getRow(0, tmp);
+			extent.x = tmp.dot(localHalfExtents);
+			abs_b.getRow(1, tmp);
+			extent.y = tmp.dot(localHalfExtents);
+			abs_b.getRow(2, tmp);
+			extent.z = tmp.dot(localHalfExtents);
+
+			extent.x += margin;
+			extent.y += margin;
+			extent.z += margin;
+
+			aabbMin.sub(center, extent);
+			aabbMax.add(center, extent);
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+	
+	@Override
+	public void getAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax) {
+		getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin());
+	}
+
+	protected final void _PolyhedralConvexShape_getAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax) {
+		getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin());
+	}
+
+	public void recalcLocalAabb() {
+		stack.vectors.push();
+		try {
+			isLocalAabbValid = true;
+
+			for (int i = 0; i < 3; i++) {
+				Vector3f vec = stack.vectors.get(0f, 0f, 0f);
+				VectorUtil.setCoord(vec, i, 1f);
+				Vector3f tmp = stack.vectors.get(localGetSupportingVertex(vec));
+				VectorUtil.setCoord(localAabbMax, i, VectorUtil.getCoord(tmp, i) + collisionMargin);
+				VectorUtil.setCoord(vec, i, -1f);
+				tmp.set(localGetSupportingVertex(vec));
+				VectorUtil.setCoord(localAabbMin, i, VectorUtil.getCoord(tmp, i) - collisionMargin);
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public abstract int getNumVertices();
+
+	public abstract int getNumEdges();
+
+	public abstract void getEdge(int i, Vector3f pa, Vector3f pb);
+
+	public abstract void getVertex(int i, Vector3f vtx);
+
+	public abstract int getNumPlanes();
+
+	public abstract void getPlane(Vector3f planeNormal, Vector3f planeSupport, int i);
+	
+//	public abstract  int getIndex(int i) const = 0 ; 
+	
+	public abstract boolean isInside(Vector3f pt, float tolerance);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/QuantizedBvhNodes.java b/src/jbullet/src/javabullet/collision/shapes/QuantizedBvhNodes.java
new file mode 100644
index 0000000..38466d3
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/QuantizedBvhNodes.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.
+ */
+
+package javabullet.collision.shapes;
+
+/**
+ * QuantizedBvhNodes is array of compressed AABB nodes, each of 16 bytes.
+ * Node can be used for leafnode or internal node. Leafnodes can point to 32-bit
+ * triangle index (non-negative range).<p>
+ * 
+ * <i>Implementation note:</i> the nodes are internally stored in int[] array
+ * and bit packed. The actual structure is:
+ * 
+ * <pre>
+ * unsigned short  quantizedAabbMin[3]
+ * unsigned short  quantizedAabbMax[3]
+ * signed   int    escapeIndexOrTriangleIndex
+ * </pre>
+ * 
+ * @author jezek2
+ */
+public class QuantizedBvhNodes {
+
+	private static final int STRIDE = 4; // 16 bytes
+	
+	private int[] buf;
+	private int size = 0;
+
+	public QuantizedBvhNodes() {
+		resize(16);
+	}
+	
+	public int add() {
+		while (size+1 >= capacity()) {
+			resize(capacity()*2);
+		}
+		return size++;
+	}
+	
+	public int size() {
+		return size;
+	}
+	
+	public int capacity() {
+		return buf.length / STRIDE;
+	}
+	
+	public void clear() {
+		size = 0;
+	}
+	
+	public void resize(int num) {
+		int[] oldBuf = buf;
+		
+		buf = new int[num*STRIDE];
+		if (oldBuf != null) {
+			System.arraycopy(oldBuf, 0, buf, 0, Math.min(oldBuf.length, buf.length));
+		}
+	}
+	
+	public static int getNodeSize() {
+		return STRIDE*4;
+	}
+	
+	public void set(int destId, QuantizedBvhNodes srcNodes, int srcId) {
+		assert (STRIDE == 4);
+
+		// save field access:
+		int[] buf = this.buf;
+		int[] srcBuf = srcNodes.buf;
+		
+		buf[destId*STRIDE+0] = srcBuf[srcId*STRIDE+0];
+		buf[destId*STRIDE+1] = srcBuf[srcId*STRIDE+1];
+		buf[destId*STRIDE+2] = srcBuf[srcId*STRIDE+2];
+		buf[destId*STRIDE+3] = srcBuf[srcId*STRIDE+3];
+	}
+	
+	public void swap(int id1, int id2) {
+		assert (STRIDE == 4);
+		
+		// save field access:
+		int[] buf = this.buf;
+		
+		int temp0 = buf[id1*STRIDE+0];
+		int temp1 = buf[id1*STRIDE+1];
+		int temp2 = buf[id1*STRIDE+2];
+		int temp3 = buf[id1*STRIDE+3];
+		
+		buf[id1*STRIDE+0] = buf[id2*STRIDE+0];
+		buf[id1*STRIDE+1] = buf[id2*STRIDE+1];
+		buf[id1*STRIDE+2] = buf[id2*STRIDE+2];
+		buf[id1*STRIDE+3] = buf[id2*STRIDE+3];
+		
+		buf[id2*STRIDE+0] = temp0;
+		buf[id2*STRIDE+1] = temp1;
+		buf[id2*STRIDE+2] = temp2;
+		buf[id2*STRIDE+3] = temp3;
+	}
+	
+	public int getQuantizedAabbMin(int nodeId, int index) {
+		switch (index) {
+			default:
+			case 0: return (buf[nodeId*STRIDE+0]) & 0xFFFF;
+			case 1: return (buf[nodeId*STRIDE+0] >>> 16) & 0xFFFF;
+			case 2: return (buf[nodeId*STRIDE+1]) & 0xFFFF;
+		}
+	}
+
+	public long getQuantizedAabbMin(int nodeId) {
+		return (buf[nodeId*STRIDE+0] & 0xFFFFFFFFL) | ((buf[nodeId*STRIDE+1] & 0xFFFFL) << 32);
+	}
+
+	public void setQuantizedAabbMin(int nodeId, long value) {
+		buf[nodeId*STRIDE+0] = (int)value;
+		setQuantizedAabbMin(nodeId, 2, (short)((value & 0xFFFF00000000L) >>> 32));
+	}
+
+	public void setQuantizedAabbMax(int nodeId, long value) {
+		setQuantizedAabbMax(nodeId, 0, (short)value);
+		buf[nodeId*STRIDE+2] = (int)(value >>> 16);
+	}
+
+	public void setQuantizedAabbMin(int nodeId, int index, int value) {
+		switch (index) {
+			case 0: buf[nodeId*STRIDE+0] = (buf[nodeId*STRIDE+0] & 0xFFFF0000) | (value & 0xFFFF); break;
+			case 1: buf[nodeId*STRIDE+0] = (buf[nodeId*STRIDE+0] & 0x0000FFFF) | ((value & 0xFFFF) << 16); break;
+			case 2: buf[nodeId*STRIDE+1] = (buf[nodeId*STRIDE+1] & 0xFFFF0000) | (value & 0xFFFF); break;
+		}
+	}
+
+	public int getQuantizedAabbMax(int nodeId, int index) {
+		switch (index) {
+			default:
+			case 0: return (buf[nodeId*STRIDE+1] >>> 16) & 0xFFFF;
+			case 1: return (buf[nodeId*STRIDE+2]) & 0xFFFF;
+			case 2: return (buf[nodeId*STRIDE+2] >>> 16) & 0xFFFF;
+		}
+	}
+
+	public long getQuantizedAabbMax(int nodeId) {
+		return ((buf[nodeId*STRIDE+1] & 0xFFFF0000L) >>> 16) | ((buf[nodeId*STRIDE+2] & 0xFFFFFFFFL) << 16);
+	}
+
+	public void setQuantizedAabbMax(int nodeId, int index, int value) {
+		switch (index) {
+			case 0: buf[nodeId*STRIDE+1] = (buf[nodeId*STRIDE+1] & 0x0000FFFF) | ((value & 0xFFFF) << 16); break;
+			case 1: buf[nodeId*STRIDE+2] = (buf[nodeId*STRIDE+2] & 0xFFFF0000) | (value & 0xFFFF); break;
+			case 2: buf[nodeId*STRIDE+2] = (buf[nodeId*STRIDE+2] & 0x0000FFFF) | ((value & 0xFFFF) << 16); break;
+		}
+	}
+	
+	public int getEscapeIndexOrTriangleIndex(int nodeId) {
+		return buf[nodeId*STRIDE+3];
+	}
+	
+	public void setEscapeIndexOrTriangleIndex(int nodeId, int value) {
+		buf[nodeId*STRIDE+3] = value;
+	}
+	
+	public boolean isLeafNode(int nodeId) {
+		// skipindex is negative (internal node), triangleindex >=0 (leafnode)
+		return (getEscapeIndexOrTriangleIndex(nodeId) >= 0);
+	}
+
+	public int getEscapeIndex(int nodeId) {
+		assert (!isLeafNode(nodeId));
+		return -getEscapeIndexOrTriangleIndex(nodeId);
+	}
+
+	public int getTriangleIndex(int nodeId) {
+		assert (isLeafNode(nodeId));
+		// Get only the lower bits where the triangle index is stored
+		return (getEscapeIndexOrTriangleIndex(nodeId) & ~((~0) << (31 - OptimizedBvh.MAX_NUM_PARTS_IN_BITS)));
+	}
+
+	public int getPartId(int nodeId) {
+		assert (isLeafNode(nodeId));
+		// Get only the highest bits where the part index is stored
+		return (getEscapeIndexOrTriangleIndex(nodeId) >>> (31 - OptimizedBvh.MAX_NUM_PARTS_IN_BITS));
+	}
+	
+	public static int getCoord(long vec, int index) {
+		switch (index) {
+			default:
+			case 0: return (int)((vec & 0x00000000FFFFL)) & 0xFFFF;
+			case 1: return (int)((vec & 0x0000FFFF0000L) >>> 16) & 0xFFFF;
+			case 2: return (int)((vec & 0xFFFF00000000L) >>> 32) & 0xFFFF;
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/ScalarType.java b/src/jbullet/src/javabullet/collision/shapes/ScalarType.java
new file mode 100644
index 0000000..98bd078
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/ScalarType.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.collision.shapes;
+
+/**
+ *
+ * @author jezek2
+ */
+public enum ScalarType {
+	PHY_FLOAT,
+	PHY_DOUBLE,
+	PHY_INTEGER,
+	PHY_SHORT,
+	PHY_FIXEDPOINT88
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/SphereShape.java b/src/jbullet/src/javabullet/collision/shapes/SphereShape.java
new file mode 100644
index 0000000..2a5c780
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/SphereShape.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.collision.shapes;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ * SphereShape implements an implicit (getSupportingVertex) Sphere.
+ * 
+ * @author jezek2
+ */
+public class SphereShape extends ConvexInternalShape {
+	
+	public SphereShape(float radius) {
+		implicitShapeDimensions.x = radius;
+	}
+
+	@Override
+	public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) {
+		return BulletGlobals.ZERO_VECTOR3;
+	}
+
+	@Override
+	public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
+		for (int i = 0; i < numVectors; i++) {
+			supportVerticesOut[i].set(0f, 0f, 0f);
+		}
+	}
+
+	@Override
+	public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) {
+		stack.vectors.push();
+		try {
+			Vector3f center = t.origin;
+			Vector3f extent = stack.vectors.get(getMargin(), getMargin(), getMargin());
+			aabbMin.sub(center, extent);
+			aabbMax.add(center, extent);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public BroadphaseNativeType getShapeType() {
+		return BroadphaseNativeType.SPHERE_SHAPE_PROXYTYPE;
+	}
+
+	@Override
+	public void calculateLocalInertia(float mass, Vector3f inertia) {
+		float elem = 0.4f * mass * getMargin() * getMargin();
+		inertia.set(elem, elem, elem);
+	}
+
+	@Override
+	public String getName() {
+		return "SPHERE";
+	}
+	
+	public float getRadius() {
+		return implicitShapeDimensions.x * localScaling.x;
+	}
+
+	@Override
+	public void setMargin(float margin) {
+		super.setMargin(margin);
+	}
+
+	@Override
+	public float getMargin() {
+		// to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case
+		// this means, non-uniform scaling is not supported anymore
+		return getRadius();
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/StaticPlaneShape.java b/src/jbullet/src/javabullet/collision/shapes/StaticPlaneShape.java
new file mode 100644
index 0000000..173be00
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/StaticPlaneShape.java
@@ -0,0 +1,161 @@
+/*
+ * 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.collision.shapes;
+
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.TransformUtil;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ * StaticPlaneShape simulates an 'infinite' plane by dynamically reporting triangles approximated by intersection of the plane with the AABB.
+ * Assumed is that the other objects is not also infinite, so a reasonable sized AABB.
+ * 
+ * @author jezek2
+ */
+public class StaticPlaneShape extends ConcaveShape {
+
+	protected final Vector3f localAabbMin = new Vector3f();
+	protected final Vector3f localAabbMax = new Vector3f();
+	
+	protected final Vector3f planeNormal = new Vector3f();
+	protected float planeConstant;
+	protected final Vector3f localScaling = new Vector3f(0f, 0f, 0f);
+
+	public StaticPlaneShape(Vector3f planeNormal, float planeConstant) {
+		this.planeNormal.set(planeNormal);
+		this.planeConstant = planeConstant;
+	}
+
+	public Vector3f getPlaneNormal() {
+		return planeNormal;
+	}
+
+	public float getPlaneConstant() {
+		return planeConstant;
+	}
+	
+	@Override
+	public void processAllTriangles(TriangleCallback callback, Vector3f aabbMin, Vector3f aabbMax) {
+		stack.vectors.push();
+		try {
+			Vector3f tmp = stack.vectors.get();
+			Vector3f tmp1 = stack.vectors.get();
+			Vector3f tmp2 = stack.vectors.get();
+
+			Vector3f halfExtents = stack.vectors.get();
+			halfExtents.sub(aabbMax, aabbMin);
+			halfExtents.scale(0.5f);
+
+			float radius = halfExtents.length();
+			Vector3f center = stack.vectors.get();
+			center.add(aabbMax, aabbMin);
+			center.scale(0.5f);
+
+			// this is where the triangles are generated, given AABB and plane equation (normal/constant)
+
+			Vector3f tangentDir0 = stack.vectors.get(), tangentDir1 = stack.vectors.get();
+
+			// tangentDir0/tangentDir1 can be precalculated
+			TransformUtil.planeSpace1(planeNormal, tangentDir0, tangentDir1);
+
+			Vector3f supVertex0 = stack.vectors.get(), supVertex1 = stack.vectors.get();
+
+			Vector3f projectedCenter = stack.vectors.get();
+			tmp.scale(planeNormal.dot(center) - planeConstant, planeNormal);
+			projectedCenter.sub(center, tmp);
+
+			Vector3f[] triangle = new Vector3f[] { stack.vectors.get(), stack.vectors.get(), stack.vectors.get() };
+
+			tmp1.scale(radius, tangentDir0);
+			tmp2.scale(radius, tangentDir1);
+			VectorUtil.add(triangle[0], projectedCenter, tmp1, tmp2);
+
+			tmp1.scale(radius, tangentDir0);
+			tmp2.scale(radius, tangentDir1);
+			tmp.sub(tmp1, tmp2);
+			VectorUtil.add(triangle[1], projectedCenter, tmp);
+
+			tmp1.scale(radius, tangentDir0);
+			tmp2.scale(radius, tangentDir1);
+			tmp.sub(tmp1, tmp2);
+			triangle[2].sub(projectedCenter, tmp);
+
+			callback.processTriangle(triangle, 0, 0);
+
+			tmp1.scale(radius, tangentDir0);
+			tmp2.scale(radius, tangentDir1);
+			tmp.sub(tmp1, tmp2);
+			triangle[0].sub(projectedCenter, tmp);
+
+			tmp1.scale(radius, tangentDir0);
+			tmp2.scale(radius, tangentDir1);
+			tmp.add(tmp1, tmp2);
+			triangle[1].sub(projectedCenter, tmp);
+
+			tmp1.scale(radius, tangentDir0);
+			tmp2.scale(radius, tangentDir1);
+			VectorUtil.add(triangle[2], projectedCenter, tmp1, tmp2);
+
+			callback.processTriangle(triangle, 0, 1);
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) {
+		aabbMin.set(-1e30f, -1e30f, -1e30f);
+		aabbMax.set(1e30f, 1e30f, 1e30f);
+	}
+
+	@Override
+	public BroadphaseNativeType getShapeType() {
+		return BroadphaseNativeType.STATIC_PLANE_PROXYTYPE;
+	}
+
+	@Override
+	public void setLocalScaling(Vector3f scaling) {
+		localScaling.set(scaling);
+	}
+
+	@Override
+	public Vector3f getLocalScaling() {
+		return localScaling;
+	}
+
+	@Override
+	public void calculateLocalInertia(float mass, Vector3f inertia) {
+		//moving concave objects not supported
+		inertia.set(0f, 0f, 0f);
+	}
+
+	@Override
+	public String getName() {
+		return "STATICPLANE";
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/StridingMeshInterface.java b/src/jbullet/src/javabullet/collision/shapes/StridingMeshInterface.java
new file mode 100644
index 0000000..70ee487
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/StridingMeshInterface.java
@@ -0,0 +1,200 @@
+/*
+ * 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.collision.shapes;
+
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ * StridingMeshInterface is the interface class for high performance access to triangle meshes.
+ * It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory.
+ * 
+ * @author jezek2
+ */
+public abstract class StridingMeshInterface {
+
+	protected final Vector3f scaling = new Vector3f(1f, 1f, 1f);
+	
+	private VertexData data = new VertexData();
+	
+	public void internalProcessAllTriangles(InternalTriangleIndexCallback callback, Vector3f aabbMin, Vector3f aabbMax) {
+		int numtotalphysicsverts = 0;
+		int part, graphicssubparts = getNumSubParts();
+		int gfxindex;
+		Vector3f[] triangle/*[3]*/ = new Vector3f[]{new Vector3f(), new Vector3f(), new Vector3f()};
+		int graphicsbase;
+
+		Vector3f meshScaling = new Vector3f(getScaling());
+
+		// if the number of parts is big, the performance might drop due to the innerloop switch on indextype
+		for (part = 0; part < graphicssubparts; part++) {
+			getLockedReadOnlyVertexIndexBase(data, part);
+			numtotalphysicsverts += data.numfaces * 3; // upper bound
+
+			switch (data.indicestype) {
+				case PHY_INTEGER: {
+					for (gfxindex = 0; gfxindex < data.numfaces; gfxindex++) {
+						//int* tri_indices= (int*)(indexbase+gfxindex*indexstride);
+						int tri_indices = gfxindex * data.indexstride;
+
+						//graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride);
+						graphicsbase = data.indexbase.getInt(tri_indices + 0) * data.stride;
+
+						//triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+						triangle[0].set(
+								data.vertexbase.getFloat(graphicsbase + 0) * meshScaling.x,
+								data.vertexbase.getFloat(graphicsbase + 4) * meshScaling.y,
+								data.vertexbase.getFloat(graphicsbase + 8) * meshScaling.z);
+
+						//graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride);
+						graphicsbase = data.indexbase.getInt(tri_indices + 4) * data.stride;
+
+						//triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+						triangle[1].set(
+								data.vertexbase.getFloat(graphicsbase + 0) * meshScaling.x,
+								data.vertexbase.getFloat(graphicsbase + 4) * meshScaling.y,
+								data.vertexbase.getFloat(graphicsbase + 8) * meshScaling.z);
+
+						//graphicsbase = (btScalar*)(vertexbase+tri_indices[2]*stride);
+						graphicsbase = data.indexbase.getInt(tri_indices + 8) * data.stride;
+
+						//triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+						triangle[2].set(
+								data.vertexbase.getFloat(graphicsbase + 0) * meshScaling.x,
+								data.vertexbase.getFloat(graphicsbase + 4) * meshScaling.y,
+								data.vertexbase.getFloat(graphicsbase + 8) * meshScaling.z);
+
+						callback.internalProcessTriangleIndex(triangle, part, gfxindex);
+					}
+					break;
+				}
+				case PHY_SHORT: {
+					for (gfxindex = 0; gfxindex < data.numfaces; gfxindex++) {
+						//short int* tri_indices= (short int*)(indexbase+gfxindex*indexstride);
+						int tri_indices = gfxindex * data.indexstride;
+
+						//graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride);
+						graphicsbase = (data.indexbase.getShort(tri_indices + 0) & 0xFFFF) * data.stride;
+
+						//triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+						triangle[0].set(
+								data.vertexbase.getFloat(graphicsbase + 0) * meshScaling.x,
+								data.vertexbase.getFloat(graphicsbase + 4) * meshScaling.y,
+								data.vertexbase.getFloat(graphicsbase + 8) * meshScaling.z);
+
+						//graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride);
+						graphicsbase = (data.indexbase.getShort(tri_indices + 2) & 0xFFFF) * data.stride;
+
+						//triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+						triangle[1].set(
+								data.vertexbase.getFloat(graphicsbase + 0) * meshScaling.x,
+								data.vertexbase.getFloat(graphicsbase + 4) * meshScaling.y,
+								data.vertexbase.getFloat(graphicsbase + 8) * meshScaling.z);
+
+						//graphicsbase = (btScalar*)(vertexbase+tri_indices[2]*stride);
+						graphicsbase = (data.indexbase.getShort(tri_indices + 4) & 0xFFFF) * data.stride;
+
+						//triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ());
+						triangle[2].set(
+								data.vertexbase.getFloat(graphicsbase + 0) * meshScaling.x,
+								data.vertexbase.getFloat(graphicsbase + 4) * meshScaling.y,
+								data.vertexbase.getFloat(graphicsbase + 8) * meshScaling.z);
+
+						callback.internalProcessTriangleIndex(triangle, part, gfxindex);
+					}
+					break;
+				}
+				default:
+					assert ((data.indicestype == ScalarType.PHY_INTEGER) || (data.indicestype == ScalarType.PHY_SHORT));
+			}
+
+			unLockReadOnlyVertexBase(part);
+		}
+		
+		data.unref();
+	}
+
+	private static class AabbCalculationCallback implements InternalTriangleIndexCallback {
+		public final Vector3f aabbMin = new Vector3f(1e30f, 1e30f, 1e30f);
+		public final Vector3f aabbMax = new Vector3f(-1e30f, -1e30f, -1e30f);
+
+		public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) {
+			VectorUtil.setMin(aabbMin, triangle[0]);
+			VectorUtil.setMax(aabbMax, triangle[0]);
+			VectorUtil.setMin(aabbMin, triangle[1]);
+			VectorUtil.setMax(aabbMax, triangle[1]);
+			VectorUtil.setMin(aabbMin, triangle[2]);
+			VectorUtil.setMax(aabbMax, triangle[2]);
+		}
+	}
+	
+	public void calculateAabbBruteForce(Vector3f aabbMin, Vector3f aabbMax) {
+		// first calculate the total aabb for all triangles
+		AabbCalculationCallback aabbCallback = new AabbCalculationCallback();
+		aabbMin.set(-1e30f, -1e30f, -1e30f);
+		aabbMax.set(1e30f, 1e30f, 1e30f);
+		internalProcessAllTriangles(aabbCallback, aabbMin, aabbMax);
+
+		aabbMin.set(aabbCallback.aabbMin);
+		aabbMax.set(aabbCallback.aabbMax);
+	}
+	
+	/**
+	 * Get read and write access to a subpart of a triangle mesh.
+	 * This subpart has a continuous array of vertices and indices.
+	 * In this way the mesh can be handled as chunks of memory with striding
+	 * very similar to OpenGL vertexarray support.
+	 * Make a call to unLockVertexBase when the read and write access is finished.
+	 */
+	public abstract void getLockedVertexIndexBase(VertexData data, int subpart/*=0*/);
+
+	public abstract void getLockedReadOnlyVertexIndexBase(VertexData data, int subpart/*=0*/);
+
+	/**
+	 * unLockVertexBase finishes the access to a subpart of the triangle mesh.
+	 * Make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished.
+	 */
+	public abstract void unLockVertexBase(int subpart);
+
+	public abstract void unLockReadOnlyVertexBase(int subpart);
+
+	/**
+	 * getNumSubParts returns the number of seperate subparts.
+	 * Each subpart has a continuous array of vertices and indices.
+	 */
+	public abstract int getNumSubParts();
+
+	public abstract void preallocateVertices(int numverts);
+	public abstract void preallocateIndices(int numindices);
+
+	public Vector3f getScaling() {
+		return scaling;
+	}
+	
+	public void setScaling(Vector3f scaling)
+	{
+		this.scaling.set(scaling);
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/TraversalMode.java b/src/jbullet/src/javabullet/collision/shapes/TraversalMode.java
new file mode 100644
index 0000000..7558205
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/TraversalMode.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.collision.shapes;
+
+/**
+ *
+ * @author jezek2
+ */
+public enum TraversalMode {
+	TRAVERSAL_STACKLESS,
+	TRAVERSAL_STACKLESS_CACHE_FRIENDLY,
+	TRAVERSAL_RECURSIVE
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/TriangleCallback.java b/src/jbullet/src/javabullet/collision/shapes/TriangleCallback.java
new file mode 100644
index 0000000..39ab89f
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/TriangleCallback.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.collision.shapes;
+
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public interface TriangleCallback {
+
+	public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex);
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/TriangleIndexVertexArray.java b/src/jbullet/src/javabullet/collision/shapes/TriangleIndexVertexArray.java
new file mode 100644
index 0000000..7f4dd16
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/TriangleIndexVertexArray.java
@@ -0,0 +1,140 @@
+/*
+ * 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.collision.shapes;
+
+import java.nio.ByteBuffer;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ *
+ * @author jezek2
+ */
+public class TriangleIndexVertexArray extends StridingMeshInterface {
+
+	private List<IndexedMesh> indexedMeshes = new ArrayList<IndexedMesh>();
+
+	public TriangleIndexVertexArray() {
+	}
+
+	/**
+	 * Just to be backwards compatible.
+	 */
+	public TriangleIndexVertexArray(int numTriangles, ByteBuffer triangleIndexBase, int triangleIndexStride, int numVertices, ByteBuffer vertexBase, int vertexStride) {
+		IndexedMesh mesh = new IndexedMesh();
+
+		mesh.numTriangles = numTriangles;
+		mesh.triangleIndexBase = triangleIndexBase;
+		mesh.triangleIndexStride = triangleIndexStride;
+		mesh.numVertices = numVertices;
+		mesh.vertexBase = vertexBase;
+		mesh.vertexStride = vertexStride;
+
+		addIndexedMesh(mesh);
+	}
+
+	public void addIndexedMesh(IndexedMesh mesh) {
+		addIndexedMesh(mesh, ScalarType.PHY_INTEGER);
+	}
+
+	public void addIndexedMesh(IndexedMesh mesh, ScalarType indexType) {
+		indexedMeshes.add(mesh);
+		indexedMeshes.get(indexedMeshes.size() - 1).indexType = indexType;
+	}
+	
+	@Override
+	public void getLockedVertexIndexBase(VertexData data, int subpart) {
+		assert (subpart < getNumSubParts());
+
+		IndexedMesh mesh = indexedMeshes.get(subpart);
+
+		data.numverts = mesh.numVertices;
+		data.vertexbase = mesh.vertexBase;
+		//#ifdef BT_USE_DOUBLE_PRECISION
+		//type = PHY_DOUBLE;
+		//#else
+		data.type = ScalarType.PHY_FLOAT;
+		//#endif
+		data.stride = mesh.vertexStride;
+
+		data.numfaces = mesh.numTriangles;
+
+		data.indexbase = mesh.triangleIndexBase;
+		data.indexstride = mesh.triangleIndexStride;
+		data.indicestype = mesh.indexType;
+	}
+
+	@Override
+	public void getLockedReadOnlyVertexIndexBase(VertexData data, int subpart) {
+		IndexedMesh mesh = indexedMeshes.get(subpart);
+
+		data.numverts = mesh.numVertices;
+		data.vertexbase = mesh.vertexBase;
+		//#ifdef BT_USE_DOUBLE_PRECISION
+		//type = PHY_DOUBLE;
+		//#else
+		data.type = ScalarType.PHY_FLOAT;
+		//#endif
+		data.stride = mesh.vertexStride;
+
+		data.numfaces = mesh.numTriangles;
+		data.indexbase = mesh.triangleIndexBase;
+		data.indexstride = mesh.triangleIndexStride;
+		data.indicestype = mesh.indexType;
+	}
+
+	/**
+	 * unLockVertexBase finishes the access to a subpart of the triangle mesh.
+	 * Make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished.
+	 */
+	@Override
+	public void unLockVertexBase(int subpart) {
+	}
+
+	@Override
+	public void unLockReadOnlyVertexBase(int subpart) {
+	}
+
+	/**
+	 * getNumSubParts returns the number of seperate subparts.
+	 * Each subpart has a continuous array of vertices and indices.
+	 */
+	@Override
+	public int getNumSubParts() {
+		return indexedMeshes.size();
+	}
+
+	public List<IndexedMesh> getIndexedMeshArray() {
+		return indexedMeshes;
+	}
+	
+	@Override
+	public void preallocateVertices(int numverts) {
+	}
+
+	@Override
+	public void preallocateIndices(int numindices) {
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/TriangleMeshShape.java b/src/jbullet/src/javabullet/collision/shapes/TriangleMeshShape.java
new file mode 100644
index 0000000..3dddf2a
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/TriangleMeshShape.java
@@ -0,0 +1,235 @@
+/*
+ * 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.collision.shapes;
+
+import javabullet.linearmath.AabbUtil2;
+import javabullet.linearmath.MatrixUtil;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public abstract class TriangleMeshShape extends ConcaveShape {
+
+	protected final Vector3f localAabbMin = new Vector3f();
+	protected final Vector3f localAabbMax = new Vector3f();
+	protected StridingMeshInterface meshInterface;
+
+	/**
+	 * TriangleMeshShape constructor has been disabled/protected, so that users will not mistakenly use this class.
+	 * Don't use btTriangleMeshShape but use btBvhTriangleMeshShape instead!
+	 */
+	protected TriangleMeshShape(StridingMeshInterface meshInterface) {
+		this.meshInterface = meshInterface;
+		
+		// JAVA NOTE: moved to BvhTriangleMeshShape
+		//recalcLocalAabb();
+	}
+	
+	public Vector3f localGetSupportingVertex(Vector3f vec) {
+		stack.pushCommonMath();
+		try {
+			Vector3f tmp = stack.vectors.get();
+
+			Vector3f supportVertex = stack.vectors.get();
+
+			Transform ident = stack.transforms.get();
+			ident.setIdentity();
+
+			SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident);
+
+			Vector3f aabbMax = stack.vectors.get(1e30f, 1e30f, 1e30f);
+			tmp.negate(aabbMax);
+
+			processAllTriangles(supportCallback, tmp, aabbMax);
+
+			supportVertex.set(supportCallback.getSupportVertexLocal());
+
+			return stack.vectors.returning(supportVertex);
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) {
+		assert (false);
+		return localGetSupportingVertex(vec);
+	}
+
+	public void recalcLocalAabb() {
+		stack.vectors.push();
+		try {
+			for (int i = 0; i < 3; i++) {
+				Vector3f vec = stack.vectors.get(0f, 0f, 0f);
+				VectorUtil.setCoord(vec, i, 1f);
+				Vector3f tmp = stack.vectors.get(localGetSupportingVertex(vec));
+				VectorUtil.setCoord(localAabbMax, i, VectorUtil.getCoord(tmp, i) + collisionMargin);
+				VectorUtil.setCoord(vec, i, -1f);
+				tmp.set(localGetSupportingVertex(vec));
+				VectorUtil.setCoord(localAabbMin, i, VectorUtil.getCoord(tmp, i) - collisionMargin);
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void getAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax) {
+		stack.pushCommonMath();
+		try {
+			Vector3f tmp = stack.vectors.get();
+
+			Vector3f localHalfExtents = stack.vectors.get();
+			localHalfExtents.sub(localAabbMax, localAabbMin);
+			localHalfExtents.scale(0.5f);
+
+			Vector3f localCenter = stack.vectors.get();
+			localCenter.add(localAabbMax, localAabbMin);
+			localCenter.scale(0.5f);
+
+			Matrix3f abs_b = stack.matrices.get(trans.basis);
+			MatrixUtil.absolute(abs_b);
+
+			Vector3f center = stack.vectors.get(localCenter);
+			trans.transform(center);
+
+			Vector3f extent = stack.vectors.get();
+			abs_b.getRow(0, tmp);
+			extent.x = tmp.dot(localHalfExtents);
+			abs_b.getRow(1, tmp);
+			extent.y = tmp.dot(localHalfExtents);
+			abs_b.getRow(2, tmp);
+			extent.z = tmp.dot(localHalfExtents);
+
+			extent.add(stack.vectors.get(getMargin(), getMargin(), getMargin()));
+
+			aabbMin.sub(center, extent);
+			aabbMax.add(center, extent);
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	@Override
+	public void processAllTriangles(TriangleCallback callback, Vector3f aabbMin, Vector3f aabbMax) {
+		FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax);
+
+		meshInterface.internalProcessAllTriangles(filterCallback, aabbMin, aabbMax);
+	}
+
+	@Override
+	public void calculateLocalInertia(float mass, Vector3f inertia) {
+		// moving concave objects not supported
+		assert (false);
+		inertia.set(0f, 0f, 0f);
+	}
+
+
+	@Override
+	public void setLocalScaling(Vector3f scaling) {
+		meshInterface.setScaling(scaling);
+		recalcLocalAabb();
+	}
+
+	@Override
+	public Vector3f getLocalScaling() {
+		return meshInterface.getScaling();
+	}
+	
+	public StridingMeshInterface getMeshInterface() {
+		return meshInterface;
+	}
+
+	@Override
+	public String getName() {
+		return "TRIANGLEMESH";
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	private class SupportVertexCallback implements TriangleCallback {
+		private final Vector3f supportVertexLocal = new Vector3f(0f, 0f, 0f);
+		public final Transform worldTrans = new Transform();
+		public float maxDot = -1e30f;
+		public final Vector3f supportVecLocal = new Vector3f();
+
+		public SupportVertexCallback(Vector3f supportVecWorld,Transform trans) {
+			this.worldTrans.set(trans);
+			MatrixUtil.transposeTransform(supportVecLocal, supportVecWorld, worldTrans.basis);
+		}
+		
+		public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) {
+			for (int i = 0; i < 3; i++) {
+				float dot = supportVecLocal.dot(triangle[i]);
+				if (dot > maxDot) {
+					maxDot = dot;
+					supportVertexLocal.set(triangle[i]);
+				}
+			}
+		}
+
+		public Vector3f getSupportVertexWorldSpace() {
+			stack.vectors.push();
+			try {
+				Vector3f tmp = stack.vectors.get(supportVertexLocal);
+				worldTrans.transform(tmp);
+				return stack.vectors.returning(tmp);
+			}
+			finally {
+				stack.vectors.pop();
+			}
+		}
+
+		public Vector3f getSupportVertexLocal() {
+			return supportVertexLocal;
+		}
+	}
+	
+	private static class FilteredCallback implements InternalTriangleIndexCallback {
+		public TriangleCallback callback;
+		public final Vector3f aabbMin = new Vector3f();
+		public final Vector3f aabbMax = new Vector3f();
+
+		public FilteredCallback(TriangleCallback callback, Vector3f aabbMin, Vector3f aabbMax) {
+			this.callback = callback;
+			this.aabbMin.set(aabbMin);
+			this.aabbMax.set(aabbMax);
+		}
+
+		public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) {
+			if (AabbUtil2.testTriangleAgainstAabb2(triangle, aabbMin, aabbMax)) {
+				// check aabb in triangle-space, before doing this
+				callback.processTriangle(triangle, partId, triangleIndex);
+			}
+		}
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/TriangleShape.java b/src/jbullet/src/javabullet/collision/shapes/TriangleShape.java
new file mode 100644
index 0000000..d5030ad
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/TriangleShape.java
@@ -0,0 +1,216 @@
+/*
+ * 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.collision.shapes;
+
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class TriangleShape extends PolyhedralConvexShape {
+	
+	public final Vector3f[] vertices1/*[3]*/ = new Vector3f[] { new Vector3f(), new Vector3f(), new Vector3f() };
+
+	// JAVA NOTE: added
+	public TriangleShape() {
+	}
+	
+	public TriangleShape(Vector3f p0, Vector3f p1, Vector3f p2) {
+		vertices1[0].set(p0);
+		vertices1[1].set(p1);
+		vertices1[2].set(p2);
+	}
+	
+	// JAVA NOTE: added
+	public void init(Vector3f p0, Vector3f p1, Vector3f p2) {
+		vertices1[0].set(p0);
+		vertices1[1].set(p1);
+		vertices1[2].set(p2);
+	}
+
+	@Override
+	public int getNumVertices() {
+		return 3;
+	}
+
+	public Vector3f getVertexPtr(int index) {
+		return vertices1[index];
+	}
+	
+	@Override
+	public void getVertex(int index, Vector3f vert) {
+		vert.set(vertices1[index]);
+	}
+
+	@Override
+	public BroadphaseNativeType getShapeType() {
+		return BroadphaseNativeType.TRIANGLE_SHAPE_PROXYTYPE;
+	}
+
+	@Override
+	public int getNumEdges() {
+		return 3;
+	}
+
+	@Override
+	public void getEdge(int i, Vector3f pa, Vector3f pb) {
+		getVertex(i, pa);
+		getVertex((i + 1) % 3, pb);
+	}
+
+	@Override
+	public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) {
+//		btAssert(0);
+		getAabbSlow(t, aabbMin, aabbMax);
+	}
+
+	@Override
+	public Vector3f localGetSupportingVertexWithoutMargin(Vector3f dir) {
+		stack.vectors.push();
+		try {
+			Vector3f dots = stack.vectors.get(dir.dot(vertices1[0]), dir.dot(vertices1[1]), dir.dot(vertices1[2]));
+			return vertices1[VectorUtil.maxAxis(dots)];
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
+		stack.vectors.push();
+		try {
+			Vector3f dots = stack.vectors.get();
+
+			for (int i = 0; i < numVectors; i++) {
+				Vector3f dir = vectors[i];
+				dots.set(dir.dot(vertices1[0]), dir.dot(vertices1[1]), dir.dot(vertices1[2]));
+				supportVerticesOut[i].set(vertices1[VectorUtil.maxAxis(dots)]);
+			}
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public void getPlane(Vector3f planeNormal, Vector3f planeSupport, int i) {
+		getPlaneEquation(i,planeNormal,planeSupport);
+	}
+
+	@Override
+	public int getNumPlanes() {
+		return 1;
+	}
+
+	public void calcNormal(Vector3f normal) {
+		stack.vectors.push();
+		try {
+			Vector3f tmp1 = stack.vectors.get();
+			Vector3f tmp2 = stack.vectors.get();
+
+			tmp1.sub(vertices1[1], vertices1[0]);
+			tmp2.sub(vertices1[2], vertices1[0]);
+
+			normal.cross(tmp1, tmp2);
+			normal.normalize();
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	public void getPlaneEquation(int i, Vector3f planeNormal, Vector3f planeSupport) {
+		calcNormal(planeNormal);
+		planeSupport.set(vertices1[0]);
+	}
+
+	@Override
+	public void calculateLocalInertia(float mass, Vector3f inertia) {
+		assert (false);
+		inertia.set(0f, 0f, 0f);
+	}
+	
+	@Override
+	public boolean isInside(Vector3f pt, float tolerance) {
+		stack.vectors.push();
+		try {
+			Vector3f normal = stack.vectors.get();
+			calcNormal(normal);
+			// distance to plane
+			float dist = pt.dot(normal);
+			float planeconst = vertices1[0].dot(normal);
+			dist -= planeconst;
+			if (dist >= -tolerance && dist <= tolerance) {
+				// inside check on edge-planes
+				int i;
+				for (i = 0; i < 3; i++) {
+					Vector3f pa = stack.vectors.get(), pb = stack.vectors.get();
+					getEdge(i, pa, pb);
+					Vector3f edge = stack.vectors.get();
+					edge.sub(pb, pa);
+					Vector3f edgeNormal = stack.vectors.get();
+					edgeNormal.cross(edge, normal);
+					edgeNormal.normalize();
+					/*float*/ dist = pt.dot(edgeNormal);
+					float edgeConst = pa.dot(edgeNormal);
+					dist -= edgeConst;
+					if (dist < -tolerance) {
+						return false;
+					}
+				}
+
+				return true;
+			}
+
+			return false;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+
+	@Override
+	public String getName() {
+		return "Triangle";
+	}
+
+	@Override
+	public int getNumPreferredPenetrationDirections() {
+		return 2;
+	}
+
+	@Override
+	public void getPreferredPenetrationDirection(int index, Vector3f penetrationVector) {
+		calcNormal(penetrationVector);
+		if (index != 0) {
+			penetrationVector.scale(-1f);
+		}
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/VertexData.java b/src/jbullet/src/javabullet/collision/shapes/VertexData.java
new file mode 100644
index 0000000..b88350e
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/VertexData.java
@@ -0,0 +1,52 @@
+/*
+ * 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.collision.shapes;
+
+import java.nio.ByteBuffer;
+
+/**
+ * Represents information for accessing vertex data.
+ * 
+ * @author jezek2
+ */
+public class VertexData {
+	
+	public ByteBuffer vertexbase;
+	public int numverts;
+	public ScalarType type;
+	public int stride;
+	public ByteBuffer indexbase;
+	public int indexstride;
+	public int numfaces;
+	public ScalarType indicestype;
+
+	/**
+	 * Unreferences data buffers to avoid memory leaks.
+	 */
+	public void unref() {
+		vertexbase = null;
+		indexbase = null;
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/collision/shapes/package-info.java b/src/jbullet/src/javabullet/collision/shapes/package-info.java
new file mode 100644
index 0000000..9705359
--- /dev/null
+++ b/src/jbullet/src/javabullet/collision/shapes/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.
+ */
+
+/**
+ * Collision shapes.
+ */
+package javabullet.collision.shapes;
+
diff --git a/src/jbullet/src/javabullet/demos/genericjoint/GenericJointDemo.java b/src/jbullet/src/javabullet/demos/genericjoint/GenericJointDemo.java
new file mode 100644
index 0000000..bf786c1
--- /dev/null
+++ b/src/jbullet/src/javabullet/demos/genericjoint/GenericJointDemo.java
@@ -0,0 +1,123 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Ragdoll Demo
+ * 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.
+ * 
+ * Originally Written by: Marten Svanfeldt
+ * ReWritten by: Francisco Le�n
+ */
+
+package javabullet.demos.genericjoint;
+
+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.DefaultCollisionConfiguration;
+import javabullet.collision.shapes.BoxShape;
+import javabullet.collision.shapes.CollisionShape;
+import javabullet.demos.opengl.DemoApplication;
+import javabullet.demos.opengl.JOGL;
+import javabullet.dynamics.DiscreteDynamicsWorld;
+import javabullet.dynamics.constraintsolver.ConstraintSolver;
+import javabullet.dynamics.constraintsolver.SequentialImpulseConstraintSolver;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+import javax.media.opengl.*;
+
+/**
+ *
+ * @author jezek2
+ */
+public class GenericJointDemo extends DemoApplication {
+
+	private List<RagDoll> ragdolls = new ArrayList<RagDoll>();
+
+	public GenericJointDemo(String[] args) {
+		super(args);
+	}
+
+	public void initPhysics() {
+		// Setup the basic world
+		DefaultCollisionConfiguration collision_config = new DefaultCollisionConfiguration();
+
+		CollisionDispatcher dispatcher = new CollisionDispatcher(collision_config);
+
+		//btPoint3 worldAabbMin(-10000,-10000,-10000);
+		//btPoint3 worldAabbMax(10000,10000,10000);
+		//btBroadphaseInterface* overlappingPairCache = new btAxisSweep3 (worldAabbMin, worldAabbMax);
+		BroadphaseInterface overlappingPairCache = new SimpleBroadphase();
+
+		//#ifdef USE_ODE_QUICKSTEP
+		//btConstraintSolver* constraintSolver = new OdeConstraintSolver();
+		//#else
+		ConstraintSolver constraintSolver = new SequentialImpulseConstraintSolver();
+		//#endif
+
+		dynamicsWorld = new DiscreteDynamicsWorld(dispatcher, overlappingPairCache, constraintSolver, collision_config);
+
+		dynamicsWorld.setGravity(new Vector3f(0f, -30f, 0f));
+
+		// Setup a big ground box
+		{
+			CollisionShape groundShape = new BoxShape(new Vector3f(200f, 10f, 200f));
+			Transform groundTransform = new Transform();
+			groundTransform.setIdentity();
+			groundTransform.origin.set(0f, -15f, 0f);
+			localCreateRigidBody(0f, groundTransform, groundShape);
+		}
+
+		// Spawn one ragdoll
+		spawnRagdoll();
+
+		clientResetScene();
+	}
+
+	public void spawnRagdoll() {
+		spawnRagdoll(false);
+	}
+	
+	public void spawnRagdoll(boolean random) {
+		RagDoll ragDoll = new RagDoll(dynamicsWorld, new Vector3f(0f, 0f, 10f), 5f);
+		ragdolls.add(ragDoll);
+	}
+	
+	@Override
+	public void keyboardCallback(char key) {
+		switch (key) {
+			case 'e':
+				spawnRagdoll(true);
+				break;
+			default:
+				super.keyboardCallback(key);
+		}
+	}
+
+	public static void main(String[] args) {
+		GenericJointDemo demoApp = new GenericJointDemo(args);
+		demoApp.initPhysics();
+		demoApp.setCameraDistance(10f);
+
+		JOGL.main("Joint 6DOF - Sequencial Impulse Solver", demoApp, args);
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/demos/genericjoint/RagDoll.java b/src/jbullet/src/javabullet/demos/genericjoint/RagDoll.java
new file mode 100644
index 0000000..20ec000
--- /dev/null
+++ b/src/jbullet/src/javabullet/demos/genericjoint/RagDoll.java
@@ -0,0 +1,474 @@
+/*
+ * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]>
+ *
+ * Bullet Continuous Collision Detection and Physics Library
+ * Ragdoll Demo
+ * 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: Marten Svanfeldt
+ */
+
+package javabullet.demos.genericjoint;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javabullet.collision.shapes.CapsuleShape;
+import javabullet.collision.shapes.CollisionShape;
+import javabullet.dynamics.DynamicsWorld;
+import javabullet.dynamics.RigidBody;
+import javabullet.dynamics.RigidBodyConstructionInfo;
+import javabullet.dynamics.constraintsolver.Generic6DofConstraint;
+import javabullet.dynamics.constraintsolver.TypedConstraint;
+import javabullet.linearmath.DefaultMotionState;
+import javabullet.linearmath.MatrixUtil;
+import javabullet.linearmath.Transform;
+import javax.vecmath.Vector3f;
+
+/**
+ *
+ * @author jezek2
+ */
+public class RagDoll {
+	
+	protected final BulletStack stack = BulletStack.get();
+
+	public enum BodyPart {
+		BODYPART_PELVIS,
+		BODYPART_SPINE,
+		BODYPART_HEAD,
+
+		BODYPART_LEFT_UPPER_LEG,
+		BODYPART_LEFT_LOWER_LEG,
+
+		BODYPART_RIGHT_UPPER_LEG,
+		BODYPART_RIGHT_LOWER_LEG,
+
+		BODYPART_LEFT_UPPER_ARM,
+		BODYPART_LEFT_LOWER_ARM,
+
+		BODYPART_RIGHT_UPPER_ARM,
+		BODYPART_RIGHT_LOWER_ARM,
+
+		BODYPART_COUNT;
+	}
+
+	public enum JointType {
+		JOINT_PELVIS_SPINE,
+		JOINT_SPINE_HEAD,
+
+		JOINT_LEFT_HIP,
+		JOINT_LEFT_KNEE,
+
+		JOINT_RIGHT_HIP,
+		JOINT_RIGHT_KNEE,
+
+		JOINT_LEFT_SHOULDER,
+		JOINT_LEFT_ELBOW,
+
+		JOINT_RIGHT_SHOULDER,
+		JOINT_RIGHT_ELBOW,
+
+		JOINT_COUNT
+	}
+
+	private DynamicsWorld ownerWorld;
+	private CollisionShape[] shapes = new CollisionShape[BodyPart.BODYPART_COUNT.ordinal()];
+	private RigidBody[] bodies = new RigidBody[BodyPart.BODYPART_COUNT.ordinal()];
+	private TypedConstraint[] joints = new TypedConstraint[JointType.JOINT_COUNT.ordinal()];
+
+	public RagDoll(DynamicsWorld ownerWorld, Vector3f positionOffset) {
+		this(ownerWorld, positionOffset, 1.0f);
+	}
+
+	public RagDoll(DynamicsWorld ownerWorld, Vector3f positionOffset, float scale_ragdoll) {
+		this.ownerWorld = ownerWorld;
+
+		stack.pushCommonMath();
+		try {
+			Transform tmpTrans = stack.transforms.get();
+
+			// Setup the geometry
+			shapes[BodyPart.BODYPART_PELVIS.ordinal()] = new CapsuleShape(scale_ragdoll * 0.15f, scale_ragdoll * 0.20f);
+			shapes[BodyPart.BODYPART_SPINE.ordinal()] = new CapsuleShape(scale_ragdoll * 0.15f, scale_ragdoll * 0.28f);
+			shapes[BodyPart.BODYPART_HEAD.ordinal()] = new CapsuleShape(scale_ragdoll * 0.10f, scale_ragdoll * 0.05f);
+			shapes[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()] = new CapsuleShape(scale_ragdoll * 0.07f, scale_ragdoll * 0.45f);
+			shapes[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()] = new CapsuleShape(scale_ragdoll * 0.05f, scale_ragdoll * 0.37f);
+			shapes[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()] = new CapsuleShape(scale_ragdoll * 0.07f, scale_ragdoll * 0.45f);
+			shapes[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()] = new CapsuleShape(scale_ragdoll * 0.05f, scale_ragdoll * 0.37f);
+			shapes[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()] = new CapsuleShape(scale_ragdoll * 0.05f, scale_ragdoll * 0.33f);
+			shapes[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()] = new CapsuleShape(scale_ragdoll * 0.04f, scale_ragdoll * 0.25f);
+			shapes[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()] = new CapsuleShape(scale_ragdoll * 0.05f, scale_ragdoll * 0.33f);
+			shapes[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()] = new CapsuleShape(scale_ragdoll * 0.04f, scale_ragdoll * 0.25f);
+
+			// Setup all the rigid bodies
+			Transform offset = stack.transforms.get();
+			offset.setIdentity();
+			offset.origin.set(positionOffset);
+
+			Transform transform = stack.transforms.get();
+			transform.setIdentity();
+			transform.origin.set(0f, scale_ragdoll * 1f, 0f);
+			tmpTrans.mul(offset, transform);
+			bodies[BodyPart.BODYPART_PELVIS.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_PELVIS.ordinal()]);
+
+			transform.setIdentity();
+			transform.origin.set(0f, scale_ragdoll * 1.2f, 0f);
+			tmpTrans.mul(offset, transform);
+			bodies[BodyPart.BODYPART_SPINE.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_SPINE.ordinal()]);
+
+			transform.setIdentity();
+			transform.origin.set(0f, scale_ragdoll * 1.6f, 0f);
+			tmpTrans.mul(offset, transform);
+			bodies[BodyPart.BODYPART_HEAD.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_HEAD.ordinal()]);
+
+			transform.setIdentity();
+			transform.origin.set(-0.18f * scale_ragdoll, 0.65f * scale_ragdoll, 0f);
+			tmpTrans.mul(offset, transform);
+			bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()]);
+
+			transform.setIdentity();
+			transform.origin.set(-0.18f * scale_ragdoll, 0.2f * scale_ragdoll, 0f);
+			tmpTrans.mul(offset, transform);
+			bodies[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()]);
+
+			transform.setIdentity();
+			transform.origin.set(0.18f * scale_ragdoll, 0.65f * scale_ragdoll, 0f);
+			tmpTrans.mul(offset, transform);
+			bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()]);
+
+			transform.setIdentity();
+			transform.origin.set(0.18f * scale_ragdoll, 0.2f * scale_ragdoll, 0f);
+			tmpTrans.mul(offset, transform);
+			bodies[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()]);
+
+			transform.setIdentity();
+			transform.origin.set(-0.35f * scale_ragdoll, 1.45f * scale_ragdoll, 0f);
+			MatrixUtil.setEulerZYX(transform.basis, 0, 0, BulletGlobals.SIMD_HALF_PI);
+			tmpTrans.mul(offset, transform);
+			bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()]);
+
+			transform.setIdentity();
+			transform.origin.set(-0.7f * scale_ragdoll, 1.45f * scale_ragdoll, 0f);
+			MatrixUtil.setEulerZYX(transform.basis, 0, 0, BulletGlobals.SIMD_HALF_PI);
+			tmpTrans.mul(offset, transform);
+			bodies[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()]);
+
+			transform.setIdentity();
+			transform.origin.set(0.35f * scale_ragdoll, 1.45f * scale_ragdoll, 0f);
+			MatrixUtil.setEulerZYX(transform.basis, 0, 0, -BulletGlobals.SIMD_HALF_PI);
+			tmpTrans.mul(offset, transform);
+			bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()]);
+
+			transform.setIdentity();
+			transform.origin.set(0.7f * scale_ragdoll, 1.45f * scale_ragdoll, 0f);
+			MatrixUtil.setEulerZYX(transform.basis, 0, 0, -BulletGlobals.SIMD_HALF_PI);
+			tmpTrans.mul(offset, transform);
+			bodies[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()]);
+
+			// Setup some damping on the m_bodies
+			for (int i = 0; i < BodyPart.BODYPART_COUNT.ordinal(); ++i) {
+				bodies[i].setDamping(0.05f, 0.85f);
+				bodies[i].setDeactivationTime(0.8f);
+				bodies[i].setSleepingThresholds(1.6f, 2.5f);
+			}
+
+			///////////////////////////// SETTING THE CONSTRAINTS /////////////////////////////////////////////7777
+			// Now setup the constraints
+			Generic6DofConstraint joint6DOF;
+			Transform localA = stack.transforms.get(), localB = stack.transforms.get();
+			boolean useLinearReferenceFrameA = true;
+			/// ******* SPINE HEAD ******** ///
+			{
+				localA.setIdentity();
+				localB.setIdentity();
+
+				localA.origin.set(0f, 0.30f * scale_ragdoll, 0f);
+
+				localB.origin.set(0f, -0.14f * scale_ragdoll, 0f);
+
+				joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_SPINE.ordinal()], bodies[BodyPart.BODYPART_HEAD.ordinal()], localA, localB, useLinearReferenceFrameA);
+
+				//#ifdef RIGID
+				//joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
+				//joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
+				//#else
+				joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_PI * 0.3f, -BulletGlobals.FLT_EPSILON, -BulletGlobals.SIMD_PI * 0.3f));
+				joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.5f, BulletGlobals.FLT_EPSILON, BulletGlobals.SIMD_PI * 0.3f));
+				//#endif
+				joints[JointType.JOINT_SPINE_HEAD.ordinal()] = joint6DOF;
+				ownerWorld.addConstraint(joints[JointType.JOINT_SPINE_HEAD.ordinal()], true);
+			}
+			/// *************************** ///
+
+			/// ******* LEFT SHOULDER ******** ///
+			{
+				localA.setIdentity();
+				localB.setIdentity();
+
+				localA.origin.set(-0.2f * scale_ragdoll, 0.15f * scale_ragdoll, 0f);
+
+				MatrixUtil.setEulerZYX(localB.basis, BulletGlobals.SIMD_HALF_PI, 0, -BulletGlobals.SIMD_HALF_PI);
+				localB.origin.set(0f, -0.18f * scale_ragdoll, 0f);
+
+				joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_SPINE.ordinal()], bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA);
+
+				//#ifdef RIGID
+				//joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
+				//joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
+				//#else
+				joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_PI * 0.8f, -BulletGlobals.FLT_EPSILON, -BulletGlobals.SIMD_PI * 0.5f));
+				joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.8f, BulletGlobals.FLT_EPSILON, BulletGlobals.SIMD_PI * 0.5f));
+				//#endif
+				joints[JointType.JOINT_LEFT_SHOULDER.ordinal()] = joint6DOF;
+				ownerWorld.addConstraint(joints[JointType.JOINT_LEFT_SHOULDER.ordinal()], true);
+			}
+			/// *************************** ///
+
+			/// ******* RIGHT SHOULDER ******** ///
+			{
+				localA.setIdentity();
+				localB.setIdentity();
+
+				localA.origin.set(0.2f * scale_ragdoll, 0.15f * scale_ragdoll, 0f);
+				MatrixUtil.setEulerZYX(localB.basis, 0, 0, BulletGlobals.SIMD_HALF_PI);
+				localB.origin.set(0f, -0.18f * scale_ragdoll, 0f);
+				joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_SPINE.ordinal()], bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA);
+
+				//#ifdef RIGID
+				//joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
+				//joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
+				//#else
+				joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_PI * 0.8f, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_PI * 0.5f));
+				joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.8f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_PI * 0.5f));
+				//#endif
+				joints[JointType.JOINT_RIGHT_SHOULDER.ordinal()] = joint6DOF;
+				ownerWorld.addConstraint(joints[JointType.JOINT_RIGHT_SHOULDER.ordinal()], true);
+			}
+			/// *************************** ///
+
+			/// ******* LEFT ELBOW ******** ///
+			{
+				localA.setIdentity();
+				localB.setIdentity();
+
+				localA.origin.set(0f, 0.18f * scale_ragdoll, 0f);
+				localB.origin.set(0f, -0.14f * scale_ragdoll, 0f);
+				joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()], bodies[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA);
+
+				//#ifdef RIGID
+				//joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
+				//joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
+				//#else
+				joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON));
+				joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.7f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON));
+				//#endif
+				joints[JointType.JOINT_LEFT_ELBOW.ordinal()] = joint6DOF;
+				ownerWorld.addConstraint(joints[JointType.JOINT_LEFT_ELBOW.ordinal()], true);
+			}
+			/// *************************** ///
+
+			/// ******* RIGHT ELBOW ******** ///
+			{
+				localA.setIdentity();
+				localB.setIdentity();
+
+				localA.origin.set(0f, 0.18f * scale_ragdoll, 0f);
+				localB.origin.set(0f, -0.14f * scale_ragdoll, 0f);
+				joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()], bodies[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA);
+
+				//#ifdef RIGID
+				//joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
+				//joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
+				//#else
+				joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON));
+				joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.7f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON));
+				//#endif
+
+				joints[JointType.JOINT_RIGHT_ELBOW.ordinal()] = joint6DOF;
+				ownerWorld.addConstraint(joints[JointType.JOINT_RIGHT_ELBOW.ordinal()], true);
+			}
+			/// *************************** ///
+
+
+			/// ******* PELVIS ******** ///
+			{
+				localA.setIdentity();
+				localB.setIdentity();
+
+				MatrixUtil.setEulerZYX(localA.basis, 0, BulletGlobals.SIMD_HALF_PI, 0);
+				localA.origin.set(0f, 0.15f * scale_ragdoll, 0f);
+				MatrixUtil.setEulerZYX(localB.basis, 0, BulletGlobals.SIMD_HALF_PI, 0);
+				localB.origin.set(0f, -0.15f * scale_ragdoll, 0f);
+				joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_PELVIS.ordinal()], bodies[BodyPart.BODYPART_SPINE.ordinal()], localA, localB, useLinearReferenceFrameA);
+
+				//#ifdef RIGID
+				//joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
+				//joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
+				//#else
+				joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_PI * 0.2f, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_PI * 0.3f));
+				joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.2f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_PI * 0.6f));
+				//#endif
+				joints[JointType.JOINT_PELVIS_SPINE.ordinal()] = joint6DOF;
+				ownerWorld.addConstraint(joints[JointType.JOINT_PELVIS_SPINE.ordinal()], true);
+			}
+			/// *************************** ///
+
+			/// ******* LEFT HIP ******** ///
+			{
+				localA.setIdentity();
+				localB.setIdentity();
+
+				localA.origin.set(-0.18f * scale_ragdoll, -0.10f * scale_ragdoll, 0f);
+
+				localB.origin.set(0f, 0.225f * scale_ragdoll, 0f);
+
+				joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_PELVIS.ordinal()], bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA);
+
+				//#ifdef RIGID
+				//joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
+				//joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
+				//#else
+				joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_HALF_PI * 0.5f, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON));
+				joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_HALF_PI * 0.8f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_HALF_PI * 0.6f));
+				//#endif
+				joints[JointType.JOINT_LEFT_HIP.ordinal()] = joint6DOF;
+				ownerWorld.addConstraint(joints[JointType.JOINT_LEFT_HIP.ordinal()], true);
+			}
+			/// *************************** ///
+
+
+			/// ******* RIGHT HIP ******** ///
+			{
+				localA.setIdentity();
+				localB.setIdentity();
+
+				localA.origin.set(0.18f * scale_ragdoll, -0.10f * scale_ragdoll, 0f);
+				localB.origin.set(0f, 0.225f * scale_ragdoll, 0f);
+
+				joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_PELVIS.ordinal()], bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA);
+
+				//#ifdef RIGID
+				//joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
+				//joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
+				//#else
+				joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_HALF_PI * 0.5f, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_HALF_PI * 0.6f));
+				joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_HALF_PI * 0.8f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON));
+				//#endif
+				joints[JointType.JOINT_RIGHT_HIP.ordinal()] = joint6DOF;
+				ownerWorld.addConstraint(joints[JointType.JOINT_RIGHT_HIP.ordinal()], true);
+			}
+			/// *************************** ///
+
+
+			/// ******* LEFT KNEE ******** ///
+			{
+				localA.setIdentity();
+				localB.setIdentity();
+
+				localA.origin.set(0f, -0.225f * scale_ragdoll, 0f);
+				localB.origin.set(0f, 0.185f * scale_ragdoll, 0f);
+				joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()], bodies[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA);
+				//
+				//#ifdef RIGID
+				//joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
+				//joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
+				//#else
+				joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON));
+				joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.7f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON));
+				//#endif
+				joints[JointType.JOINT_LEFT_KNEE.ordinal()] = joint6DOF;
+				ownerWorld.addConstraint(joints[JointType.JOINT_LEFT_KNEE.ordinal()], true);
+			}
+			/// *************************** ///
+
+			/// ******* RIGHT KNEE ******** ///
+			{
+				localA.setIdentity();
+				localB.setIdentity();
+
+				localA.origin.set(0f, -0.225f * scale_ragdoll, 0f);
+				localB.origin.set(0f, 0.185f * scale_ragdoll, 0f);
+				joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()], bodies[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA);
+
+				//#ifdef RIGID
+				//joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON));
+				//joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON));
+				//#else
+				joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON));
+				joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.7f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON));
+				//#endif
+				joints[JointType.JOINT_RIGHT_KNEE.ordinal()] = joint6DOF;
+				ownerWorld.addConstraint(joints[JointType.JOINT_RIGHT_KNEE.ordinal()], true);
+			}
+			/// *************************** ///
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+
+	public void destroy() {
+		int i;
+
+		// Remove all constraints
+		for (i = 0; i < JointType.JOINT_COUNT.ordinal(); ++i) {
+			ownerWorld.removeConstraint(joints[i]);
+			//joints[i].destroy();
+			joints[i] = null;
+		}
+
+		// Remove all bodies and shapes
+		for (i = 0; i < BodyPart.BODYPART_COUNT.ordinal(); ++i) {
+			ownerWorld.removeRigidBody(bodies[i]);
+
+			//bodies[i].getMotionState().destroy();
+
+			bodies[i].destroy();
+			bodies[i] = null;
+
+			//shapes[i].destroy();
+			shapes[i] = null;
+		}
+	}
+	
+	private RigidBody localCreateRigidBody(float mass, Transform startTransform, CollisionShape shape) {
+		stack.vectors.push();
+		try {
+			boolean isDynamic = (mass != 0f);
+
+			Vector3f localInertia = stack.vectors.get(0f, 0f, 0f);
+			if (isDynamic) {
+				shape.calculateLocalInertia(mass, localInertia);
+			}
+
+			DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
+			RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia);
+			rbInfo.additionalDamping = true;
+			RigidBody body = new RigidBody(rbInfo);
+
+			ownerWorld.addRigidBody(body);
+
+			return body;
+		}
+		finally {
+			stack.vectors.pop();
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/demos/opengl/DejaVu_Sans_11.fnt b/src/jbullet/src/javabullet/demos/opengl/DejaVu_Sans_11.fnt
new file mode 100644
index 0000000..0abba78
--- /dev/null
+++ b/src/jbullet/src/javabullet/demos/opengl/DejaVu_Sans_11.fnt
diff --git a/src/jbullet/src/javabullet/demos/opengl/DemoApplication.java b/src/jbullet/src/javabullet/demos/opengl/DemoApplication.java
new file mode 100644
index 0000000..18c0bae
--- /dev/null
+++ b/src/jbullet/src/javabullet/demos/opengl/DemoApplication.java
@@ -0,0 +1,1203 @@
+/*
+ * 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.opengl;
+
+import javabullet.BulletGlobals;
+import javabullet.BulletStack;
+import javabullet.collision.dispatch.CollisionObject;
+import javabullet.collision.dispatch.CollisionWorld;
+import javabullet.collision.shapes.BoxShape;
+import javabullet.collision.shapes.CollisionShape;
+import javabullet.dynamics.DynamicsWorld;
+import javabullet.dynamics.RigidBody;
+import javabullet.dynamics.RigidBodyConstructionInfo;
+import javabullet.dynamics.constraintsolver.Point2PointConstraint;
+import javabullet.dynamics.constraintsolver.TypedConstraint;
+import javabullet.linearmath.Clock;
+import javabullet.linearmath.DebugDrawModes;
+import javabullet.linearmath.DefaultMotionState;
+import javabullet.linearmath.QuaternionUtil;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Color3f;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Quat4f;
+import javax.vecmath.Vector3f;
+import javax.media.opengl.*;
+import javax.media.opengl.glu.*;
+import java.nio.*;
+
+import com.sun.javafx.newt.*;
+
+/**
+ *
+ * @author jezek2
+ */
+public abstract class DemoApplication 
+    implements GLEventListener, MouseListener, KeyListener
+{
+
+	protected final BulletStack stack = BulletStack.get();
+	
+	private static final float STEPSIZE = 5;
+	
+	public static int numObjects = 0;
+	public static final int maxNumObjects = 16384;
+	public static Transform[] startTransforms = new Transform[maxNumObjects];
+	public static CollisionShape[] gShapePtr = new CollisionShape[maxNumObjects]; //1 rigidbody has 1 shape (no re-use of shapes)
+	
+	public static RigidBody pickedBody = null; // for deactivation state
+	
+	static {
+		for (int i=0; i<startTransforms.length; i++) {
+			startTransforms[i] = new Transform();
+		}
+	}
+	// TODO: class CProfileIterator* m_profileIterator;
+	
+	protected Clock clock = new Clock();
+
+	// this is the most important class
+	protected DynamicsWorld dynamicsWorld = null;
+
+	// constraint for mouse picking
+	protected TypedConstraint pickConstraint = null;
+
+	protected CollisionShape shootBoxShape = null;
+
+	protected float cameraDistance = 15f;
+	protected int debugMode = 0;
+	
+	protected float ele = 20f;
+	protected float azi = 0f;
+	protected final Vector3f cameraPosition = new Vector3f(0f, 0f, 0f);
+	protected final Vector3f cameraTargetPosition = new Vector3f(0f, 0f, 0f); // look at
+
+	protected float scaleBottom = 0.5f;
+	protected float scaleFactor = 2f;
+	protected final Vector3f cameraUp = new Vector3f(0f, 1f, 0f);
+	protected int forwardAxis = 2;
+
+	protected int glutScreenWidth = 0;
+	protected int glutScreenHeight = 0;
+
+	protected float ShootBoxInitialSpeed = 40f;
+	
+	protected boolean stepping = true;
+	protected boolean singleStep = false;
+	protected boolean idle = false;
+	protected int lastKey;
+
+	protected GLU    glu=null;
+	protected GLSRT  glsrt=null;
+
+	protected boolean useLight0 = true;
+	protected boolean useLight1 = true;
+
+	public DemoApplication(String[] args) {
+        // debugMode |= DebugDrawModes.DRAW_WIREFRAME;
+        debugMode |= DebugDrawModes.NO_HELP_TEXT;
+        //debugMode |= DebugDrawModes.DISABLE_BULLET_LCP;
+
+        for(int i=args.length-1; i>=0; i--) {
+            if(args[i].equals("-nolight0")) {
+                useLight0=false;
+            } else if(args[i].equals("-nolight1")) {
+                useLight1=false;
+            }
+        }
+	}
+	
+	public abstract void initPhysics() throws Exception;
+	
+	public void destroy() {
+		// TODO: CProfileManager::Release_Iterator(m_profileIterator);
+		//if (m_shootBoxShape)
+		//	delete m_shootBoxShape;
+	}
+
+    // 
+    // GLEventListener
+    //
+
+    public void init(GLAutoDrawable drawable) {
+		float[] light_ambient = new float[] { 0.2f, 0.2f, 0.2f, 1.0f };
+		float[] light_diffuse = new float[] { 1.0f, 1.0f, 1.0f, 1.0f };
+		float[] light_specular = new float[] { 1.0f, 1.0f, 1.0f, 1.0f };
+		/* light_position is NOT default value */
+		float[] light_position0 = new float[] { 1.0f, 10.0f, 1.0f, 0.0f };
+		float[] light_position1 = new float[] { -1.0f, -10.0f, -1.0f, 0.0f };
+        gl = drawable.getGL();
+        glu = GLU.createGLU();
+
+        if(gl.isGLES2()) {
+            gl.getGLES2().enableFixedFunctionEmulationMode(GLES2.FIXED_EMULATION_VERTEXCOLORTEXTURE);
+        }
+
+        System.err.println("Entering initialization");
+        System.err.print("GL Profile: ");
+        System.err.println(GLProfile.getProfile());
+        System.err.print("GL:");
+        System.err.println(gl);
+        System.err.print("GL_VERSION=");
+        System.err.println(gl.glGetString(gl.GL_VERSION));
+        System.err.print("GL_EXTENSIONS: ");
+        System.err.println(gl.glGetString(gl.GL_EXTENSIONS));
+
+        glsrt = new GLSRT(glu, gl);
+        if(useLight0) {
+            gl.glLightfv(gl.GL_LIGHT0, gl.GL_AMBIENT, light_ambient, 0);
+            gl.glLightfv(gl.GL_LIGHT0, gl.GL_DIFFUSE, light_diffuse, 0);
+            gl.glLightfv(gl.GL_LIGHT0, gl.GL_SPECULAR, light_specular, 0);
+            gl.glLightfv(gl.GL_LIGHT0, gl.GL_POSITION, light_position0, 0);
+        }
+
+        if(useLight1) {
+            gl.glLightfv(gl.GL_LIGHT1, gl.GL_AMBIENT, light_ambient, 0);
+            gl.glLightfv(gl.GL_LIGHT1, gl.GL_DIFFUSE, light_diffuse, 0);
+            gl.glLightfv(gl.GL_LIGHT1, gl.GL_SPECULAR, light_specular, 0);
+            gl.glLightfv(gl.GL_LIGHT1, gl.GL_POSITION, light_position1, 0);
+        }
+
+        if(useLight0 || useLight1) {
+            gl.glEnable(gl.GL_LIGHTING);
+        }
+        if(useLight0) {
+            gl.glEnable(gl.GL_LIGHT0);
+        }
+        if(useLight1) {
+            gl.glEnable(gl.GL_LIGHT1);
+        }
+
+        gl.glShadeModel(gl.GL_SMOOTH);
+
+		gl.glEnable(gl.GL_DEPTH_TEST);
+		gl.glDepthFunc(gl.GL_LESS);
+
+		gl.glClearColor(0.7f, 0.7f, 0.7f, 0f);
+
+        // JAU
+		gl.glEnable(gl.GL_CULL_FACE);
+		gl.glCullFace(gl.GL_BACK);
+	}
+
+    public void dispose(GLAutoDrawable drawable) {
+        gl=null;
+    }
+
+    public void reshape(GLAutoDrawable drawable, int x, int y, int width, int height) {
+        gl = drawable.getGL();
+		glutScreenWidth = width;
+		glutScreenHeight = height;
+
+		//gl.glViewport(x, y, width, height);
+		updateCamera();
+
+        System.out.println("DemoApplication RESHAPE");
+	}
+
+    public void display(GLAutoDrawable drawable) {
+        gl = drawable.getGL();
+
+        gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT);
+
+        if(!isIdle()) {
+            // simple dynamics world doesn't handle fixed-time-stepping
+            float ms = clock.getTimeMicroseconds();
+            clock.reset();
+            float minFPS = 1000000f / 60f;
+            if (ms > minFPS) {
+                ms = minFPS;
+            }
+            if (dynamicsWorld != null) {
+                dynamicsWorld.stepSimulation(ms / 1000000.f);
+            }
+        }
+
+        if (dynamicsWorld != null) {
+            // optional but useful: debug drawing
+            dynamicsWorld.debugDrawWorld();
+        }
+
+        renderme();
+
+        //glFlush();
+        //glutSwapBuffers();
+    }
+
+    public void displayChanged(GLAutoDrawable drawable, boolean modeChanged, boolean deviceChanged) {
+    }
+
+    protected GL  gl;
+
+    // 
+    // MouseListener
+    //
+    public void mouseClicked(MouseEvent e) {
+        mouseClick(e.getButton(), e.getX(), e.getY());
+    }
+    public void mouseEntered(MouseEvent e) {
+    }
+    public void mouseExited(MouseEvent e) {
+    }
+    public void mousePressed(MouseEvent e) {
+        pickConstrain(e.getButton(), 1, e.getX(), e.getY());
+    }
+    public void mouseReleased(MouseEvent e)  {
+        pickConstrain(e.getButton(), 0, e.getX(), e.getY());
+    }
+
+    // 
+    // MouseMotionListener
+    //
+    public void mouseDragged(MouseEvent e) {
+        mouseMotionFunc(e.getX(), e.getY());
+    }
+
+    public void mouseMoved(MouseEvent e) {
+    }
+
+    //
+    // KeyListener
+    //
+    public void keyPressed(KeyEvent e) {
+    }
+    public void keyReleased(KeyEvent e) {
+        char c = e.getKeyChar();
+        if(e.isActionKey()) {
+            specialKeyboard(e.getKeyCode());
+        } else {
+            keyboardCallback(e.getKeyChar());
+        }
+    }
+    public void keyTyped(KeyEvent e) {
+    }
+
+    //
+    // 
+    //
+
+	public void setCameraDistance(float dist) {
+		cameraDistance = dist;
+	}
+
+	public float getCameraDistance() {
+		return cameraDistance;
+	}
+
+	public void toggleIdle() {
+		if (idle) {
+			idle = false;
+		}
+		else {
+			idle = true;
+		}
+	}
+	
+	public void updateCamera() {
+		stack.vectors.push();
+		stack.matrices.push();
+		stack.quats.push();
+		try {
+			gl.glMatrixMode(gl.GL_PROJECTION);
+			gl.glLoadIdentity();
+			float rele = ele * 0.01745329251994329547f; // rads per deg
+			float razi = azi * 0.01745329251994329547f; // rads per deg
+
+			Quat4f rot = stack.quats.get();
+			QuaternionUtil.setRotation(rot, cameraUp, razi);
+
+			Vector3f eyePos = stack.vectors.get(0f, 0f, 0f);
+			VectorUtil.setCoord(eyePos, forwardAxis, -cameraDistance);
+
+			Vector3f forward = stack.vectors.get(eyePos.x, eyePos.y, eyePos.z);
+			if (forward.lengthSquared() < BulletGlobals.FLT_EPSILON) {
+				forward.set(1f, 0f, 0f);
+			}
+			Vector3f right = stack.vectors.get();
+			right.cross(cameraUp, forward);
+			Quat4f roll = stack.quats.get();
+			QuaternionUtil.setRotation(roll, right, -rele);
+
+			Matrix3f tmpMat1 = stack.matrices.get();
+			Matrix3f tmpMat2 = stack.matrices.get();
+			tmpMat1.set(rot);
+			tmpMat2.set(roll);
+			tmpMat1.mul(tmpMat2);
+			tmpMat1.transform(eyePos);
+
+			cameraPosition.set(eyePos);
+
+			gl.glFrustumf(-1.0f, 1.0f, -1.0f, 1.0f, 1.0f, 10000.0f);
+			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);
+			gl.glLoadIdentity();
+		}
+		finally {
+			stack.vectors.pop();
+			stack.matrices.pop();
+			stack.quats.pop();
+		}
+	}
+	
+	public void stepLeft() {
+		azi -= STEPSIZE;
+		if (azi < 0) {
+			azi += 360;
+		}
+	}
+
+	public void stepRight() {
+		azi += STEPSIZE;
+		if (azi >= 360) {
+			azi -= 360;
+		}
+	}
+
+	public void stepFront() {
+		ele += STEPSIZE;
+		if (ele >= 360) {
+			ele -= 360;
+		}
+	}
+
+	public void stepBack() {
+		ele -= STEPSIZE;
+		if (ele < 0) {
+			ele += 360;
+		}
+	}
+
+	public void zoomIn() {
+		cameraDistance -= 0.4f;
+		if (cameraDistance < 0.1f) {
+			cameraDistance = 0.1f;
+		}
+	}
+
+	public void zoomOut() {
+		cameraDistance += 0.4f;
+	}
+
+	public void keyboardCallback(char key) {
+		lastKey = 0;
+
+		if (key >= 0x31 && key < 0x37) {
+			int child = key - 0x31;
+			// TODO: m_profileIterator->Enter_Child(child);
+		}
+		if (key == 0x30) {
+			// TODO: m_profileIterator->Enter_Parent();
+		}
+
+		switch (key) {
+			case 'l':
+				stepLeft();
+				break;
+			case 'r':
+				stepRight();
+				break;
+			case 'f':
+				stepFront();
+				break;
+			case 'b':
+				stepBack();
+				break;
+			case 'z':
+				zoomIn();
+				break;
+			case 'x':
+				zoomOut();
+				break;
+			case 'i':
+				toggleIdle();
+				break;
+			case 'h':
+				if ((debugMode & DebugDrawModes.NO_HELP_TEXT) != 0) {
+					debugMode = debugMode & (~DebugDrawModes.NO_HELP_TEXT);
+				}
+				else {
+					debugMode |= DebugDrawModes.NO_HELP_TEXT;
+				}
+				break;
+
+			case 'w':
+				if ((debugMode & DebugDrawModes.DRAW_WIREFRAME) != 0) {
+					debugMode = debugMode & (~DebugDrawModes.DRAW_WIREFRAME);
+				}
+				else {
+					debugMode |= DebugDrawModes.DRAW_WIREFRAME;
+				}
+				break;
+
+			case 'p':
+				if ((debugMode & DebugDrawModes.PROFILE_TIMINGS) != 0) {
+					debugMode = debugMode & (~DebugDrawModes.PROFILE_TIMINGS);
+				}
+				else {
+					debugMode |= DebugDrawModes.PROFILE_TIMINGS;
+				}
+				break;
+
+			case 'm':
+				if ((debugMode & DebugDrawModes.ENABLE_SAT_COMPARISON) != 0) {
+					debugMode = debugMode & (~DebugDrawModes.ENABLE_SAT_COMPARISON);
+				}
+				else {
+					debugMode |= DebugDrawModes.ENABLE_SAT_COMPARISON;
+				}
+				break;
+
+			case 'n':
+				if ((debugMode & DebugDrawModes.DISABLE_BULLET_LCP) != 0) {
+					debugMode = debugMode & (~DebugDrawModes.DISABLE_BULLET_LCP);
+				}
+				else {
+					debugMode |= DebugDrawModes.DISABLE_BULLET_LCP;
+				}
+				break;
+
+			case 't':
+				if ((debugMode & DebugDrawModes.DRAW_TEXT) != 0) {
+					debugMode = debugMode & (~DebugDrawModes.DRAW_TEXT);
+				}
+				else {
+					debugMode |= DebugDrawModes.DRAW_TEXT;
+				}
+				break;
+			case 'y':
+				if ((debugMode & DebugDrawModes.DRAW_FEATURES_TEXT) != 0) {
+					debugMode = debugMode & (~DebugDrawModes.DRAW_FEATURES_TEXT);
+				}
+				else {
+					debugMode |= DebugDrawModes.DRAW_FEATURES_TEXT;
+				}
+				break;
+			case 'a':
+				if ((debugMode & DebugDrawModes.DRAW_AABB) != 0) {
+					debugMode = debugMode & (~DebugDrawModes.DRAW_AABB);
+				}
+				else {
+					debugMode |= DebugDrawModes.DRAW_AABB;
+				}
+				break;
+			case 'c':
+				if ((debugMode & DebugDrawModes.DRAW_CONTACT_POINTS) != 0) {
+					debugMode = debugMode & (~DebugDrawModes.DRAW_CONTACT_POINTS);
+				}
+				else {
+					debugMode |= DebugDrawModes.DRAW_CONTACT_POINTS;
+				}
+				break;
+
+			case 'd':
+				if ((debugMode & DebugDrawModes.NO_DEACTIVATION) != 0) {
+					debugMode = debugMode & (~DebugDrawModes.NO_DEACTIVATION);
+				}
+				else {
+					debugMode |= DebugDrawModes.NO_DEACTIVATION;
+				}
+				if ((debugMode & DebugDrawModes.NO_DEACTIVATION) != 0) {
+					BulletGlobals.gDisableDeactivation = true;
+				}
+				else {
+					BulletGlobals.gDisableDeactivation = false;
+				}
+				break;
+
+			case 'o': {
+				stepping = !stepping;
+				break;
+			}
+			case 's':
+				break;
+			//    case ' ' : newRandom(); break;
+			case ' ':
+				clientResetScene();
+				break;
+			case '1': {
+				if ((debugMode & DebugDrawModes.ENABLE_CCD) != 0) {
+					debugMode = debugMode & (~DebugDrawModes.ENABLE_CCD);
+				}
+				else {
+					debugMode |= DebugDrawModes.ENABLE_CCD;
+				}
+				break;
+			}
+
+			case '.': {
+				shootBox(getCameraTargetPosition());
+				break;
+			}
+
+			case '+': {
+				ShootBoxInitialSpeed += 10f;
+				break;
+			}
+			case '-': {
+				ShootBoxInitialSpeed -= 10f;
+				break;
+			}
+
+			default:
+				// std::cout << "unused key : " << key << std::endl;
+				break;
+		}
+
+		if (getDynamicsWorld() != null && getDynamicsWorld().getDebugDrawer() != null) {
+			getDynamicsWorld().getDebugDrawer().setDebugMode(debugMode);
+		}
+
+		//LWJGL.postRedisplay();
+	}
+
+	public int getDebugMode() {
+		return debugMode;
+	}
+	
+	public void setDebugMode(int mode) {
+		debugMode = mode;
+		if (getDynamicsWorld() != null && getDynamicsWorld().getDebugDrawer() != null) {
+			getDynamicsWorld().getDebugDrawer().setDebugMode(mode);
+		}
+	}
+	
+	public void specialKeyboard(int keycode) {
+		switch (keycode) {
+			case KeyEvent.VK_F1: {
+				break;
+			}
+			case KeyEvent.VK_F2: {
+				break;
+			}
+			case KeyEvent.VK_END: {
+				int numObj = getDynamicsWorld().getNumCollisionObjects();
+				if (numObj != 0) {
+					CollisionObject obj = getDynamicsWorld().getCollisionObjectArray().get(numObj - 1);
+
+					getDynamicsWorld().removeCollisionObject(obj);
+					RigidBody body = RigidBody.upcast(obj);
+					if (body != null && body.getMotionState() != null) {
+						//delete body->getMotionState();
+					}
+					//delete obj;
+				}
+				break;
+			}
+			case KeyEvent.VK_LEFT:
+				stepLeft();
+				break;
+			case KeyEvent.VK_RIGHT:
+				stepRight();
+				break;
+			case KeyEvent.VK_UP:
+				stepFront();
+				break;
+			case KeyEvent.VK_DOWN:
+				stepBack();
+				break;
+            /*
+			case KeyEvent.VK_PRIOR:
+				zoomIn();
+				break;
+			case KeyEvent.VK_NEXT:
+				zoomOut();
+				break;
+            */
+			case KeyEvent.VK_HOME:
+				toggleIdle();
+				break;
+			default:
+				// std::cout << "unused (special) key : " << key << std::endl;
+				break;
+		}
+	}
+	
+	public void shootBox(Vector3f destination) {
+		if (dynamicsWorld != null) {
+			float mass = 10f;
+			Transform startTransform = new Transform();
+			startTransform.setIdentity();
+			Vector3f camPos = new Vector3f(getCameraPosition());
+			startTransform.origin.set(camPos);
+
+			if (shootBoxShape == null) {
+				//#define TEST_UNIFORM_SCALING_SHAPE 1
+				//#ifdef TEST_UNIFORM_SCALING_SHAPE
+				//btConvexShape* childShape = new btBoxShape(btVector3(1.f,1.f,1.f));
+				//m_shootBoxShape = new btUniformScalingShape(childShape,0.5f);
+				//#else
+				shootBoxShape = new BoxShape(new Vector3f(1f, 1f, 1f));
+				//#endif//
+			}
+
+			RigidBody body = this.localCreateRigidBody(mass, startTransform, shootBoxShape);
+
+			Vector3f linVel = new Vector3f(destination.x - camPos.x, destination.y - camPos.y, destination.z - camPos.z);
+			linVel.normalize();
+			linVel.scale(ShootBoxInitialSpeed);
+
+			body.getWorldTransform().origin.set(camPos);
+			body.getWorldTransform().setRotation(new Quat4f(0f, 0f, 0f, 1f));
+			body.setLinearVelocity(linVel);
+			body.setAngularVelocity(new Vector3f(0f, 0f, 0f));
+		}
+	}
+	
+	public Vector3f getRayTo(int x, int y) {
+		float top = 1f;
+		float bottom = -1f;
+		float nearPlane = 1f;
+		float tanFov = (top - bottom) * 0.5f / nearPlane;
+		float fov = 2f * (float) Math.atan(tanFov);
+
+		Vector3f rayFrom = new Vector3f(getCameraPosition());
+		Vector3f rayForward = new Vector3f();
+		rayForward.sub(getCameraTargetPosition(), getCameraPosition());
+		rayForward.normalize();
+		float farPlane = 600f;
+		rayForward.scale(farPlane);
+
+		Vector3f rightOffset = new Vector3f();
+		Vector3f vertical = new Vector3f(cameraUp);
+
+		Vector3f hor = new Vector3f();
+		// TODO: check: hor = rayForward.cross(vertical);
+		hor.cross(rayForward, vertical);
+		hor.normalize();
+		// TODO: check: vertical = hor.cross(rayForward);
+		vertical.cross(hor, rayForward);
+		vertical.normalize();
+
+		float tanfov = (float) Math.tan(0.5f * fov);
+		hor.scale(2f * farPlane * tanfov);
+		vertical.scale(2f * farPlane * tanfov);
+		Vector3f rayToCenter = new Vector3f();
+		rayToCenter.add(rayFrom, rayForward);
+		Vector3f dHor = new Vector3f(hor);
+		dHor.scale(1f / (float) glutScreenWidth);
+		Vector3f dVert = new Vector3f(vertical);
+		dVert.scale(1.f / (float) glutScreenHeight);
+
+		Vector3f tmp1 = new Vector3f();
+		Vector3f tmp2 = new Vector3f();
+		tmp1.scale(0.5f, hor);
+		tmp2.scale(0.5f, vertical);
+
+		Vector3f rayTo = new Vector3f();
+		rayTo.sub(rayToCenter, tmp1);
+		rayTo.add(tmp2);
+
+		tmp1.scale(x, dHor);
+		tmp2.scale(y, dVert);
+
+		rayTo.add(tmp1);
+		rayTo.sub(tmp2);
+		return rayTo;
+	}
+	
+	private void mouseClick(int button, int x, int y) {
+		Vector3f rayTo = new Vector3f(getRayTo(x, y));
+
+		switch (button) {
+			case MouseEvent.BUTTON3: {
+                shootBox(rayTo);
+				break;
+			}
+			case MouseEvent.BUTTON2 : {
+                // apply an impulse
+                if (dynamicsWorld != null) {
+                    CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(cameraPosition, rayTo);
+                    dynamicsWorld.rayTest(cameraPosition, rayTo, rayCallback);
+                    if (rayCallback.hasHit()) {
+                        RigidBody body = RigidBody.upcast(rayCallback.collisionObject);
+                        if (body != null) {
+                            body.setActivationState(CollisionObject.ACTIVE_TAG);
+                            Vector3f impulse = new Vector3f(rayTo);
+                            impulse.normalize();
+                            float impulseStrength = 10f;
+                            impulse.scale(impulseStrength);
+                            Vector3f relPos = new Vector3f();
+                            relPos.sub(rayCallback.hitPointWorld, body.getCenterOfMassPosition());
+                            body.applyImpulse(impulse, relPos);
+                        }
+                    }
+                }
+				break;
+			}
+		}
+	}
+
+	private void pickConstrain(int button, int state, int x, int y) {
+		Vector3f rayTo = new Vector3f(getRayTo(x, y));
+
+		switch (button) {
+			case MouseEvent.BUTTON1 : {
+				if (state == 1) {
+					// add a point to point constraint for picking
+					if (dynamicsWorld != null) {
+						CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(cameraPosition, rayTo);
+						dynamicsWorld.rayTest(cameraPosition, rayTo, rayCallback);
+						if (rayCallback.hasHit()) {
+							RigidBody body = RigidBody.upcast(rayCallback.collisionObject);
+							if (body != null) {
+								// other exclusions?
+								if (!(body.isStaticObject() || body.isKinematicObject())) {
+									pickedBody = body;
+									pickedBody.setActivationState(CollisionObject.DISABLE_DEACTIVATION);
+
+									Vector3f pickPos = new Vector3f(rayCallback.hitPointWorld);
+
+									Transform tmpTrans = new Transform(body.getCenterOfMassTransform());
+									tmpTrans.inverse();
+									Vector3f localPivot = new Vector3f(pickPos);
+									tmpTrans.transform(localPivot);
+
+									Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
+									dynamicsWorld.addConstraint(p2p);
+									pickConstraint = p2p;
+									// save mouse position for dragging
+									BulletGlobals.gOldPickingPos.set(rayTo);
+									Vector3f eyePos = new Vector3f(cameraPosition);
+									Vector3f tmp = new Vector3f();
+									tmp.sub(pickPos, eyePos);
+									BulletGlobals.gOldPickingDist = tmp.length();
+									// very weak constraint for picking
+									p2p.setting.tau = 0.1f;
+								}
+							}
+						}
+					}
+
+				}
+				else {
+
+					if (pickConstraint != null && dynamicsWorld != null) {
+						dynamicsWorld.removeConstraint(pickConstraint);
+						// delete m_pickConstraint;
+						pickConstraint = null;
+						pickedBody.forceActivationState(CollisionObject.ACTIVE_TAG);
+						pickedBody.setDeactivationTime(0f);
+						pickedBody = null;
+					}
+				}
+				break;
+			}
+			default: {
+			}
+		}
+	}
+	
+	private void mouseMotionFunc(int x, int y) {
+		if (pickConstraint != null) {
+			// move the constraint pivot
+			Point2PointConstraint p2p = (Point2PointConstraint) pickConstraint;
+			if (p2p != null) {
+				// keep it at the same picking distance
+
+				Vector3f newRayTo = new Vector3f(getRayTo(x, y));
+				Vector3f eyePos = new Vector3f(cameraPosition);
+				Vector3f dir = new Vector3f();
+				dir.sub(newRayTo, eyePos);
+				dir.normalize();
+				dir.scale(BulletGlobals.gOldPickingDist);
+
+				Vector3f newPos = new Vector3f();
+				newPos.add(eyePos, dir);
+				p2p.setPivotB(newPos);
+			}
+		}
+	}
+
+	public RigidBody localCreateRigidBody(float mass, Transform startTransform, CollisionShape shape) {
+		// rigidbody is dynamic if and only if mass is non zero, otherwise static
+		boolean isDynamic = (mass != 0f);
+
+		Vector3f localInertia = new Vector3f(0f, 0f, 0f);
+		if (isDynamic) {
+			shape.calculateLocalInertia(mass, localInertia);
+		}
+
+		// using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+
+		//#define USE_MOTIONSTATE 1
+		//#ifdef USE_MOTIONSTATE
+		DefaultMotionState myMotionState = new DefaultMotionState(startTransform);
+		RigidBody body = new RigidBody(new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia));
+		//#else
+		//btRigidBody* body = new btRigidBody(mass,0,shape,localInertia);	
+		//body->setWorldTransform(startTransform);
+		//#endif//
+		dynamicsWorld.addRigidBody(body);
+
+		return body;
+	}
+
+	// See http://www.lighthouse3d.com/opengl/glut/index.php?bmpfontortho
+	public void setOrthographicProjection() {
+		// switch to projection mode
+		gl.glMatrixMode(gl.GL_PROJECTION);
+		// save previous matrix which contains the 
+		//settings for the perspective projection
+		// gl.glPushMatrix();
+		// reset matrix
+		gl.glLoadIdentity();
+		// set a 2D orthographic projection
+		glu.gluOrtho2D(0f, glutScreenWidth, 0f, glutScreenHeight);
+		// invert the y axis, down is positive
+		gl.glScalef(1f, -1f, 1f);
+		// mover the origin from the bottom left corner
+		// to the upper left corner
+		gl.glTranslatef(0f, -glutScreenHeight, 0f);
+		gl.glMatrixMode(gl.GL_MODELVIEW);
+	}
+	
+    /**
+	public void resetPerspectiveProjection() {
+		gl.glMatrixMode(gl.GL_PROJECTION);
+		gl.glPopMatrix();
+		gl.glMatrixMode(gl.GL_MODELVIEW);
+	}
+	
+	private void displayProfileString(int xOffset, int yStart, String message) {
+		//glRasterPos3f(xOffset, yStart, 0);
+		// TODO: BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),message);
+	}
+	
+	// TODO: protected void showProfileInfo(float& xOffset,float& yStart, float yIncr);
+    */
+
+	private final Transform m = new Transform();
+	private Vector3f wireColor = new Vector3f();
+	private Color3f TEXT_COLOR = new Color3f(0f, 0f, 0f);
+	// private StringBuilder buf = new StringBuilder();
+
+	public void renderme() {
+		// JAU updateCamera();
+
+		if (dynamicsWorld != null) {
+			int numObjects = dynamicsWorld.getNumCollisionObjects();
+			wireColor.set(1f, 0f, 0f);
+			for (int i = 0; i < numObjects; i++) {
+				CollisionObject colObj = dynamicsWorld.getCollisionObjectArray().get(i);
+				RigidBody body = RigidBody.upcast(colObj);
+
+				if (body != null && body.getMotionState() != null) {
+					DefaultMotionState myMotionState = (DefaultMotionState) body.getMotionState();
+					m.set(myMotionState.graphicsWorldTrans);
+				} else {
+					m.set(colObj.getWorldTransform());
+				}
+
+                if(0==i) {
+                    wireColor.set(0.5f, 1f, 0.5f); // wants deactivation
+                } else {
+                    wireColor.set(1f, 1f, 0.5f); // wants deactivation
+                }
+				if ((i & 1) != 0) {
+					wireColor.set(0f, 0f, 1f);
+				}
+
+				// color differently for active, sleeping, wantsdeactivation states
+				if (colObj.getActivationState() == 1) // active
+				{
+					if ((i & 1) != 0) {
+						//wireColor.add(new Vector3f(1f, 0f, 0f));
+						wireColor.x += 1f;
+					}
+					else {
+						//wireColor.add(new Vector3f(0.5f, 0f, 0f));
+						wireColor.x += 0.5f;
+					}
+				}
+				if (colObj.getActivationState() == 2) // ISLAND_SLEEPING
+				{
+					if ((i & 1) != 0) {
+						//wireColor.add(new Vector3f(0f, 1f, 0f));
+						wireColor.y += 1f;
+					}
+					else {
+						//wireColor.add(new Vector3f(0f, 0.5f, 0f));
+						wireColor.y += 0.5f;
+					}
+				}
+
+                // draw (saves the matrix already ..)
+				GLShapeDrawer.drawOpenGL(glsrt, gl, m, colObj.getCollisionShape(), wireColor, getDebugMode());
+			}
+            GLShapeDrawer.drawCoordSystem(gl);
+            if(false) {
+            System.err.println("++++++++++++++++++++++++++++++++");
+            System.err.println("++++++++++++++++++++++++++++++++");
+            try {
+                Thread.sleep(2000);
+            } catch (Exception e) {}
+            }
+
+			float xOffset = 10f;
+			float yStart = 20f;
+			float yIncr = 20f;
+
+			// gl.glDisable(gl.GL_LIGHTING);
+			// JAU gl.glColor4f(0f, 0f, 0f, 0f);
+
+/*
+			if ((debugMode & DebugDrawModes.NO_HELP_TEXT) == 0) {
+				setOrthographicProjection();
+
+				// TODO: showProfileInfo(xOffset,yStart,yIncr);
+
+//					#ifdef USE_QUICKPROF
+//					if ( getDebugMode() & btIDebugDraw::DBG_ProfileTimings)
+//					{
+//						static int counter = 0;
+//						counter++;
+//						std::map<std::string, hidden::ProfileBlock*>::iterator iter;
+//						for (iter = btProfiler::mProfileBlocks.begin(); iter != btProfiler::mProfileBlocks.end(); ++iter)
+//						{
+//							char blockTime[128];
+//							sprintf(blockTime, "%s: %lf",&((*iter).first[0]),btProfiler::getBlockTime((*iter).first, btProfiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT));
+//							glRasterPos3f(xOffset,yStart,0);
+//							BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),blockTime);
+//							yStart += yIncr;
+//
+//						}
+//					}
+//					#endif //USE_QUICKPROF
+
+
+				String s = "mouse to interact";
+				drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				// JAVA NOTE: added
+				s = "LMB=shoot, RMB=drag, MIDDLE=apply impulse";
+				drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+				
+				s = "space to reset";
+				drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				s = "cursor keys and z,x to navigate";
+				drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				s = "i to toggle simulation, s single step";
+				drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				s = "q to quit";
+				drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				s = ". to shoot box";
+				drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				// not yet hooked up again after refactoring...
+
+				s = "d to toggle deactivation";
+				drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				s = "g to toggle mesh animation (ConcaveDemo)";
+				drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				// JAVA NOTE: added
+				s = "e to spawn new body (GenericJointDemo)";
+				drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				s = "h to toggle help text";
+				drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				//buf = "p to toggle profiling (+results to file)";
+				//drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				//bool useBulletLCP = !(getDebugMode() & btIDebugDraw::DBG_DisableBulletLCP);
+				//bool useCCD = (getDebugMode() & btIDebugDraw::DBG_EnableCCD);
+				//glRasterPos3f(xOffset,yStart,0);
+				//sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD);
+				//BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				//yStart += yIncr;
+
+				//glRasterPos3f(xOffset, yStart, 0);
+				//buf = String.format(%10.2f", ShootBoxInitialSpeed);
+				buf.setLength(0);
+				buf.append("+- shooting speed = ");
+				FastFormat.append(buf, ShootBoxInitialSpeed);
+				drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				//#ifdef SHOW_NUM_DEEP_PENETRATIONS
+				buf.setLength(0);
+				buf.append("gNumDeepPenetrationChecks = ");
+				FastFormat.append(buf, BulletGlobals.gNumDeepPenetrationChecks);
+				drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				buf.setLength(0);
+				buf.append("gNumGjkChecks = ");
+				FastFormat.append(buf, BulletGlobals.gNumGjkChecks);
+				drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+
+				//buf = String.format("gNumAlignedAllocs = %d", BulletGlobals.gNumAlignedAllocs);
+				// TODO: BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				//yStart += yIncr;
+
+				//buf = String.format("gNumAlignedFree= %d", BulletGlobals.gNumAlignedFree);
+				// TODO: BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				//yStart += yIncr;
+
+				//buf = String.format("# alloc-free = %d", BulletGlobals.gNumAlignedAllocs - BulletGlobals.gNumAlignedFree);
+				// TODO: BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				//yStart += yIncr;
+
+				//enable BT_DEBUG_MEMORY_ALLOCATIONS define in Bullet/src/LinearMath/btAlignedAllocator.h for memory leak detection
+				//#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
+				//glRasterPos3f(xOffset,yStart,0);
+				//sprintf(buf,"gTotalBytesAlignedAllocs = %d",gTotalBytesAlignedAllocs);
+				//BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+				//yStart += yIncr;
+				//#endif //BT_DEBUG_MEMORY_ALLOCATIONS
+
+				if (getDynamicsWorld() != null) {
+					buf.setLength(0);
+					buf.append("# objects = ");
+					FastFormat.append(buf, getDynamicsWorld().getNumCollisionObjects());
+					drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+					yStart += yIncr;
+					
+					buf.setLength(0);
+					buf.append("# pairs = ");
+					FastFormat.append(buf, getDynamicsWorld().getBroadphase().getOverlappingPairCache().getNumOverlappingPairs());
+					drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+					yStart += yIncr;
+
+				}
+				//#endif //SHOW_NUM_DEEP_PENETRATIONS
+
+				// JAVA NOTE: added
+				int free = (int)Runtime.getRuntime().freeMemory();
+				int total = (int)Runtime.getRuntime().totalMemory();
+				buf.setLength(0);
+				buf.append("heap = ");
+				FastFormat.append(buf, (float)(total - free) / (1024*1024));
+				buf.append(" / ");
+				FastFormat.append(buf, (float)(total) / (1024*1024));
+				buf.append(" MB");
+				drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR);
+				yStart += yIncr;
+				
+				resetPerspectiveProjection();
+			} */
+
+			// gl.glEnable(gl.GL_LIGHTING);
+		}
+	}
+	
+	public void clientResetScene() {
+		//#ifdef SHOW_NUM_DEEP_PENETRATIONS
+		BulletGlobals.gNumDeepPenetrationChecks = 0;
+		BulletGlobals.gNumGjkChecks = 0;
+		//#endif //SHOW_NUM_DEEP_PENETRATIONS
+
+		int numObjects = 0;
+		if (dynamicsWorld != null) {
+			dynamicsWorld.stepSimulation(1f / 60f, 0);
+			numObjects = dynamicsWorld.getNumCollisionObjects();
+		}
+
+		for (int i = 0; i < numObjects; i++) {
+			CollisionObject colObj = dynamicsWorld.getCollisionObjectArray().get(i);
+			RigidBody body = RigidBody.upcast(colObj);
+			if (body != null) {
+				if (body.getMotionState() != null) {
+					DefaultMotionState myMotionState = (DefaultMotionState) body.getMotionState();
+					myMotionState.graphicsWorldTrans.set(myMotionState.startWorldTrans);
+					colObj.setWorldTransform(myMotionState.graphicsWorldTrans);
+					colObj.setInterpolationWorldTransform(myMotionState.startWorldTrans);
+					colObj.activate();
+				}
+				// removed cached contact points
+				dynamicsWorld.getBroadphase().getOverlappingPairCache().cleanProxyFromPairs(colObj.getBroadphaseHandle(), getDynamicsWorld().getDispatcher());
+
+				body = RigidBody.upcast(colObj);
+				if (body != null && !body.isStaticObject()) {
+					RigidBody.upcast(colObj).setLinearVelocity(new Vector3f(0f, 0f, 0f));
+					RigidBody.upcast(colObj).setAngularVelocity(new Vector3f(0f, 0f, 0f));
+				}
+			}
+
+			/*
+			//quickly search some issue at a certain simulation frame, pressing space to reset
+			int fixed=18;
+			for (int i=0;i<fixed;i++)
+			{
+			getDynamicsWorld()->stepSimulation(1./60.f,1);
+			}
+			*/
+		}
+	}
+	
+	public DynamicsWorld getDynamicsWorld() {
+		return dynamicsWorld;
+	}
+
+	public void setCameraUp(Vector3f camUp) {
+		cameraUp.set(camUp);
+	}
+
+	public void setCameraForwardAxis(int axis) {
+		forwardAxis = axis;
+	}
+
+	public Vector3f getCameraPosition() {
+		return cameraPosition;
+	}
+
+	public Vector3f getCameraTargetPosition() {
+		return cameraTargetPosition;
+	}
+
+	public boolean isIdle() {
+		return idle;
+	}
+
+	public void setIdle(boolean idle) {
+		this.idle = idle;
+	}
+	
+	public void drawString(CharSequence s, int x, int y, Color3f color) {
+		glsrt.drawString(gl, s, x, y, color.x, color.y, color.z);
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/demos/opengl/FastFormat.java-nope b/src/jbullet/src/javabullet/demos/opengl/FastFormat.java-nope
new file mode 100644
index 0000000..d9311a2
--- /dev/null
+++ b/src/jbullet/src/javabullet/demos/opengl/FastFormat.java-nope
@@ -0,0 +1,62 @@
+/*
+ * 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.opengl;
+
+/**
+ *
+ * @author jezek2
+ */
+public class FastFormat {
+	
+	private static final char[] DIGITS = "0123456789".toCharArray();
+
+	public static void append(StringBuilder b, int i) {
+		if (i < 0) {
+			b.append('-');
+			i = Math.abs(i);
+		}
+		
+		int digit = 1000000000;
+		boolean first = true;
+		while (digit >= 1) {
+			int v = (i/digit);
+			if (v != 0 || !first) {
+				b.append(DIGITS[v]);
+				first = false;
+			}
+			i -= v*digit;
+			digit /= 10;
+		}
+		
+		if (first) b.append('0');
+	}
+
+	public static void append(StringBuilder b, float f) {
+		int val = Math.round(f*100f);
+		append(b, val / 100);
+		b.append('.');
+		append(b, Math.abs(val % 100));
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/demos/opengl/FontRender.java-nope b/src/jbullet/src/javabullet/demos/opengl/FontRender.java-nope
new file mode 100644
index 0000000..33468c6
--- /dev/null
+++ b/src/jbullet/src/javabullet/demos/opengl/FontRender.java-nope
@@ -0,0 +1,331 @@
+/*
+ * 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.opengl;
+
+import java.awt.*;
+import java.awt.color.ColorSpace;
+import java.awt.font.*;
+import java.awt.geom.Rectangle2D;
+import java.awt.image.*;
+import java.io.*;
+import java.nio.*;
+import java.util.Hashtable;
+
+import javax.media.opengl.*;
+import javax.media.opengl.glu.*;
+import com.sun.opengl.util.*;
+
+/**
+ *
+ * @author jezek2
+ */
+public class FontRender {
+	
+	//private static final File cacheDir = new File("/path/to/font/cache/dir/");
+	
+	private FontRender() {
+	}
+	
+	protected static class Glyph {
+		int x,y,w,h;		
+		int list = -1;
+	}
+	
+	public static class GLFont {
+		protected int texture;
+		protected int width, height;
+		protected Glyph[] glyphs = new Glyph[128-32];
+		protected GL gl;
+		
+		public GLFont(GL gl) {
+            this.gl=gl;
+			for (int i=0; i<glyphs.length; i++) glyphs[i] = new Glyph();
+		}
+		
+		public GLFont(GL gl, InputStream in) throws IOException {
+			this(gl);
+			load(in);
+		}
+
+		public void destroy() {
+			gl.glDeleteTextures(1, new int[] { texture }, 0);
+		}
+		
+		protected void save(File f) throws IOException {
+/*
+			DataOutputStream out = new DataOutputStream(new FileOutputStream(f));
+			out.writeInt(width);
+			out.writeInt(height);
+
+			gl.glPixelStorei(gl.GL_PACK_ROW_LENGTH, 0);
+			gl.glPixelStorei(gl.GL_PACK_ALIGNMENT, 1);
+			gl.glPixelStorei(gl.GL_PACK_SKIP_ROWS, 0);
+			gl.glPixelStorei(gl.GL_PACK_SKIP_PIXELS, 0);
+			
+			int size = width*height*4;
+			ByteBuffer buf = BufferUtils.createByteBuffer(size);
+			byte[] data = new byte[size];
+			glBindTexture(gl.GL_TEXTURE_2D, texture);
+			glGetTexImage(gl.GL_TEXTURE_2D, 0, gl.GL_RGBA, gl.GL_UNSIGNED_BYTE, (ByteBuffer)buf.position(0));
+			buf.get(data);
+			out.write(data);
+
+			for (int i=0; i<glyphs.length; i++) {
+				out.writeShort(glyphs[i].x);
+				out.writeShort(glyphs[i].y);
+				out.writeShort(glyphs[i].w);
+				out.writeShort(glyphs[i].h);
+			}
+			
+			out.close();
+*/
+		}
+
+		protected void load(File f) throws IOException {
+			load(new FileInputStream(f));
+		}
+		
+		protected void load(InputStream _in) throws IOException {
+			DataInputStream in = new DataInputStream(_in);
+			int w = in.readInt();
+			int h = in.readInt();
+			int size = w*h*4;
+			
+			gl.glPixelStorei(gl.GL_UNPACK_ROW_LENGTH, 0);
+			gl.glPixelStorei(gl.GL_UNPACK_ALIGNMENT, 1);
+			gl.glPixelStorei(gl.GL_UNPACK_SKIP_ROWS, 0);
+			gl.glPixelStorei(gl.GL_UNPACK_SKIP_PIXELS, 0);
+			
+			ByteBuffer buf = BufferUtil.newByteBuffer(size);
+			byte[] data = new byte[size];
+			in.read(data);
+			buf.put(data);
+
+			int[] id = new int[1];
+			gl.glGenTextures(1, id, 0);
+			texture = id[0];
+			width = w;
+			height = h;
+			
+			gl.glBindTexture(gl.GL_TEXTURE_2D, texture);
+			gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MIN_FILTER, gl.GL_LINEAR);
+			gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MAG_FILTER, gl.GL_LINEAR);
+			gl.glTexImage2D(gl.GL_TEXTURE_2D, 0, gl.GL_RGBA, w, h, 0, gl.GL_RGBA, gl.GL_UNSIGNED_BYTE, (ByteBuffer)buf.position(0));
+			
+			for (int i=0; i<glyphs.length; i++) {
+				glyphs[i].x = in.readShort();
+				glyphs[i].y = in.readShort();
+				glyphs[i].w = in.readShort();
+				glyphs[i].h = in.readShort();
+			}
+			
+			in.close();
+		}
+	}
+	
+	private static String getFontFileName(String family, int size, boolean bold) {
+		return family.replace(' ','_')+"_"+size+(bold? "_bold":"")+".fnt";
+	}
+	
+	public static GLFont createFont(GLU glu, GL gl, String family, int size, boolean bold, boolean antialiasing) throws IOException {
+		GLFont gf = new GLFont(gl);
+		/*File f = new File(cacheDir, getFontFileName(family, size, bold));
+		if (f.exists()) {
+			gf.load(f);
+			return gf;
+		}*/
+		
+		BufferedImage img = renderFont(new Font(family, bold? Font.BOLD : Font.PLAIN, size), antialiasing, gf.glyphs);
+		gf.texture = createTexture(glu, gl, img, false);
+		gf.width = img.getWidth();
+		gf.height = img.getHeight();
+		//gf.save(f);
+		return gf;
+	}
+	
+	public static BufferedImage renderFont(Font font, boolean antialiasing, Glyph[] glyphs) {
+		FontRenderContext frc = new FontRenderContext(null, antialiasing, false);
+		
+		int imgw = 256;
+		if (font.getSize() >= 36) imgw <<= 1;
+		if (font.getSize() >= 72) imgw <<= 1;
+		
+		//BufferedImage img = new BufferedImage(imgw, 1024, BufferedImage.TYPE_INT_ARGB);
+		BufferedImage img = createImage(imgw, 1024, true);
+		Graphics2D g = (Graphics2D)img.getGraphics();
+		
+		if (antialiasing) {
+			g.setRenderingHint(RenderingHints.KEY_TEXT_ANTIALIASING, RenderingHints.VALUE_TEXT_ANTIALIAS_ON);
+		}
+		
+		g.setColor(Color.WHITE);
+		g.setFont(font);
+		
+		int x=0, y=0,rowsize=0;
+		for (int c=32; c<128; c++) {
+			String s = ""+(char)c;
+			Rectangle2D rect = font.getStringBounds(s, frc);
+			LineMetrics lm = font.getLineMetrics(s, frc);
+			int w = (int)rect.getWidth()+1;
+			int h = (int)rect.getHeight()+2;
+
+			if (x+w+2 > img.getWidth()) {
+				x = 0;
+				y += rowsize;
+				rowsize = 0;
+			}
+			
+			g.drawString(s, x+1, y+(int)lm.getAscent()+1);
+			
+			if (glyphs != null) {
+				glyphs[c-32].x = x+1;
+				glyphs[c-32].y = y+1;
+				glyphs[c-32].w = w;
+				glyphs[c-32].h = h;
+			}
+			
+			w += 2;
+			h += 2;
+			
+			x += w;
+			rowsize = Math.max(rowsize, h);
+		}
+		
+		y += rowsize;
+		g.dispose();
+		
+		if (y < 128) img = img.getSubimage(0, 0, img.getWidth(), 128);
+		else if (y < 256) img = img.getSubimage(0, 0, img.getWidth(), 256);
+		else if (y < 512) img = img.getSubimage(0, 0, img.getWidth(), 512);
+		
+		return img;
+	}
+	
+	private static void renderGlyph(GL gl, GLFont font, Glyph g) {
+		if (g.list != -1) {
+			gl.glCallList(g.list);
+			return;
+		}
+		
+		g.list = gl.glGenLists(1);
+		gl.glNewList(g.list, gl.GL_COMPILE);
+		
+		float tw = font.width;
+		float th = font.height;
+		
+		int x=0, y=0;
+		
+		gl.glBegin(gl.GL_QUADS);
+			gl.glTexCoord2f((float)(g.x)/tw, (float)(g.y)/th);
+			gl.glVertex3f(x, y, 1);
+
+			gl.glTexCoord2f((float)(g.x+g.w-1)/tw, (float)(g.y)/th);
+			gl.glVertex3f(x+g.w-1, y, 1);
+
+			gl.glTexCoord2f((float)(g.x+g.w-1)/tw, (float)(g.y+g.h-1)/th);
+			gl.glVertex3f(x+g.w-1, y+g.h-1, 1);
+
+			gl.glTexCoord2f((float)(g.x)/tw, (float)(g.y+g.h-1)/th);
+			gl.glVertex3f(x, y+g.h-1, 1);
+		gl.glEnd();
+		
+		gl.glEndList();
+		gl.glCallList(g.list);
+	}
+
+	public static void drawString(GL gl, GLFont font, CharSequence s, int x, int y, float red, float green, float blue) {
+		drawString(gl, font, s, x, y, red, green, blue, 1);
+	}
+	
+	public static void drawString(GL gl, GLFont font, CharSequence s, int x, int y, float red, float green, float blue, float alpha) {
+		gl.glEnable(gl.GL_BLEND);
+		gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA);
+		
+		gl.glPushMatrix();
+		gl.glTranslatef(x, y, 0);
+		
+		gl.glBindTexture(gl.GL_TEXTURE_2D, font.texture);
+		gl.glEnable(gl.GL_TEXTURE_2D);
+		gl.glColor4f(red, green, blue, alpha);
+		//gl.glColor4f(1, 1, 1, 1);
+		for (int i=0, n=s.length(); i<n; i++) {
+			char c = s.charAt(i);
+			if (c < 32 || c > 128) c = '?';
+			Glyph g = font.glyphs[c-32];
+			renderGlyph(gl, font, g);
+			//x += g.w;
+			//glTranslatef(g.w, 0, 0);
+			gl.glTranslatef(g.w-2, 0, 0);
+		}
+		gl.glDisable(gl.GL_TEXTURE_2D);
+		
+		gl.glPopMatrix();
+
+		gl.glDisable(gl.GL_BLEND);
+	}
+	
+	private static ColorModel glColorModel = new ComponentColorModel(ColorSpace.getInstance(ColorSpace.CS_sRGB), new int[] {8,8,8,0}, false, false, ComponentColorModel.OPAQUE, DataBuffer.TYPE_BYTE);
+	private static ColorModel glColorModelAlpha = new ComponentColorModel(ColorSpace.getInstance(ColorSpace.CS_sRGB), new int[] {8,8,8,8}, true, false, ComponentColorModel.OPAQUE, DataBuffer.TYPE_BYTE);
+	
+	private static int createTexture(GLU glu, GL gl, BufferedImage img, boolean mipMap) {
+		boolean USE_COMPRESSION = false;
+
+		int[] id = new int[1];
+		gl.glGenTextures(1, id, 0);
+		int tex = id[0];
+		
+		gl.glBindTexture(gl.GL_TEXTURE_2D, tex);
+		gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MIN_FILTER, mipMap? gl.GL_LINEAR_MIPMAP_LINEAR : gl.GL_LINEAR);
+		gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MAG_FILTER, gl.GL_LINEAR);
+		
+		byte[] data = ((DataBufferByte)img.getRaster().getDataBuffer()).getData();
+		
+		ByteBuffer buf = ByteBuffer.allocateDirect(data.length);
+		buf.order(ByteOrder.nativeOrder());
+		buf.put(data, 0, data.length);
+		buf.flip();
+		
+		boolean alpha = img.getColorModel().hasAlpha();
+		
+		//gl.glTexImage2D(GL_TEXTURE_2D, 0, alpha? GL_RGBA:GL_RGB, img.getWidth(), img.getHeight(), 0, alpha? GL_RGBA:GL_RGB, GL_UNSIGNED_BYTE, buf);
+		gl.glTexImage2D(gl.GL_TEXTURE_2D, 0, USE_COMPRESSION? (alpha? gl.GL_COMPRESSED_RGBA:gl.GL_COMPRESSED_RGB) : (alpha? gl.GL_RGBA:gl.GL_RGB), img.getWidth(), img.getHeight(), 0, alpha? gl.GL_RGBA:gl.GL_RGB, gl.GL_UNSIGNED_BYTE, buf);
+		if (mipMap) {
+			glu.gluBuild2DMipmaps(gl.GL_TEXTURE_2D, USE_COMPRESSION? (alpha? gl.GL_COMPRESSED_RGBA:gl.GL_COMPRESSED_RGB) : (alpha? gl.GL_RGBA:gl.GL_RGB), img.getWidth(), img.getHeight(), alpha? gl.GL_RGBA:gl.GL_RGB, gl.GL_UNSIGNED_BYTE, buf);
+			//glu.gluBuild2DMipmaps(GL_TEXTURE_2D, GL_COMPRESSED_RGB, img.getWidth(), img.getHeight(), GL_RGB, GL_UNSIGNED_BYTE, buf);
+		}
+		
+		return tex;
+	}
+	
+	private static BufferedImage createImage(int width, int height, boolean alpha) {
+		if (alpha) {
+			WritableRaster raster = Raster.createInterleavedRaster(DataBuffer.TYPE_BYTE, width, height, 4, null);
+			return new BufferedImage(glColorModelAlpha, raster, false, new Hashtable());
+		}
+		
+		WritableRaster raster = Raster.createInterleavedRaster(DataBuffer.TYPE_BYTE, width, height, 3, null);
+		return new BufferedImage(glColorModel, raster, false, new Hashtable());
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/demos/opengl/GLSRT.java b/src/jbullet/src/javabullet/demos/opengl/GLSRT.java
new file mode 100644
index 0000000..cdd4aef
--- /dev/null
+++ b/src/jbullet/src/javabullet/demos/opengl/GLSRT.java
@@ -0,0 +1,293 @@
+/*
+ * 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.opengl;
+
+import java.net.URL;
+import java.io.IOException;
+import java.nio.FloatBuffer;
+import java.nio.ByteBuffer;
+import java.util.HashMap;
+import java.util.Map;
+// import javabullet.demos.opengl.FontRender.GLFont;
+import javax.media.opengl.*;
+import javax.media.opengl.glu.*;
+import javax.media.opengl.util.ImmModeSink;
+import javax.media.opengl.util.BufferUtil;
+
+/**
+ *
+ * @author jezek2
+ */
+public class GLSRT {
+
+    public static final boolean VBO_CACHE = true; // JAU
+    // public static final boolean VBO_CACHE = false;
+
+    /*static {
+        ImmModeSink.setVBOUsage(false);
+    }*/
+
+	private GLU    glu;
+	// private GLFont font;
+
+	public GLSRT(GLU glu, GL gl) {
+        System.out.println("VBO_CACHE: "+VBO_CACHE);
+        this.glu = glu;
+        /*
+		try {
+			font = new GLFont(gl, DemoApplication.class.getResourceAsStream("DejaVu_Sans_11.fnt"));
+            URL fontURL = DemoApplication.class.getResource("DejaVu_Sans_11.fnt");
+            if(fontURL!=null) {
+                font = new GLFont(gl, fontURL.openStream());
+            }
+		}
+		catch (IOException e) {
+			e.printStackTrace();
+		} */
+	}
+	
+    ImmModeSink vboCube = null;
+
+	public void drawCube(GL gl, float extent) {
+		extent = extent * 0.5f;
+		
+        if(vboCube==null) {
+            vboCube = ImmModeSink.createFixed(GL.GL_STATIC_DRAW, 24,
+                                  3, GL.GL_FLOAT,  // vertex
+                                  0, GL.GL_FLOAT,  // color
+                                  3, GL.GL_FLOAT,  // normal
+                                  0, GL.GL_FLOAT); // texture
+
+            vboCube.glBegin(GL.GL_TRIANGLES);
+            vboCube.glNormal3f( 0f, 0f, 1f); 
+            vboCube.glNormal3f( 0f, 0f, 1f); 
+            vboCube.glNormal3f( 0f, 0f, 1f); 
+            vboCube.glNormal3f( 0f, 0f, 1f); 
+            vboCube.glVertex3f(-extent,+extent,+extent); 
+            vboCube.glVertex3f(+extent,-extent,+extent); 
+            vboCube.glVertex3f(+extent,+extent,+extent); 
+            vboCube.glVertex3f(-extent,-extent,+extent);
+            vboCube.glNormal3f( 0f, 0f,-1f); 
+            vboCube.glNormal3f( 0f, 0f,-1f); 
+            vboCube.glNormal3f( 0f, 0f,-1f); 
+            vboCube.glNormal3f( 0f, 0f,-1f); 
+            vboCube.glVertex3f(-extent,+extent,-extent); 
+            vboCube.glVertex3f(+extent,-extent,-extent); 
+            vboCube.glVertex3f(+extent,+extent,-extent); 
+            vboCube.glVertex3f(+extent,+extent,+extent);
+            vboCube.glNormal3f( 0f, -1f, 0f); 
+            vboCube.glNormal3f( 0f, -1f, 0f); 
+            vboCube.glNormal3f( 0f, -1f, 0f); 
+            vboCube.glNormal3f( 0f, -1f, 0f); 
+            vboCube.glVertex3f(-extent,-extent,+extent); 
+            vboCube.glVertex3f(+extent,-extent,-extent); 
+            vboCube.glVertex3f(+extent,-extent,+extent); 
+            vboCube.glVertex3f(-extent,-extent,-extent);
+            vboCube.glNormal3f( 0f,1f, 0f); 
+            vboCube.glNormal3f( 0f,1f, 0f); 
+            vboCube.glNormal3f( 0f,1f, 0f); 
+            vboCube.glNormal3f( 0f,1f, 0f); 
+            vboCube.glVertex3f(-extent,+extent,+extent); 
+            vboCube.glVertex3f(+extent,+extent,-extent); 
+            vboCube.glVertex3f(+extent,+extent,+extent); 
+            vboCube.glVertex3f(-extent,+extent,-extent);
+            vboCube.glNormal3f( 1f,0f, 0f); 
+            vboCube.glNormal3f( 1f,0f, 0f); 
+            vboCube.glNormal3f( 1f,0f, 0f); 
+            vboCube.glNormal3f( 1f,0f, 0f); 
+            vboCube.glVertex3f(+extent,-extent,+extent); 
+            vboCube.glVertex3f(+extent,+extent,-extent); 
+            vboCube.glVertex3f(+extent,+extent,+extent); 
+            vboCube.glVertex3f(+extent,-extent,-extent);
+            vboCube.glNormal3f( -1f, 0f, 0f); 
+            vboCube.glNormal3f( -1f, 0f, 0f); 
+            vboCube.glNormal3f( -1f, 0f, 0f); 
+            vboCube.glNormal3f( -1f, 0f, 0f); 
+            vboCube.glVertex3f(-extent,-extent,+extent); 
+            vboCube.glVertex3f(-extent,+extent,-extent); 
+            vboCube.glVertex3f(-extent,+extent,+extent); 
+            vboCube.glVertex3f(-extent,-extent,-extent);
+            vboCube.glEnd(gl, false);
+        }
+        vboCube.draw(gl, cubeIndices, true);
+	}
+    private static final byte[] s_cubeIndices =
+        {
+            0, 3, 1, 2, 0, 1, /* front  */
+            6, 5, 4, 5, 7, 4, /* back   */
+            8, 11, 9, 10, 8, 9, /* top    */
+            15, 12, 13, 12, 14, 13, /* bottom */
+            16, 19, 17, 18, 16, 17, /* right  */
+            23, 20, 21, 20, 22, 21 /* left   */
+    };
+    private ByteBuffer cubeIndices=BufferUtil.newByteBuffer(s_cubeIndices);
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	private static GLUquadric cylinder=null;
+	private static GLUquadric sphere=null;
+	
+	private static class SphereKey {
+		public float radius;
+
+		public SphereKey() {
+		}
+
+		public SphereKey(SphereKey key) {
+			radius = key.radius;
+		}
+
+		@Override
+		public boolean equals(Object obj) {
+			if (obj == null || !(obj instanceof SphereKey)) return false;
+			SphereKey other = (SphereKey)obj;
+			return radius == other.radius;
+		}
+
+		@Override
+		public int hashCode() {
+			return Float.floatToIntBits(radius);
+		}
+	}
+	
+	private static Map<SphereKey,ImmModeSink> sphereDisplayLists = new HashMap<SphereKey,ImmModeSink>();
+	private static SphereKey sphereKey = new SphereKey();
+	
+	public void drawSphere(GL gl, float radius, int slices, int stacks) {
+        if(sphere==null) {
+            sphere = glu.gluNewQuadric();
+            sphere.setImmMode((VBO_CACHE)?false:true);
+        }
+		sphereKey.radius = radius;
+		ImmModeSink vbo = sphereDisplayLists.get(sphereKey);
+		if (vbo == null) {
+			glu.gluSphere(sphere, radius, 8, 8);
+            if(VBO_CACHE) {
+                vbo = sphere.replaceImmModeSink();
+                sphereDisplayLists.put(new SphereKey(sphereKey), vbo);
+            }
+		}
+		
+        if(VBO_CACHE && null!=vbo) {
+            vbo.draw(gl, true);
+        }
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+
+	
+	private static class CylinderKey {
+		public float radius;
+		public float halfHeight;
+
+		public CylinderKey() {
+		}
+
+		public CylinderKey(CylinderKey key) {
+			radius = key.radius;
+			halfHeight = key.halfHeight;
+		}
+
+		public void set(float radius, float halfHeight) {
+			this.radius = radius;
+			this.halfHeight = halfHeight;
+		}
+
+		@Override
+		public boolean equals(Object obj) {
+			if (obj == null || !(obj instanceof CylinderKey)) return false;
+			CylinderKey other = (CylinderKey) obj;
+			if (radius != other.radius) return false;
+			if (halfHeight != other.halfHeight) return false;
+			return true;
+		}
+
+		@Override
+		public int hashCode() {
+			int hash = 7;
+			hash = 23 * hash + Float.floatToIntBits(radius);
+			hash = 23 * hash + Float.floatToIntBits(halfHeight);
+			return hash;
+		}
+	}
+	
+	private static Map<CylinderKey,ImmModeSink> cylinderDisplayLists = new HashMap<CylinderKey,ImmModeSink>();
+	private static CylinderKey cylinderKey = new CylinderKey();
+	
+	public void drawCylinder(GL gl, float radius, float halfHeight, int upAxis) {
+        if(cylinder==null) {
+            cylinder = glu.gluNewQuadric();
+            cylinder.setImmMode((VBO_CACHE)?false:true);
+        }
+		gl.glPushMatrix();
+		switch (upAxis) {
+			case 0:
+				gl.glRotatef(-90f, 0.0f, 1.0f, 0.0f);
+				gl.glTranslatef(0.0f, 0.0f, -halfHeight);
+				break;
+			case 1:
+				gl.glRotatef(-90.0f, 1.0f, 0.0f, 0.0f);
+				gl.glTranslatef(0.0f, 0.0f, -halfHeight);
+				break;
+			case 2:
+				gl.glTranslatef(0.0f, 0.0f, -halfHeight);
+				break;
+			default: {
+				assert (false);
+			}
+		}
+
+		// The gluCylinder subroutine draws a cylinder that is oriented along the z axis. 
+		// The base of the cylinder is placed at z = 0; the top of the cylinder is placed at z=height. 
+		// Like a sphere, the cylinder is subdivided around the z axis into slices and along the z axis into stacks.
+
+		cylinderKey.set(radius, halfHeight);
+		ImmModeSink vbo = cylinderDisplayLists.get(cylinderKey);
+		if (vbo == null) {
+			glu.gluQuadricDrawStyle(cylinder, glu.GLU_FILL);
+			glu.gluQuadricNormals(cylinder, glu.GLU_SMOOTH);
+			glu.gluCylinder(cylinder, radius, radius, 2f * halfHeight, 15, 10);
+            if(VBO_CACHE) {
+                vbo = cylinder.replaceImmModeSink();
+                cylinderDisplayLists.put(new CylinderKey(cylinderKey), vbo);
+            }
+		}
+		
+        if(VBO_CACHE && null!=vbo) {
+            vbo.draw(gl, true);
+        }
+
+		gl.glPopMatrix();
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+
+	public void drawString(GL gl, CharSequence s, int x, int y, float red, float green, float blue) {
+        /*
+		if (font != null) {
+			FontRender.drawString(gl, font, s, x, y, red, green, blue);
+		} */
+	}
+
+}
diff --git a/src/jbullet/src/javabullet/demos/opengl/GLShapeDrawer.java b/src/jbullet/src/javabullet/demos/opengl/GLShapeDrawer.java
new file mode 100644
index 0000000..76fb9b1
--- /dev/null
+++ b/src/jbullet/src/javabullet/demos/opengl/GLShapeDrawer.java
@@ -0,0 +1,504 @@
+/*
+ * 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.opengl;
+
+import javabullet.BulletStack;
+import javabullet.collision.broadphase.BroadphaseNativeType;
+import javabullet.collision.shapes.BoxShape;
+import javabullet.collision.shapes.CapsuleShape;
+import javabullet.collision.shapes.CollisionShape;
+import javabullet.collision.shapes.CompoundShape;
+import javabullet.collision.shapes.ConcaveShape;
+import javabullet.collision.shapes.CylinderShape;
+import javabullet.collision.shapes.InternalTriangleIndexCallback;
+import javabullet.collision.shapes.PolyhedralConvexShape;
+import javabullet.collision.shapes.SphereShape;
+import javabullet.collision.shapes.TriangleCallback;
+import javabullet.linearmath.DebugDrawModes;
+import javabullet.linearmath.Transform;
+import javabullet.linearmath.VectorUtil;
+import javax.vecmath.Vector3f;
+import javax.media.opengl.*;
+import javax.media.opengl.util.ImmModeSink;
+
+/**
+ *
+ * @author jezek2
+ */
+public class GLShapeDrawer {
+
+	/*
+	private static Map<CollisionShape,TriMeshKey> g_display_lists = new HashMap<CollisionShape,TriMeshKey>();
+	
+	private static int OGL_get_displaylist_for_shape(CollisionShape shape) {
+		// JAVA NOTE: rewritten
+		TriMeshKey trimesh = g_display_lists.get(shape);
+		if (trimesh != null) {
+			return trimesh.dlist;
+		}
+
+		return 0;
+	}
+
+	private static void OGL_displaylist_clean() {
+		// JAVA NOTE: rewritten
+		for (TriMeshKey trimesh : g_display_lists.values()) {
+			glDeleteLists(trimesh.dlist, 1);
+		}
+
+		g_display_lists.clear();
+	}
+	*/
+
+	public static void drawCoordSystem(GL gl) {
+        ImmModeSink vbo = ImmModeSink.createFixed(GL.GL_STATIC_DRAW, 10,
+                              3, GL.GL_FLOAT,  // vertex
+                              4, GL.GL_FLOAT,  // color
+                              0, GL.GL_FLOAT,  // normal
+                              0, GL.GL_FLOAT); // texture
+		vbo.glBegin(gl.GL_LINES);
+		vbo.glColor4f ( 1f,  1f,  1f, 1f);
+		vbo.glVertex3f( 0f,  0f,  0f);
+		vbo.glColor4f ( 1f,  1f,  1f, 1f);
+		vbo.glVertex3f( 1f,  0f,  0f);
+		vbo.glColor4f ( 1f,  1f,  1f, 1f);
+		vbo.glVertex3f( 0f,  0f,  0f);
+		vbo.glColor4f ( 1f,  1f,  1f, 1f);
+		vbo.glVertex3f( 0f,  1f,  0f);
+		vbo.glColor4f ( 1f,  1f,  1f, 1f);
+		vbo.glVertex3f( 0f,  0f,  0f);
+		vbo.glColor4f ( 1f,  1f,  1f, 1f);
+		vbo.glVertex3f( 0f,  0f,  1f);
+		vbo.glEnd(gl);
+	}
+
+	private static float[] glMat = new float[16];
+	
+	public static void drawOpenGL(GLSRT glsrt, GL gl, Transform trans, CollisionShape shape, Vector3f color, int debugMode) {
+		BulletStack stack = BulletStack.get();
+		
+		stack.pushCommonMath();
+		try {
+			//System.out.println("shape="+shape+" type="+BroadphaseNativeTypes.forValue(shape.getShapeType()));
+
+			gl.glPushMatrix();
+			trans.getOpenGLMatrix(glMat);
+			gl.glMultMatrixf(glMat, 0);
+	//		if (shape.getShapeType() == BroadphaseNativeTypes.UNIFORM_SCALING_SHAPE_PROXYTYPE.getValue())
+	//		{
+	//			const btUniformScalingShape* scalingShape = static_cast<const btUniformScalingShape*>(shape);
+	//			const btConvexShape* convexShape = scalingShape->getChildShape();
+	//			float	scalingFactor = (float)scalingShape->getUniformScalingFactor();
+	//			{
+	//				btScalar tmpScaling[4][4]={{scalingFactor,0,0,0},
+	//					{0,scalingFactor,0,0},
+	//					{0,0,scalingFactor,0},
+	//					{0,0,0,1}};
+	//
+	//				drawOpenGL( (btScalar*)tmpScaling,convexShape,color,debugMode);
+	//			}
+	//			return;
+	//		}
+
+			if (shape.getShapeType() == BroadphaseNativeType.COMPOUND_SHAPE_PROXYTYPE) {
+				CompoundShape compoundShape = (CompoundShape) shape;
+				for (int i = compoundShape.getNumChildShapes() - 1; i >= 0; i--) {
+					Transform childTrans = stack.transforms.get(compoundShape.getChildTransform(i));
+					CollisionShape colShape = compoundShape.getChildShape(i);
+					drawOpenGL(glsrt, gl, childTrans, colShape, color, debugMode);
+				}
+			}
+			else {
+                gl.glEnable(gl.GL_COLOR_MATERIAL);
+                gl.glColor4f(color.x, color.y, color.z, 1f);
+
+				boolean useWireframeFallback = true;
+
+				if ( (debugMode & DebugDrawModes.DRAW_WIREFRAME) == 0) {
+					switch (shape.getShapeType()) {
+						case BOX_SHAPE_PROXYTYPE: {
+							BoxShape boxShape = (BoxShape) shape;
+							Vector3f halfExtent = stack.vectors.get(boxShape.getHalfExtentsWithMargin());
+							gl.glScalef(2f * halfExtent.x, 2f * halfExtent.y, 2f * halfExtent.z);
+							glsrt.drawCube(gl, 1f);
+							useWireframeFallback = false;
+							break;
+						}
+						case TRIANGLE_SHAPE_PROXYTYPE:
+						case TETRAHEDRAL_SHAPE_PROXYTYPE: {
+							//todo:	
+							//					useWireframeFallback = false;
+							break;
+						}
+						case CONVEX_HULL_SHAPE_PROXYTYPE:
+							break;
+						case SPHERE_SHAPE_PROXYTYPE: {
+							SphereShape sphereShape = (SphereShape) shape;
+							float radius = sphereShape.getMargin(); // radius doesn't include the margin, so draw with margin
+							// TODO: glutSolidSphere(radius,10,10);
+							//sphere.draw(radius, 8, 8);
+							glsrt.drawSphere(gl, radius, 10, 10);
+							/*
+							glPointSize(10f);
+							glBegin(gl.GL_POINTS);
+							glVertex3f(0f, 0f, 0f);
+							glEnd();
+							glPointSize(1f);
+							*/
+							useWireframeFallback = false;
+							break;
+						}
+						case CAPSULE_SHAPE_PROXYTYPE:
+						{
+							CapsuleShape capsuleShape = (CapsuleShape)shape;
+							float radius = capsuleShape.getRadius();
+							float halfHeight = capsuleShape.getHalfHeight();
+							int upAxis = 1;
+
+							glsrt.drawCylinder(gl, radius,halfHeight,upAxis);
+
+							gl.glTranslatef(0f, -halfHeight, 0f);
+							//glutSolidSphere(radius,10,10);
+							//sphere.draw(radius, 10, 10);
+							glsrt.drawSphere(gl, radius, 10, 10);
+							gl.glTranslatef(0f, 2f*halfHeight,0f);
+							//glutSolidSphere(radius,10,10);
+							//sphere.draw(radius, 10, 10);
+							glsrt.drawSphere(gl, radius, 10, 10);
+							useWireframeFallback = false;
+							break;
+						}
+						case MULTI_SPHERE_SHAPE_PROXYTYPE: {
+							break;
+						}
+	//				case CONE_SHAPE_PROXYTYPE:
+	//					{
+	//						const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
+	//						int upIndex = coneShape->getConeUpIndex();
+	//						float radius = coneShape->getRadius();//+coneShape->getMargin();
+	//						float height = coneShape->getHeight();//+coneShape->getMargin();
+	//						switch (upIndex)
+	//						{
+	//						case 0:
+	//							glRotatef(90.0, 0.0, 1.0, 0.0);
+	//							break;
+	//						case 1:
+	//							glRotatef(-90.0, 1.0, 0.0, 0.0);
+	//							break;
+	//						case 2:
+	//							break;
+	//						default:
+	//							{
+	//							}
+	//						};
+	//
+	//						glTranslatef(0.0, 0.0, -0.5*height);
+	//						glutSolidCone(radius,height,10,10);
+	//						useWireframeFallback = false;
+	//						break;
+	//
+	//					}
+						case CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE: {
+							useWireframeFallback = false;
+							break;
+						}
+
+					case CONVEX_SHAPE_PROXYTYPE:
+					case CYLINDER_SHAPE_PROXYTYPE:
+						{
+							CylinderShape cylinder = (CylinderShape) shape;
+							int upAxis = cylinder.getUpAxis();
+
+							float radius = cylinder.getRadius();
+							float halfHeight = VectorUtil.getCoord(cylinder.getHalfExtentsWithMargin(), upAxis);
+
+							glsrt.drawCylinder(gl, radius, halfHeight, upAxis);
+
+							break;
+						}
+						default: {
+						}
+
+					}
+
+				}
+
+				if (useWireframeFallback) {
+					// for polyhedral shapes
+					if (shape.isPolyhedral()) {
+						PolyhedralConvexShape polyshape = (PolyhedralConvexShape) shape;
+
+                        ImmModeSink vbo = ImmModeSink.createFixed(GL.GL_STATIC_DRAW, polyshape.getNumEdges()+3,
+                                              3, GL.GL_FLOAT,  // vertex
+                                              0, GL.GL_FLOAT,  // color
+                                              0, GL.GL_FLOAT,  // normal
+                                              0, GL.GL_FLOAT); // texture
+
+						vbo.glBegin(gl.GL_LINES);
+
+						Vector3f a = stack.vectors.get(), b = stack.vectors.get();
+						int i;
+						for (i = 0; i < polyshape.getNumEdges(); i++) {
+							polyshape.getEdge(i, a, b);
+
+							vbo.glVertex3f(a.x, a.y, a.z);
+							vbo.glVertex3f(b.x, b.y, b.z);
+						}
+						vbo.glEnd(gl);
+
+	//					if (debugMode==btIDebugDraw::DBG_DrawFeaturesText)
+	//					{
+	//						glRasterPos3f(0.0,  0.0,  0.0);
+	//						//BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->getExtraDebugInfo());
+	//
+	//						glColor3f(1.f, 1.f, 1.f);
+	//						for (i=0;i<polyshape->getNumVertices();i++)
+	//						{
+	//							btPoint3 vtx;
+	//							polyshape->getVertex(i,vtx);
+	//							glRasterPos3f(vtx.x(),  vtx.y(),  vtx.z());
+	//							char buf[12];
+	//							sprintf(buf," %d",i);
+	//							BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+	//						}
+	//
+	//						for (i=0;i<polyshape->getNumPlanes();i++)
+	//						{
+	//							btVector3 normal;
+	//							btPoint3 vtx;
+	//							polyshape->getPlane(normal,vtx,i);
+	//							btScalar d = vtx.dot(normal);
+	//
+	//							glRasterPos3f(normal.x()*d,  normal.y()*d, normal.z()*d);
+	//							char buf[12];
+	//							sprintf(buf," plane %d",i);
+	//							BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
+	//
+	//						}
+	//					}
+
+
+					}
+				}
+
+	//		#ifdef USE_DISPLAY_LISTS
+	//
+	//		if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE||shape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
+	//			{
+	//				GLuint dlist =   OGL_get_displaylist_for_shape((btCollisionShape * )shape);
+	//				if (dlist)
+	//				{
+	//					glCallList(dlist);
+	//				}
+	//				else
+	//				{
+	//		#else		
+				if (shape.isConcave())//>getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE||shape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
+				//		if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
+				{
+					ConcaveShape concaveMesh = (ConcaveShape) shape;
+					//btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
+					//btVector3 aabbMax(100,100,100);//btScalar(1e30),btScalar(1e30),btScalar(1e30));
+
+					//todo pass camera, for some culling
+					Vector3f aabbMax = stack.vectors.get(1e30f, 1e30f, 1e30f);
+					Vector3f aabbMin = stack.vectors.get(-1e30f, -1e30f, -1e30f);
+
+					GlDrawcallback drawCallback = new GlDrawcallback(gl);
+					drawCallback.wireframe = (debugMode & DebugDrawModes.DRAW_WIREFRAME) != 0;
+
+					concaveMesh.processAllTriangles(drawCallback, aabbMin, aabbMax);
+				}
+				//#endif
+
+				//#ifdef USE_DISPLAY_LISTS
+				//		}
+				//	}
+				//#endif
+
+	//			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));
+	//				TriangleGlDrawcallback drawCallback;
+	//				convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
+	//
+	//			}
+
+				// TODO: error in original sources GL_DEPTH_BUFFER_BIT instead of GL_DEPTH_TEST
+				//gl.glDisable(GL_DEPTH_TEST);
+				//glRasterPos3f(0, 0, 0);//mvtx.x(),  vtx.y(),  vtx.z());
+				if ((debugMode & DebugDrawModes.DRAW_TEXT) != 0) {
+					// TODO: BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->getName());
+				}
+
+				if ((debugMode & DebugDrawModes.DRAW_FEATURES_TEXT) != 0) {
+					//BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->getExtraDebugInfo());
+				}
+				//gl.glEnable(GL_DEPTH_TEST);
+
+			}
+		}
+		finally {
+			gl.glPopMatrix();
+			stack.popCommonMath();
+		}
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	private static class TriMeshKey {
+		public CollisionShape shape;
+		public int dlist; // OpenGL display list	
+	}
+	
+	private static class GlDisplaylistDrawcallback implements TriangleCallback {
+		private GL gl;
+		
+		private final Vector3f diff1 = new Vector3f();
+		private final Vector3f diff2 = new Vector3f();
+		private final Vector3f normal = new Vector3f();
+
+		public GlDisplaylistDrawcallback(GL gl) {
+			this.gl = gl;
+		}
+		
+		public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) {
+			diff1.sub(triangle[1], triangle[0]);
+			diff2.sub(triangle[2], triangle[0]);
+			normal.cross(diff1, diff2);
+
+			normal.normalize();
+
+            ImmModeSink vbo = ImmModeSink.createFixed(GL.GL_STATIC_DRAW, 3,
+                                  3, GL.GL_FLOAT,  // vertex
+                                  4, GL.GL_FLOAT,  // color
+                                  3, GL.GL_FLOAT,  // normal
+                                  0, GL.GL_FLOAT); // texture
+
+			vbo.glBegin(gl.GL_TRIANGLES);
+			vbo.glColor4f(0, 1f, 0, 1f);
+			vbo.glNormal3f(normal.x, normal.y, normal.z);
+			vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z);
+
+			vbo.glColor4f(0, 1f, 0, 1f);
+			vbo.glNormal3f(normal.x, normal.y, normal.z);
+			vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z);
+
+			vbo.glColor4f(0, 1f, 0, 1f);
+			vbo.glNormal3f(normal.x, normal.y, normal.z);
+			vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z);
+			vbo.glEnd(gl);
+
+			/*glBegin(gl.GL_LINES);
+			glColor3f(1, 1, 0);
+			glNormal3d(normal.getX(),normal.getY(),normal.getZ());
+			glVertex3d(triangle[0].getX(), triangle[0].getY(), triangle[0].getZ());
+			glNormal3d(normal.getX(),normal.getY(),normal.getZ());
+			glVertex3d(triangle[1].getX(), triangle[1].getY(), triangle[1].getZ());
+			glColor3f(1, 1, 0);
+			glNormal3d(normal.getX(),normal.getY(),normal.getZ());
+			glVertex3d(triangle[2].getX(), triangle[2].getY(), triangle[2].getZ());
+			glNormal3d(normal.getX(),normal.getY(),normal.getZ());
+			glVertex3d(triangle[1].getX(), triangle[1].getY(), triangle[1].getZ());
+			glColor3f(1, 1, 0);
+			glNormal3d(normal.getX(),normal.getY(),normal.getZ());
+			glVertex3d(triangle[2].getX(), triangle[2].getY(), triangle[2].getZ());
+			glNormal3d(normal.getX(),normal.getY(),normal.getZ());
+			glVertex3d(triangle[0].getX(), triangle[0].getY(), triangle[0].getZ());
+			glEnd();*/
+		}
+	}
+	
+	private static class GlDrawcallback implements TriangleCallback {
+		private GL gl;
+		public boolean wireframe = false;
+
+		public GlDrawcallback(GL gl) {
+			this.gl = gl;
+		}
+		
+		public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) {
+            ImmModeSink vbo = ImmModeSink.createFixed(GL.GL_STATIC_DRAW, 10,
+                                  3, GL.GL_FLOAT,  // vertex
+                                  4, GL.GL_FLOAT,  // color
+                                  0, GL.GL_FLOAT,  // normal
+                                  0, GL.GL_FLOAT); // texture
+			if (wireframe) {
+				vbo.glBegin(gl.GL_LINES);
+				vbo.glColor4f(1, 0, 0, 1);
+				vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z);
+				vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z);
+				vbo.glColor4f(0, 1, 0, 1);
+				vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z);
+				vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z);
+				vbo.glColor4f(0, 0, 1, 1);
+				vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z);
+				vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z);
+				vbo.glEnd(gl);
+			}
+			else {
+				vbo.glBegin(gl.GL_TRIANGLES);
+				vbo.glColor4f(1, 0, 0, 1);
+				vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z);
+				vbo.glColor4f(0, 1, 0, 1);
+				vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z);
+				vbo.glColor4f(0, 0, 1, 1);
+				vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z);
+				vbo.glEnd(gl);
+			}
+		}
+	}
+	
+	private static class TriangleGlDrawcallback implements InternalTriangleIndexCallback {
+		private GL gl;
+
+		public TriangleGlDrawcallback(GL gl) {
+			this.gl = gl;
+		}
+		
+		public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) {
+            ImmModeSink vbo = ImmModeSink.createFixed(GL.GL_STATIC_DRAW, 10,
+                                  3, GL.GL_FLOAT,  // vertex
+                                  4, GL.GL_FLOAT,  // color
+                                  0, GL.GL_FLOAT,  // normal
+                                  0, GL.GL_FLOAT); // texture
+			vbo.glBegin(gl.GL_TRIANGLES);//LINES);
+			vbo.glColor4f(1, 0, 0, 1);
+			vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z);
+			vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z);
+			vbo.glColor4f(0, 1, 0, 1);
+			vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z);
+			vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z);
+			vbo.glColor4f(0, 0, 1, 1);
+			vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z);
+			vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z);
+			vbo.glEnd(gl);
+		}
+	}
+	
+}
diff --git a/src/jbullet/src/javabullet/demos/opengl/JOGL.java b/src/jbullet/src/javabullet/demos/opengl/JOGL.java
new file mode 100644
index 0000000..1e3408e
--- /dev/null
+++ b/src/jbullet/src/javabullet/demos/opengl/JOGL.java
@@ -0,0 +1,149 @@
+/*
+ * 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.opengl;
+
+import com.sun.javafx.newt.*;
+import javax.media.nativewindow.*;
+import javax.media.opengl.*;
+import javax.media.opengl.glu.*;
+import javax.media.opengl.util.*;
+import java.nio.*;
+
+/**
+ *
+ * @author jezek2
+ */
+
+public class JOGL implements WindowListener, MouseListener {
+	
+    private GLWindow window;
+    public boolean quit = false;
+
+    public void windowResized(WindowEvent e) { }
+    public void windowMoved(WindowEvent e) { }
+    public void windowDestroyNotify(WindowEvent e) {
+        quit = true;
+    }
+
+    public void mouseClicked(MouseEvent e) {
+        //if(e.getClickCount()>1) {
+            quit=true;
+        //}
+    }
+    public void mouseEntered(MouseEvent e) {
+    }
+    public void mouseExited(MouseEvent e) {
+    }
+    public void mousePressed(MouseEvent e) {
+    }
+    public void mouseReleased(MouseEvent e) {
+    }
+    public void mouseMoved(MouseEvent e) {
+    }
+    public void mouseDragged(MouseEvent e) {
+    }
+
+	private static boolean redisplay = false;
+	
+	public static void postRedisplay() {
+		redisplay = true;
+	}
+
+    private void run(String title, DemoApplication demoApp, int type, int width, int height) {
+        System.err.print(title);
+        System.err.println(" run()");
+        GLProfile.setProfileGLAny();
+        try {
+            NWCapabilities caps = new NWCapabilities();
+            // For emulation library, use 16 bpp
+            caps.setRedBits(5);
+            caps.setGreenBits(6);
+            caps.setBlueBits(5);
+            caps.setDepthBits(16);
+
+            Window nWindow = null;
+            if(0!=(type&USE_AWT)) {
+                Display nDisplay = NewtFactory.createDisplay(NativeWindowFactory.TYPE_AWT, null); // local display
+                Screen nScreen  = NewtFactory.createScreen(NativeWindowFactory.TYPE_AWT, nDisplay, 0); // screen 0
+                nWindow = NewtFactory.createWindow(NativeWindowFactory.TYPE_AWT, nScreen, caps);
+            }
+            window = GLWindow.create(nWindow, caps);
+
+            window.addWindowListener(this);
+            window.addMouseListener(this);
+            window.addMouseListener(demoApp);
+            window.addKeyListener(demoApp);
+            window.addGLEventListener(demoApp);
+            // window.setEventHandlerMode(GLWindow.EVENT_HANDLER_GL_CURRENT); // default
+            // window.setEventHandlerMode(GLWindow.EVENT_HANDLER_GL_NONE); // no current ..
+
+            window.enablePerfLog(true);
+            // Size OpenGL to Video Surface
+            window.setSize(width, height);
+            window.setFullscreen(true);
+            window.setVisible(true);
+            width = window.getWidth();
+            height = window.getHeight();
+
+            while (!quit && window.getDuration() < 200000) {
+                window.display();
+            }
+
+            // Shut things down cooperatively
+            window.destroy();
+            window.getFactory().shutdown();
+            System.out.print(title);
+            System.out.println(" shut down cleanly.");
+        } catch (Throwable t) {
+            t.printStackTrace();
+        }
+    }
+
+    public static int USE_NEWT      = 0;
+    public static int USE_AWT       = 1 << 0;
+
+    public static void main(String title, DemoApplication demo, String[] args) {
+        int width = 480;
+        int height = 800;
+        int type = USE_NEWT ;
+        for(int i=args.length-1; i>=0; i--) {
+            if(args[i].equals("-awt")) {
+                type |= USE_AWT; 
+            }
+            if(args[i].equals("-width") && i+1<args.length) {
+                try {
+                    width = Integer.parseInt(args[i+1]);
+                } catch (NumberFormatException nfe) {}
+            }
+            if(args[i].equals("-height") && i+1<args.length) {
+                try {
+                    height = Integer.parseInt(args[i+1]);
+                } catch (NumberFormatException nfe) {}
+            }
+        }
+        new JOGL().run(title, demo, type, width, height);
+        System.exit(0);
+    }
+
+}
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/package-info.java b/src/jbullet/src/javabullet/package-info.java
new file mode 100644
index 0000000..d7a4cc1
--- /dev/null
+++ b/src/jbullet/src/javabullet/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.
+ */
+
+/**
+ * Contains globals, like variables, constants and object pools.
+ */
+package javabullet;
+
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;
+