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

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

126 files changed, 18109 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..5341071
--- /dev/null
+++ b/src/jbullet/make-all.sh
@@ -0,0 +1,5 @@
+#! /bin/sh
+
+sh make-cdc-es1.sh 
+sh make-jar.sh 
+cp -v ragdoll.jar /data/Incoming/phone/ragdoll.jar 
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..996efa7
--- /dev/null
+++ b/src/jbullet/make-jar.sh
@@ -0,0 +1,8 @@
+rm -f ragdoll.jar 
+cd build/classes/
+jar xf ../../lib/vecmath/vecmath.jar 
+jar xf ../../lib/trove/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..c61a4cd
--- /dev/null
+++ b/src/jbullet/setenv-jbullet.sh
@@ -0,0 +1,30 @@
+#! /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 
+
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..5de2626
--- /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() {
+		super();
+	}
+
+	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();
+		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..cfd1e23
--- /dev/null
+++ b/src/jbullet/src/javabullet/demos/opengl/DemoApplication.java
@@ -0,0 +1,1147 @@
+/*
+ * 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;
+
+	public DemoApplication() {
+        // debugMode |= DebugDrawModes.DRAW_WIREFRAME;
+        debugMode |= DebugDrawModes.NO_HELP_TEXT;
+        //debugMode |= DebugDrawModes.DISABLE_BULLET_LCP;
+	}
+	
+	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().getGL2ES1();
+        glu = GLU.createGLU();
+        glsrt = new GLSRT(glu, gl);
+		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);
+
+		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);
+
+		gl.glEnable(gl.GL_LIGHTING);
+		gl.glEnable(gl.GL_LIGHT0);
+		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 reshape(GLAutoDrawable drawable, int x, int y, int width, int height) {
+        gl = drawable.getGL().getGL2ES1();
+		glutScreenWidth = width;
+		glutScreenHeight = height;
+
+		gl.glViewport(x, y, width, height);
+		updateCamera();
+
+        System.out.println("DemoApplication RESHAPE");
+	}
+
+    public void display(GLAutoDrawable drawable) {
+        gl = drawable.getGL().getGL2ES1();
+
+        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 GL2ES1  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);
+		}
+		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());
+				}
+
+				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;
+					}
+				}
+
+				GLShapeDrawer.drawOpenGL(glsrt, gl, m, colObj.getCollisionShape(), wireColor, getDebugMode());
+			}
+
+			float xOffset = 10f;
+			float yStart = 20f;
+			float yIncr = 20f;
+
+			gl.glDisable(gl.GL_LIGHTING);
+			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..6487664
--- /dev/null
+++ b/src/jbullet/src/javabullet/demos/opengl/GLSRT.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.demos.opengl;
+
+import java.net.URL;
+import java.io.IOException;
+import java.nio.FloatBuffer;
+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;
+
+/**
+ *
+ * @author jezek2
+ */
+public class GLSRT {
+
+    public static final boolean VBO_CACHE = true;
+
+	private GLU    glu;
+	// private GLFont font;
+
+	public GLSRT(GLU glu, GL2ES1 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(GL2ES1 gl, float extent) {
+		extent = extent * 0.5f;
+		
+        if(vboCube==null) {
+            vboCube = new ImmModeSink(GL.GL_FLOAT, GL.GL_STATIC_DRAW, 3, 3, 0, 0, 24);
+
+            vboCube.glBegin(ImmModeSink.GL_QUADS);
+            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( 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, 0f, 1f); 
+            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.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.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.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, true);
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	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(GL2ES1 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(GL2ES1 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(GL2ES1 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..a89ea91
--- /dev/null
+++ b/src/jbullet/src/javabullet/demos/opengl/GLShapeDrawer.java
@@ -0,0 +1,489 @@
+/*
+ * 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(GL2ES1 gl) {
+        ImmModeSink vbo = new ImmModeSink(gl.GL_FLOAT, gl.GL_STATIC_DRAW, 3, 0, 3, 0, 10);
+		vbo.glBegin(gl.GL_LINES);
+		vbo.glColor3f(1, 0, 0);
+		vbo.glVertex3f(0, 0, 0);
+		vbo.glVertex3f(1, 0, 0);
+		vbo.glColor3f(0, 1, 0);
+		vbo.glVertex3f(0, 0, 0);
+		vbo.glVertex3f(0, 1, 0);
+		vbo.glColor3f(0, 0, 1);
+		vbo.glVertex3f(0, 0, 0);
+		vbo.glVertex3f(0, 0, 1);
+		vbo.glEnd(gl);
+	}
+
+	private static float[] glMat = new float[16];
+	
+	public static void drawOpenGL(GLSRT glsrt, GL2ES1 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);
+	//			}
+	//			glPopMatrix();
+	//			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 {
+				//drawCoordSystem();
+
+				//glPushMatrix();
+
+				gl.glEnable(gl.GL_COLOR_MATERIAL);
+				gl.glColor4f(color.x, color.y, color.z, 0f);
+
+				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.glPushMatrix();
+							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);
+							gl.glPopMatrix();
+							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 = new ImmModeSink(gl.GL_FLOAT, gl.GL_STATIC_DRAW, 3, 0, 0, 0, polyshape.getNumEdges());
+
+						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);
+
+				//glPopMatrix();
+			}
+			gl.glPopMatrix();
+		}
+		finally {
+			stack.popCommonMath();
+		}
+	}
+	
+	////////////////////////////////////////////////////////////////////////////
+	
+	private static class TriMeshKey {
+		public CollisionShape shape;
+		public int dlist; // OpenGL display list	
+	}
+	
+	private static class GlDisplaylistDrawcallback implements TriangleCallback {
+		private GL2ES1 gl;
+		
+		private final Vector3f diff1 = new Vector3f();
+		private final Vector3f diff2 = new Vector3f();
+		private final Vector3f normal = new Vector3f();
+
+		public GlDisplaylistDrawcallback(GL2ES1 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 = new ImmModeSink(gl.GL_FLOAT, gl.GL_STATIC_DRAW, 3, 3, 3, 0, 3);
+
+			vbo.glBegin(gl.GL_TRIANGLES);
+			vbo.glColor3f(0, 1, 0);
+			vbo.glNormal3f(normal.x, normal.y, normal.z);
+			vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z);
+
+			vbo.glColor3f(0, 1, 0);
+			vbo.glNormal3f(normal.x, normal.y, normal.z);
+			vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z);
+
+			vbo.glColor3f(0, 1, 0);
+			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 GL2ES1 gl;
+		public boolean wireframe = false;
+
+		public GlDrawcallback(GL2ES1 gl) {
+			this.gl = gl;
+		}
+		
+		public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) {
+            ImmModeSink vbo = new ImmModeSink(gl.GL_FLOAT, gl.GL_STATIC_DRAW, 3, 0, 3, 0, 10);
+			if (wireframe) {
+				vbo.glBegin(gl.GL_LINES);
+				vbo.glColor3f(1, 0, 0);
+				vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z);
+				vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z);
+				vbo.glColor3f(0, 1, 0);
+				vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z);
+				vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z);
+				vbo.glColor3f(0, 0, 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.glColor3f(1, 0, 0);
+				vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z);
+				vbo.glColor3f(0, 1, 0);
+				vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z);
+				vbo.glColor3f(0, 0, 1);
+				vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z);
+				vbo.glEnd(gl);
+			}
+		}
+	}
+	
+	private static class TriangleGlDrawcallback implements InternalTriangleIndexCallback {
+		private GL2ES1 gl;
+
+		public TriangleGlDrawcallback(GL2ES1 gl) {
+			this.gl = gl;
+		}
+		
+		public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) {
+            ImmModeSink vbo = new ImmModeSink(gl.GL_FLOAT, gl.GL_STATIC_DRAW, 3, 0, 3, 0, 10);
+			vbo.glBegin(gl.GL_TRIANGLES);//LINES);
+			vbo.glColor3f(1, 0, 0);
+			vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z);
+			vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z);
+			vbo.glColor3f(0, 1, 0);
+			vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z);
+			vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z);
+			vbo.glColor3f(0, 0, 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..08d9b6b
--- /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.opengl.*;
+import javax.media.opengl.glu.*;
+import javax.media.opengl.util.*;
+import java.nio.*;
+
+/**
+ *
+ * @author jezek2
+ */
+
+public class JOGL implements MouseListener {
+	
+    private GLWindow window;
+    public boolean quit = false;
+
+    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 = 480;
+        int height = 800;
+        System.err.println(title+"run()");
+        GLProfile.setProfileGL2ES1();
+        try {
+            Window nWindow = null;
+            if(0!=(type&USE_AWT)) {
+                Display nDisplay = NewtFactory.createDisplay(NewtFactory.AWT, null); // local display
+                Screen nScreen  = NewtFactory.createScreen(NewtFactory.AWT, nDisplay, 0); // screen 0
+                nWindow = NewtFactory.createWindow(NewtFactory.AWT, nScreen, 0); // dummy VisualID
+            }
+
+            GLCapabilities caps = new GLCapabilities();
+            // For emulation library, use 16 bpp
+            caps.setRedBits(5);
+            caps.setGreenBits(6);
+            caps.setBlueBits(5);
+            caps.setDepthBits(16);
+            window = GLWindow.create(nWindow, caps);
+
+            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 ..
+
+            // Size OpenGL to Video Surface
+            window.setSize(width, height);
+            window.setFullscreen(true);
+            window.setVisible(true);
+            width = window.getWidth();
+            height = window.getHeight();
+
+            long startTime = System.currentTimeMillis();
+            long lastTime = startTime, curTime = 0, dt0, dt1;
+            int totalFrames = 0, lastFrames = 0;
+
+            while (!quit) {
+                window.display();
+
+                totalFrames++; lastFrames++;
+                curTime = System.currentTimeMillis();
+                dt0 = curTime-lastTime;
+                if ( (curTime-lastTime) > 5000 ) {
+                    dt1 = curTime-startTime;
+                    StringBuffer sb = new StringBuffer();
+                    sb.append(dt1/1000);
+                    sb.append("s, 5s: ");
+                    sb.append((lastFrames*1000)/dt0);
+                    sb.append(" fps, total: ");
+                    sb.append((totalFrames*1000)/dt1);
+                    sb.append(" fps");
+                    System.out.println(sb);
+                    lastTime=curTime;
+                    lastFrames=0;
+                }
+            }
+
+            // Shut things down cooperatively
+            window.close();
+            window.getFactory().shutdown();
+            System.out.println(title+" 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 type = USE_NEWT ;
+        for(int i=args.length-1; i>=0; i--) {
+            if(args[i].equals("-awt")) {
+                type |= USE_AWT; 
+            }
+        }
+        new JOGL().run(title, demo, type);
+        System.exit(0);
+    }
+
+}
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;
+