diff options
Diffstat (limited to 'src/jbullet')
182 files changed, 28851 insertions, 0 deletions
diff --git a/src/jbullet/build.xml b/src/jbullet/build.xml new file mode 100644 index 0000000..5cac11e --- /dev/null +++ b/src/jbullet/build.xml @@ -0,0 +1,108 @@ +<?xml version="1.0" encoding="UTF-8"?> +<!-- You may freely edit this file. See commented blocks below for --> +<!-- some examples of how to customize the build. --> +<!-- (If you delete it and reopen the project it will be recreated.) --> +<project name="javabullet" default="default" basedir="."> + <description>Builds, tests, and runs the project javabullet.</description> + <import file="nbproject/build-impl.xml"/> + + <import file="nbproject/profiler-build-impl.xml"/> <!-- + + There exist several targets which are by default empty and which can be + used for execution of your tasks. These targets are usually executed + before and after some main targets. They are: + + -pre-init: called before initialization of project properties + -post-init: called after initialization of project properties + -pre-compile: called before javac compilation + -post-compile: called after javac compilation + -pre-compile-single: called before javac compilation of single file + -post-compile-single: called after javac compilation of single file + -pre-compile-test: called before javac compilation of JUnit tests + -post-compile-test: called after javac compilation of JUnit tests + -pre-compile-test-single: called before javac compilation of single JUnit test + -post-compile-test-single: called after javac compilation of single JUunit test + -pre-jar: called before JAR building + -post-jar: called after JAR building + -post-clean: called after cleaning build products + + (Targets beginning with '-' are not intended to be called on their own.) + + Example of inserting an obfuscator after compilation could look like this: + + <target name="-post-compile"> + <obfuscate> + <fileset dir="${build.classes.dir}"/> + </obfuscate> + </target> + + For list of available properties check the imported + nbproject/build-impl.xml file. + + + Another way to customize the build is by overriding existing main targets. + The targets of interest are: + + -init-macrodef-javac: defines macro for javac compilation + -init-macrodef-junit: defines macro for junit execution + -init-macrodef-debug: defines macro for class debugging + -init-macrodef-java: defines macro for class execution + -do-jar-with-manifest: JAR building (if you are using a manifest) + -do-jar-without-manifest: JAR building (if you are not using a manifest) + run: execution of project + -javadoc-build: Javadoc generation + test-report: JUnit report generation + + An example of overriding the target for project execution could look like this: + + <target name="run" depends="javabullet-impl.jar"> + <exec dir="bin" executable="launcher.exe"> + <arg file="${dist.jar}"/> + </exec> + </target> + + Notice that the overridden target depends on the jar target and not only on + the compile target as the regular run target does. Again, for a list of available + properties which you can use, check the target you are overriding in the + nbproject/build-impl.xml file. + + --> + + <target name="-do-jar-without-manifest"> + <!-- jbullet.jar --> + <jar destfile="dist/jbullet.jar" basedir="build/classes" excludes="**/*.java,**/*.form,javabullet/demos/**"/> + + <!-- jbullet-demos.jar --> + <jar destfile="dist/jbullet-demos.jar" basedir="build/classes" includes="javabullet/demos/**" excludes="**/*.java,**/*.form,**/*.diff"/> + </target> + + <target name="webstart" depends="jar"> + <mkdir dir="dist/webstart"/> + <copy todir="dist/webstart"> + <fileset file="dist/jbullet.jar"/> + <fileset file="dist/jbullet-demos.jar"/> + <fileset dir="lib/vecmath" includes="**/*.jar"/> + <fileset dir="lib/jogl" includes="**/*.jar"/> + </copy> + + <input message="keyPass" addproperty="keypass"/> + <input message="storePass" addproperty="storepass" defaultvalue="${keypass}"/> + + <signjar keystore="keystore" alias="jezek2" keypass="${keypass}" storepass="${storepass}"> + <fileset dir="dist/webstart"> + <include name="*.jar"/> + </fileset> + </signjar> + </target> + + <target name="applet" depends="jar"> + <!-- jbullet-applet.jar --> + <jar destfile="dist/jbullet-applet.jar"> + <zipfileset src="dist/jbullet.jar" excludes="META-INF/**" filemode="644" dirmode="755"/> + <zipfileset src="dist/jbullet-demos.jar" excludes="META-INF/**" filemode="644" dirmode="755"/> + <zipfileset src="lib/vecmath/vecmath.jar" excludes="META-INF/**" filemode="644" dirmode="755"/> + <zipfileset src="lib/swing-layout/swing-layout-1.0.3.jar" excludes="META-INF/**" filemode="644" dirmode="755"/> + </jar> + </target> + +</project> diff --git a/src/jbullet/changelog.txt b/src/jbullet/changelog.txt new file mode 100644 index 0000000..7c8d30c --- /dev/null +++ b/src/jbullet/changelog.txt @@ -0,0 +1,46 @@ +Release 20080311: +- Added some JavaDoc documentation +- Added RaycastVehicle and VehicleDemo +- Refactored accessing of vertex data +- Added CylinderShape +- Implemented ray/trimesh hit detection +- Added applet demo +- Added binaries and dependant libraries into package + +Release 20080303: +- Refactored enums +- Fixed bug that caused occasional jitter +- Added ConvexConcaveCollisionAlgorithm +- Memory optimalizations +- Implemented quantized BVH nodes +- Made ConcaveDemo working + +Release 20080206: +- Memory optimalizations +- Added heap info +- Implemented HeapSort +- Added optional support for GNU Trove +- Added BspDemo and fixed ConvexHullShape +- Added ConcaveDemo and it's supporting classes +- Abstracted OpenGL rendering + +Release 20080122: +- Fixed convex/plane collision detection +- Added GLDebugDrawer and fixed some bugs +- Added CapsuleShape +- Added ConeTwistConstraint, HingeConstraint and Generic6DofConstraint +- Added GenericJointDemo +- Optimized drawing of spheres and cylinders using display lists +- Fixed collision of boxes +- Added text overlay + +Release 20080116: +- Moved all push/popProfile to try/finally blocks +- Added final for Vectors/Transforms/etc fields where applicable, and fixed some discovered bugs +- Fixed bug with non-functional removeOverlappingPair +- Enabled ground BoxShape in BasicDemo +- Implemented drawing of BoxShape +- Fixed VectorUtil.maxAxis + +Release 20080111: +- Initial release based on Bullet 2.66 diff --git a/src/jbullet/lib/java/lang/Enum.class b/src/jbullet/lib/java/lang/Enum.class Binary files differnew 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 Binary files differnew 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 Binary files differnew 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 Binary files differnew file mode 100644 index 0000000..ddfd73b --- /dev/null +++ b/src/jbullet/lib/vecmath.jar diff --git a/src/jbullet/make-all.sh b/src/jbullet/make-all.sh new file mode 100644 index 0000000..2c4ca3e --- /dev/null +++ b/src/jbullet/make-all.sh @@ -0,0 +1,5 @@ +#! /bin/sh + +sh make-cdc-es1.sh jar +sh make-jar.sh +cp -v ragdoll.jar /jordan/data/packs/sun/mobile/apx2500.phone_image diff --git a/src/jbullet/make-cdc-es1.sh b/src/jbullet/make-cdc-es1.sh new file mode 100644 index 0000000..204b6b9 --- /dev/null +++ b/src/jbullet/make-cdc-es1.sh @@ -0,0 +1,7 @@ +#! /bin/sh + +# -Djavac.bootclasspath.jar=$(pwd)/../../../gluegen/make/lib/cdc_fp.jar \ +#ant -Djavac.source=1.5 -Djavac.bootclasspath.jar=/usr/local/projects/SUN/JOGL2/gluegen/make/lib/cdc_fp.jar 2>&1 | tee make-cdc-es1.log +ant -v \ + -Djogl.home=$JOGL_HOME \ + $* 2>&1 | tee make-cdc-es1.log diff --git a/src/jbullet/make-jar.sh b/src/jbullet/make-jar.sh new file mode 100644 index 0000000..c991a61 --- /dev/null +++ b/src/jbullet/make-jar.sh @@ -0,0 +1,8 @@ +rm -f ragdoll.jar +cd build/classes/ +jar xf ../../lib/vecmath.jar +#jar xf ../../lib/trove.jar +cp -a ../../lib/java . +rm -rf META-INF +find . -name \*nope -exec rm -fv \{\} \; +jar cf ../../ragdoll.jar . diff --git a/src/jbullet/nbproject/build-impl.xml b/src/jbullet/nbproject/build-impl.xml new file mode 100644 index 0000000..70cc4e9 --- /dev/null +++ b/src/jbullet/nbproject/build-impl.xml @@ -0,0 +1,650 @@ +<?xml version="1.0" encoding="UTF-8"?> +<!-- +*** GENERATED FROM project.xml - DO NOT EDIT *** +*** EDIT ../build.xml INSTEAD *** + +For the purpose of easier reading the script +is divided into following sections: + + - initialization + - compilation + - jar + - execution + - debugging + - javadoc + - junit compilation + - junit execution + - junit debugging + - applet + - cleanup + + --> +<project xmlns:j2seproject1="http://www.netbeans.org/ns/j2se-project/1" xmlns:j2seproject3="http://www.netbeans.org/ns/j2se-project/3" xmlns:jaxrpc="http://www.netbeans.org/ns/j2se-project/jax-rpc" basedir=".." default="default" name="jbullet-impl"> + <target depends="test,jar,javadoc" description="Build and test whole project." name="default"/> + <!-- + ====================== + INITIALIZATION SECTION + ====================== + --> + <target name="-pre-init"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target depends="-pre-init" name="-init-private"> + <property file="nbproject/private/config.properties"/> + <property file="nbproject/private/configs/${config}.properties"/> + <property file="nbproject/private/private.properties"/> + </target> + <target depends="-pre-init,-init-private" name="-init-libraries"> + <property location="lib/nblibraries.properties" name="libraries.1.path"/> + <dirname file="${libraries.1.path}" property="libraries.1.dir.nativedirsep"/> + <pathconvert dirsep="/" property="libraries.1.dir"> + <path path="${libraries.1.dir.nativedirsep}"/> + </pathconvert> + <basename file="${libraries.1.path}" property="libraries.1.basename" suffix=".properties"/> + <touch file="${libraries.1.dir}/${libraries.1.basename}-private.properties"/> + <loadproperties srcfile="${libraries.1.dir}/${libraries.1.basename}-private.properties"> + <filterchain> + <replacestring from="$${base}" to="${libraries.1.dir}"/> + </filterchain> + </loadproperties> + <loadproperties srcfile="${libraries.1.path}"> + <filterchain> + <replacestring from="$${base}" to="${libraries.1.dir}"/> + </filterchain> + </loadproperties> + </target> + <target depends="-pre-init,-init-private,-init-libraries" name="-init-user"> + <property file="${user.properties.file}"/> + <!-- The two properties below are usually overridden --> + <!-- by the active platform. Just a fallback. --> + <property name="default.javac.source" value="1.4"/> + <property name="default.javac.target" value="1.4"/> + </target> + <target depends="-pre-init,-init-private,-init-libraries,-init-user" name="-init-project"> + <property file="nbproject/configs/${config}.properties"/> + <property file="nbproject/project.properties"/> + </target> + <target depends="-pre-init,-init-private,-init-libraries,-init-user,-init-project,-init-macrodef-property" name="-do-init"> + <available file="${manifest.file}" property="manifest.available"/> + <condition property="manifest.available+main.class"> + <and> + <isset property="manifest.available"/> + <isset property="main.class"/> + <not> + <equals arg1="${main.class}" arg2="" trim="true"/> + </not> + </and> + </condition> + <condition property="manifest.available+main.class+mkdist.available"> + <and> + <istrue value="${manifest.available+main.class}"/> + <isset property="libs.CopyLibs.classpath"/> + </and> + </condition> + <condition property="have.tests"> + <or> + <available file="${test.src.dir}"/> + </or> + </condition> + <condition property="have.sources"> + <or> + <available file="${src.dir}"/> + </or> + </condition> + <condition property="netbeans.home+have.tests"> + <and> + <isset property="netbeans.home"/> + <isset property="have.tests"/> + </and> + </condition> + <condition property="no.javadoc.preview"> + <and> + <isset property="javadoc.preview"/> + <isfalse value="${javadoc.preview}"/> + </and> + </condition> + <property name="run.jvmargs" value=""/> + <condition property="javac.compilerargs" value="-bootclasspath ${javac.bootclasspath}"> + <isset property="javac.bootclasspath"/> + </condition> + <property name="work.dir" value="${basedir}"/> + <condition property="no.deps"> + <and> + <istrue value="${no.dependencies}"/> + </and> + </condition> + <property name="javac.debug" value="true"/> + <property name="javadoc.preview" value="true"/> + <property name="application.args" value=""/> + <property name="source.encoding" value="${file.encoding}"/> + <condition property="javadoc.encoding.used" value="${javadoc.encoding}"> + <and> + <isset property="javadoc.encoding"/> + <not> + <equals arg1="${javadoc.encoding}" arg2=""/> + </not> + </and> + </condition> + <property name="javadoc.encoding.used" value="${source.encoding}"/> + <property name="includes" value="**"/> + <property name="excludes" value=""/> + <property name="do.depend" value="false"/> + <condition property="do.depend.true"> + <istrue value="${do.depend}"/> + </condition> + <condition else="" property="javac.compilerargs.jaxws" value="-Djava.endorsed.dirs='${jaxws.endorsed.dir}'"> + <and> + <isset property="jaxws.endorsed.dir"/> + <available file="nbproject/jaxws-build.xml"/> + </and> + </condition> + </target> + <target name="-post-init"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target depends="-pre-init,-init-private,-init-libraries,-init-user,-init-project,-do-init" name="-init-check"> + <fail unless="src.dir">Must set src.dir</fail> + <fail unless="test.src.dir">Must set test.src.dir</fail> + <fail unless="build.dir">Must set build.dir</fail> + <fail unless="dist.dir">Must set dist.dir</fail> + <fail unless="build.classes.dir">Must set build.classes.dir</fail> + <fail unless="dist.javadoc.dir">Must set dist.javadoc.dir</fail> + <fail unless="build.test.classes.dir">Must set build.test.classes.dir</fail> + <fail unless="build.test.results.dir">Must set build.test.results.dir</fail> + <fail unless="build.classes.excludes">Must set build.classes.excludes</fail> + <fail unless="dist.jar">Must set dist.jar</fail> + </target> + <target name="-init-macrodef-property"> + <macrodef name="property" uri="http://www.netbeans.org/ns/j2se-project/1"> + <attribute name="name"/> + <attribute name="value"/> + <sequential> + <property name="@{name}" value="${@{value}}"/> + </sequential> + </macrodef> + </target> + <target name="-init-macrodef-javac"> + <macrodef name="javac" uri="http://www.netbeans.org/ns/j2se-project/3"> + <attribute default="${src.dir}" name="srcdir"/> + <attribute default="${build.classes.dir}" name="destdir"/> + <attribute default="${javac.classpath}" name="classpath"/> + <attribute default="${includes}" name="includes"/> + <attribute default="${excludes}" name="excludes"/> + <attribute default="${javac.debug}" name="debug"/> + <attribute default="" name="sourcepath"/> + <element name="customize" optional="true"/> + <sequential> + <javac debug="@{debug}" deprecation="${javac.deprecation}" destdir="@{destdir}" encoding="${source.encoding}" excludes="@{excludes}" includeantruntime="false" includes="@{includes}" source="${javac.source}" sourcepath="@{sourcepath}" srcdir="@{srcdir}" target="${javac.target}" bootclasspath="${javac.bootclasspath.jar}"> + <classpath> + <path path="@{classpath}"/> + </classpath> + <compilerarg line="${javac.compilerargs} ${javac.compilerargs.jaxws}"/> + <customize/> + </javac> + </sequential> + </macrodef> + <macrodef name="depend" uri="http://www.netbeans.org/ns/j2se-project/3"> + <attribute default="${src.dir}" name="srcdir"/> + <attribute default="${build.classes.dir}" name="destdir"/> + <attribute default="${javac.classpath}" name="classpath"/> + <sequential> + <depend cache="${build.dir}/depcache" destdir="@{destdir}" excludes="${excludes}" includes="${includes}" srcdir="@{srcdir}"> + <classpath> + <path path="@{classpath}"/> + </classpath> + </depend> + </sequential> + </macrodef> + <macrodef name="force-recompile" uri="http://www.netbeans.org/ns/j2se-project/3"> + <attribute default="${build.classes.dir}" name="destdir"/> + <sequential> + <fail unless="javac.includes">Must set javac.includes</fail> + <pathconvert pathsep="," property="javac.includes.binary"> + <path> + <filelist dir="@{destdir}" files="${javac.includes}"/> + </path> + <globmapper from="*.java" to="*.class"/> + </pathconvert> + <delete> + <files includes="${javac.includes.binary}"/> + </delete> + </sequential> + </macrodef> + </target> + <target name="-init-macrodef-junit"> + <macrodef name="junit" uri="http://www.netbeans.org/ns/j2se-project/3"> + <attribute default="${includes}" name="includes"/> + <attribute default="${excludes}" name="excludes"/> + <attribute default="**" name="testincludes"/> + <sequential> + <junit dir="${work.dir}" errorproperty="tests.failed" failureproperty="tests.failed" fork="true" showoutput="true"> + <batchtest todir="${build.test.results.dir}"> + <fileset dir="${test.src.dir}" excludes="@{excludes},${excludes}" includes="@{includes}"> + <filename name="@{testincludes}"/> + </fileset> + </batchtest> + <classpath> + <path path="${run.test.classpath}"/> + </classpath> + <syspropertyset> + <propertyref prefix="test-sys-prop."/> + <mapper from="test-sys-prop.*" to="*" type="glob"/> + </syspropertyset> + <formatter type="brief" usefile="false"/> + <formatter type="xml"/> + <jvmarg line="${run.jvmargs}"/> + </junit> + </sequential> + </macrodef> + </target> + <target name="-init-macrodef-nbjpda"> + <macrodef name="nbjpdastart" uri="http://www.netbeans.org/ns/j2se-project/1"> + <attribute default="${main.class}" name="name"/> + <attribute default="${debug.classpath}" name="classpath"/> + <attribute default="" name="stopclassname"/> + <sequential> + <nbjpdastart addressproperty="jpda.address" name="@{name}" stopclassname="@{stopclassname}" transport="dt_socket"> + <classpath> + <path path="@{classpath}"/> + </classpath> + </nbjpdastart> + </sequential> + </macrodef> + <macrodef name="nbjpdareload" uri="http://www.netbeans.org/ns/j2se-project/1"> + <attribute default="${build.classes.dir}" name="dir"/> + <sequential> + <nbjpdareload> + <fileset dir="@{dir}" includes="${fix.classes}"> + <include name="${fix.includes}*.class"/> + </fileset> + </nbjpdareload> + </sequential> + </macrodef> + </target> + <target name="-init-debug-args"> + <property name="version-output" value="java version "${ant.java.version}"/> + <condition property="have-jdk-older-than-1.4"> + <or> + <contains string="${version-output}" substring="java version "1.0"/> + <contains string="${version-output}" substring="java version "1.1"/> + <contains string="${version-output}" substring="java version "1.2"/> + <contains string="${version-output}" substring="java version "1.3"/> + </or> + </condition> + <condition else="-Xdebug" property="debug-args-line" value="-Xdebug -Xnoagent -Djava.compiler=none"> + <istrue value="${have-jdk-older-than-1.4}"/> + </condition> + </target> + <target depends="-init-debug-args" name="-init-macrodef-debug"> + <macrodef name="debug" uri="http://www.netbeans.org/ns/j2se-project/3"> + <attribute default="${main.class}" name="classname"/> + <attribute default="${debug.classpath}" name="classpath"/> + <element name="customize" optional="true"/> + <sequential> + <java classname="@{classname}" dir="${work.dir}" fork="true"> + <jvmarg line="${debug-args-line}"/> + <jvmarg value="-Xrunjdwp:transport=dt_socket,address=${jpda.address}"/> + <jvmarg line="${run.jvmargs}"/> + <classpath> + <path path="@{classpath}"/> + </classpath> + <syspropertyset> + <propertyref prefix="run-sys-prop."/> + <mapper from="run-sys-prop.*" to="*" type="glob"/> + </syspropertyset> + <customize/> + </java> + </sequential> + </macrodef> + </target> + <target name="-init-macrodef-java"> + <macrodef name="java" uri="http://www.netbeans.org/ns/j2se-project/1"> + <attribute default="${main.class}" name="classname"/> + <element name="customize" optional="true"/> + <sequential> + <java classname="@{classname}" dir="${work.dir}" fork="true"> + <jvmarg line="${run.jvmargs}"/> + <classpath> + <path path="${run.classpath}"/> + </classpath> + <syspropertyset> + <propertyref prefix="run-sys-prop."/> + <mapper from="run-sys-prop.*" to="*" type="glob"/> + </syspropertyset> + <customize/> + </java> + </sequential> + </macrodef> + </target> + <target name="-init-presetdef-jar"> + <presetdef name="jar" uri="http://www.netbeans.org/ns/j2se-project/1"> + <jar compress="${jar.compress}" jarfile="${dist.jar}"> + <j2seproject1:fileset dir="${build.classes.dir}"/> + </jar> + </presetdef> + </target> + <target depends="-pre-init,-init-private,-init-libraries,-init-user,-init-project,-do-init,-post-init,-init-check,-init-macrodef-property,-init-macrodef-javac,-init-macrodef-junit,-init-macrodef-nbjpda,-init-macrodef-debug,-init-macrodef-java,-init-presetdef-jar" name="init"/> + <!-- + =================== + COMPILATION SECTION + =================== + --> + <target depends="init" name="deps-jar" unless="no.deps"/> + <target depends="init,deps-jar" name="-pre-pre-compile"> + <mkdir dir="${build.classes.dir}"/> + </target> + <target name="-pre-compile"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target if="do.depend.true" name="-compile-depend"> + <j2seproject3:depend/> + </target> + <target depends="init,deps-jar,-pre-pre-compile,-pre-compile,-compile-depend" if="have.sources" name="-do-compile"> + <j2seproject3:javac/> + <copy todir="${build.classes.dir}"> + <fileset dir="${src.dir}" excludes="${build.classes.excludes},${excludes}" includes="${includes}"/> + </copy> + </target> + <target name="-post-compile"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target depends="init,deps-jar,-pre-pre-compile,-pre-compile,-do-compile,-post-compile" description="Compile project." name="compile"/> + <target name="-pre-compile-single"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target depends="init,deps-jar,-pre-pre-compile" name="-do-compile-single"> + <fail unless="javac.includes">Must select some files in the IDE or set javac.includes</fail> + <j2seproject3:force-recompile/> + <j2seproject3:javac excludes="" includes="${javac.includes}" sourcepath="${src.dir}"/> + </target> + <target name="-post-compile-single"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target depends="init,deps-jar,-pre-pre-compile,-pre-compile-single,-do-compile-single,-post-compile-single" name="compile-single"/> + <!-- + ==================== + JAR BUILDING SECTION + ==================== + --> + <target depends="init" name="-pre-pre-jar"> + <dirname file="${dist.jar}" property="dist.jar.dir"/> + <mkdir dir="${dist.jar.dir}"/> + </target> + <target name="-pre-jar"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target depends="init,compile,-pre-pre-jar,-pre-jar" name="-do-jar-without-manifest" unless="manifest.available"> + <j2seproject1:jar/> + </target> + <target depends="init,compile,-pre-pre-jar,-pre-jar" if="manifest.available" name="-do-jar-with-manifest" unless="manifest.available+main.class"> + <j2seproject1:jar manifest="${manifest.file}"/> + </target> + <target depends="init,compile,-pre-pre-jar,-pre-jar" if="manifest.available+main.class" name="-do-jar-with-mainclass" unless="manifest.available+main.class+mkdist.available"> + <j2seproject1:jar manifest="${manifest.file}"> + <j2seproject1:manifest> + <j2seproject1:attribute name="Main-Class" value="${main.class}"/> + </j2seproject1:manifest> + </j2seproject1:jar> + <echo>To run this application from the command line without Ant, try:</echo> + <property location="${build.classes.dir}" name="build.classes.dir.resolved"/> + <property location="${dist.jar}" name="dist.jar.resolved"/> + <pathconvert property="run.classpath.with.dist.jar"> + <path path="${run.classpath}"/> + <map from="${build.classes.dir.resolved}" to="${dist.jar.resolved}"/> + </pathconvert> + <echo>java -cp "${run.classpath.with.dist.jar}" ${main.class}</echo> + </target> + <target depends="init,compile,-pre-pre-jar,-pre-jar" if="manifest.available+main.class+mkdist.available" name="-do-jar-with-libraries"> + <property location="${build.classes.dir}" name="build.classes.dir.resolved"/> + <pathconvert property="run.classpath.without.build.classes.dir"> + <path path="${run.classpath}"/> + <map from="${build.classes.dir.resolved}" to=""/> + </pathconvert> + <pathconvert pathsep=" " property="jar.classpath"> + <path path="${run.classpath.without.build.classes.dir}"/> + <chainedmapper> + <flattenmapper/> + <globmapper from="*" to="lib/*"/> + </chainedmapper> + </pathconvert> + <taskdef classname="org.netbeans.modules.java.j2seproject.copylibstask.CopyLibs" classpath="${libs.CopyLibs.classpath}" name="copylibs"/> + <copylibs compress="${jar.compress}" jarfile="${dist.jar}" manifest="${manifest.file}" runtimeclasspath="${run.classpath.without.build.classes.dir}"> + <fileset dir="${build.classes.dir}"/> + <manifest> + <attribute name="Main-Class" value="${main.class}"/> + <attribute name="Class-Path" value="${jar.classpath}"/> + </manifest> + </copylibs> + <echo>To run this application from the command line without Ant, try:</echo> + <property location="${dist.jar}" name="dist.jar.resolved"/> + <echo>java -jar "${dist.jar.resolved}"</echo> + </target> + <target name="-post-jar"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target depends="init,compile,-pre-jar,-do-jar-with-manifest,-do-jar-without-manifest,-do-jar-with-mainclass,-do-jar-with-libraries,-post-jar" description="Build JAR." name="jar"/> + <!-- + ================= + EXECUTION SECTION + ================= + --> + <target depends="init,compile" description="Run a main class." name="run"> + <j2seproject1:java> + <customize> + <arg line="${application.args}"/> + </customize> + </j2seproject1:java> + </target> + <target name="-do-not-recompile"> + <property name="javac.includes.binary" value=""/> + </target> + <target depends="init,-do-not-recompile,compile-single" name="run-single"> + <fail unless="run.class">Must select one file in the IDE or set run.class</fail> + <j2seproject1:java classname="${run.class}"/> + </target> + <!-- + ================= + DEBUGGING SECTION + ================= + --> + <target depends="init" if="netbeans.home" name="-debug-start-debugger"> + <j2seproject1:nbjpdastart name="${debug.class}"/> + </target> + <target depends="init,compile" name="-debug-start-debuggee"> + <j2seproject3:debug> + <customize> + <arg line="${application.args}"/> + </customize> + </j2seproject3:debug> + </target> + <target depends="init,compile,-debug-start-debugger,-debug-start-debuggee" description="Debug project in IDE." if="netbeans.home" name="debug"/> + <target depends="init" if="netbeans.home" name="-debug-start-debugger-stepinto"> + <j2seproject1:nbjpdastart stopclassname="${main.class}"/> + </target> + <target depends="init,compile,-debug-start-debugger-stepinto,-debug-start-debuggee" if="netbeans.home" name="debug-stepinto"/> + <target depends="init,compile-single" if="netbeans.home" name="-debug-start-debuggee-single"> + <fail unless="debug.class">Must select one file in the IDE or set debug.class</fail> + <j2seproject3:debug classname="${debug.class}"/> + </target> + <target depends="init,-do-not-recompile,compile-single,-debug-start-debugger,-debug-start-debuggee-single" if="netbeans.home" name="debug-single"/> + <target depends="init" name="-pre-debug-fix"> + <fail unless="fix.includes">Must set fix.includes</fail> + <property name="javac.includes" value="${fix.includes}.java"/> + </target> + <target depends="init,-pre-debug-fix,compile-single" if="netbeans.home" name="-do-debug-fix"> + <j2seproject1:nbjpdareload/> + </target> + <target depends="init,-pre-debug-fix,-do-debug-fix" if="netbeans.home" name="debug-fix"/> + <!-- + =============== + JAVADOC SECTION + =============== + --> + <target depends="init" name="-javadoc-build"> + <mkdir dir="${dist.javadoc.dir}"/> + <javadoc additionalparam="${javadoc.additionalparam}" author="${javadoc.author}" charset="UTF-8" destdir="${dist.javadoc.dir}" docencoding="UTF-8" encoding="${javadoc.encoding.used}" failonerror="true" noindex="${javadoc.noindex}" nonavbar="${javadoc.nonavbar}" notree="${javadoc.notree}" private="${javadoc.private}" source="${javac.source}" splitindex="${javadoc.splitindex}" use="${javadoc.use}" useexternalfile="true" version="${javadoc.version}" windowtitle="${javadoc.windowtitle}"> + <classpath> + <path path="${javac.classpath}"/> + </classpath> + <fileset dir="${src.dir}" excludes="${excludes}" includes="${includes}"> + <filename name="**/*.java"/> + </fileset> + </javadoc> + </target> + <target depends="init,-javadoc-build" if="netbeans.home" name="-javadoc-browse" unless="no.javadoc.preview"> + <nbbrowse file="${dist.javadoc.dir}/index.html"/> + </target> + <target depends="init,-javadoc-build,-javadoc-browse" description="Build Javadoc." name="javadoc"/> + <!-- + ========================= + JUNIT COMPILATION SECTION + ========================= + --> + <target depends="init,compile" if="have.tests" name="-pre-pre-compile-test"> + <mkdir dir="${build.test.classes.dir}"/> + </target> + <target name="-pre-compile-test"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target if="do.depend.true" name="-compile-test-depend"> + <j2seproject3:depend classpath="${javac.test.classpath}" destdir="${build.test.classes.dir}" srcdir="${test.src.dir}"/> + </target> + <target depends="init,compile,-pre-pre-compile-test,-pre-compile-test,-compile-test-depend" if="have.tests" name="-do-compile-test"> + <j2seproject3:javac classpath="${javac.test.classpath}" debug="true" destdir="${build.test.classes.dir}" srcdir="${test.src.dir}"/> + <copy todir="${build.test.classes.dir}"> + <fileset dir="${test.src.dir}" excludes="${build.classes.excludes},${excludes}" includes="${includes}"/> + </copy> + </target> + <target name="-post-compile-test"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target depends="init,compile,-pre-pre-compile-test,-pre-compile-test,-do-compile-test,-post-compile-test" name="compile-test"/> + <target name="-pre-compile-test-single"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target depends="init,compile,-pre-pre-compile-test,-pre-compile-test-single" if="have.tests" name="-do-compile-test-single"> + <fail unless="javac.includes">Must select some files in the IDE or set javac.includes</fail> + <j2seproject3:force-recompile destdir="${build.test.classes.dir}"/> + <j2seproject3:javac classpath="${javac.test.classpath}" debug="true" destdir="${build.test.classes.dir}" excludes="" includes="${javac.includes}" sourcepath="${test.src.dir}" srcdir="${test.src.dir}"/> + <copy todir="${build.test.classes.dir}"> + <fileset dir="${test.src.dir}" excludes="${build.classes.excludes},${excludes}" includes="${includes}"/> + </copy> + </target> + <target name="-post-compile-test-single"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target depends="init,compile,-pre-pre-compile-test,-pre-compile-test-single,-do-compile-test-single,-post-compile-test-single" name="compile-test-single"/> + <!-- + ======================= + JUNIT EXECUTION SECTION + ======================= + --> + <target depends="init" if="have.tests" name="-pre-test-run"> + <mkdir dir="${build.test.results.dir}"/> + </target> + <target depends="init,compile-test,-pre-test-run" if="have.tests" name="-do-test-run"> + <j2seproject3:junit testincludes="**/*Test.java"/> + </target> + <target depends="init,compile-test,-pre-test-run,-do-test-run" if="have.tests" name="-post-test-run"> + <fail if="tests.failed">Some tests failed; see details above.</fail> + </target> + <target depends="init" if="have.tests" name="test-report"/> + <target depends="init" if="netbeans.home+have.tests" name="-test-browse"/> + <target depends="init,compile-test,-pre-test-run,-do-test-run,test-report,-post-test-run,-test-browse" description="Run unit tests." name="test"/> + <target depends="init" if="have.tests" name="-pre-test-run-single"> + <mkdir dir="${build.test.results.dir}"/> + </target> + <target depends="init,compile-test-single,-pre-test-run-single" if="have.tests" name="-do-test-run-single"> + <fail unless="test.includes">Must select some files in the IDE or set test.includes</fail> + <j2seproject3:junit excludes="" includes="${test.includes}"/> + </target> + <target depends="init,compile-test-single,-pre-test-run-single,-do-test-run-single" if="have.tests" name="-post-test-run-single"> + <fail if="tests.failed">Some tests failed; see details above.</fail> + </target> + <target depends="init,-do-not-recompile,compile-test-single,-pre-test-run-single,-do-test-run-single,-post-test-run-single" description="Run single unit test." name="test-single"/> + <!-- + ======================= + JUNIT DEBUGGING SECTION + ======================= + --> + <target depends="init,compile-test" if="have.tests" name="-debug-start-debuggee-test"> + <fail unless="test.class">Must select one file in the IDE or set test.class</fail> + <property location="${build.test.results.dir}/TEST-${test.class}.xml" name="test.report.file"/> + <delete file="${test.report.file}"/> + <mkdir dir="${build.test.results.dir}"/> + <j2seproject3:debug classname="org.apache.tools.ant.taskdefs.optional.junit.JUnitTestRunner" classpath="${ant.home}/lib/ant.jar:${ant.home}/lib/ant-junit.jar:${debug.test.classpath}"> + <customize> + <syspropertyset> + <propertyref prefix="test-sys-prop."/> + <mapper from="test-sys-prop.*" to="*" type="glob"/> + </syspropertyset> + <arg value="${test.class}"/> + <arg value="showoutput=true"/> + <arg value="formatter=org.apache.tools.ant.taskdefs.optional.junit.BriefJUnitResultFormatter"/> + <arg value="formatter=org.apache.tools.ant.taskdefs.optional.junit.XMLJUnitResultFormatter,${test.report.file}"/> + </customize> + </j2seproject3:debug> + </target> + <target depends="init,compile-test" if="netbeans.home+have.tests" name="-debug-start-debugger-test"> + <j2seproject1:nbjpdastart classpath="${debug.test.classpath}" name="${test.class}"/> + </target> + <target depends="init,-do-not-recompile,compile-test-single,-debug-start-debugger-test,-debug-start-debuggee-test" name="debug-test"/> + <target depends="init,-pre-debug-fix,compile-test-single" if="netbeans.home" name="-do-debug-fix-test"> + <j2seproject1:nbjpdareload dir="${build.test.classes.dir}"/> + </target> + <target depends="init,-pre-debug-fix,-do-debug-fix-test" if="netbeans.home" name="debug-fix-test"/> + <!-- + ========================= + APPLET EXECUTION SECTION + ========================= + --> + <target depends="init,compile-single" name="run-applet"> + <fail unless="applet.url">Must select one file in the IDE or set applet.url</fail> + <j2seproject1:java classname="sun.applet.AppletViewer"> + <customize> + <arg value="${applet.url}"/> + </customize> + </j2seproject1:java> + </target> + <!-- + ========================= + APPLET DEBUGGING SECTION + ========================= + --> + <target depends="init,compile-single" if="netbeans.home" name="-debug-start-debuggee-applet"> + <fail unless="applet.url">Must select one file in the IDE or set applet.url</fail> + <j2seproject3:debug classname="sun.applet.AppletViewer"> + <customize> + <arg value="${applet.url}"/> + </customize> + </j2seproject3:debug> + </target> + <target depends="init,compile-single,-debug-start-debugger,-debug-start-debuggee-applet" if="netbeans.home" name="debug-applet"/> + <!-- + =============== + CLEANUP SECTION + =============== + --> + <target depends="init" name="deps-clean" unless="no.deps"/> + <target depends="init" name="-do-clean"> + <delete dir="${build.dir}"/> + <delete dir="${dist.dir}"/> + </target> + <target name="-post-clean"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target depends="init,deps-clean,-do-clean,-post-clean" description="Clean build products." name="clean"/> +</project> diff --git a/src/jbullet/nbproject/genfiles.properties b/src/jbullet/nbproject/genfiles.properties new file mode 100644 index 0000000..80f3a63 --- /dev/null +++ b/src/jbullet/nbproject/genfiles.properties @@ -0,0 +1,11 @@ +build.xml.data.CRC32=ff8e82d1 +build.xml.script.CRC32=26cff537 +build.xml.stylesheet.CRC32=be360661 +# This file is used by a NetBeans-based IDE to track changes in generated files such as build-impl.xml. +# Do not edit this file. You may delete it but then the IDE will never regenerate such files for you. +nbproject/build-impl.xml.data.CRC32=aa374877 +nbproject/build-impl.xml.script.CRC32=daffc434 +nbproject/build-impl.xml.stylesheet.CRC32=487672f9 +nbproject/profiler-build-impl.xml.data.CRC32=ff8e82d1 +nbproject/profiler-build-impl.xml.script.CRC32=abda56ed +nbproject/profiler-build-impl.xml.stylesheet.CRC32=42cb6bcf diff --git a/src/jbullet/nbproject/profiler-build-impl.xml b/src/jbullet/nbproject/profiler-build-impl.xml new file mode 100644 index 0000000..7c8995d --- /dev/null +++ b/src/jbullet/nbproject/profiler-build-impl.xml @@ -0,0 +1,131 @@ +<?xml version="1.0" encoding="UTF-8"?> +<!-- +*** GENERATED FROM project.xml - DO NOT EDIT *** +*** EDIT ../build.xml INSTEAD *** + +For the purpose of easier reading the script +is divided into following sections: + + - initialization + - profiling + - applet profiling + +--> +<project name="-profiler-impl" default="profile" basedir=".."> + <target name="default" depends="profile" description="Build and profile the project."/> + <!-- + ====================== + INITIALIZATION SECTION + ====================== + --> + <target name="profile-init" depends="-profile-pre-init, init, -profile-post-init, -profile-init-macrodef-profile, -profile-init-check"/> + <target name="-profile-pre-init"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target name="-profile-post-init"> + <!-- Empty placeholder for easier customization. --> + <!-- You can override this target in the ../build.xml file. --> + </target> + <target name="-profile-init-macrodef-profile"> + <macrodef name="resolve"> + <attribute name="name"/> + <attribute name="value"/> + <sequential> + <property name="@{name}" value="${env.@{value}}"/> + </sequential> + </macrodef> + <macrodef name="profile"> + <attribute name="classname" default="${main.class}"/> + <element name="customize" optional="true"/> + <sequential> + <property environment="env"/> + <resolve name="profiler.current.path" value="${profiler.info.pathvar}"/> + <java fork="true" classname="@{classname}" dir="${profiler.info.dir}" jvm="${profiler.info.jvm}"> + <jvmarg value="${profiler.info.jvmargs.agent}"/> + <jvmarg line="${profiler.info.jvmargs}"/> + <env key="${profiler.info.pathvar}" path="${profiler.info.agentpath}:${profiler.current.path}"/> + <arg line="${application.args}"/> + <classpath> + <path path="${run.classpath}"/> + </classpath> + <syspropertyset> + <propertyref prefix="run-sys-prop."/> + <mapper type="glob" from="run-sys-prop.*" to="*"/> + </syspropertyset> + <customize/> + </java> + </sequential> + </macrodef> + </target> + <target name="-profile-init-check" depends="-profile-pre-init, init, -profile-post-init, -profile-init-macrodef-profile"> + <fail unless="profiler.info.jvm">Must set JVM to use for profiling in profiler.info.jvm</fail> + <fail unless="profiler.info.jvmargs.agent">Must set profiler agent JVM arguments in profiler.info.jvmargs.agent</fail> + </target> + <!-- + ================= + PROFILING SECTION + ================= + --> + <target name="profile" if="netbeans.home" depends="profile-init,compile" description="Profile a project in the IDE."> + <nbprofiledirect> + <classpath> + <path path="${run.classpath}"/> + </classpath> + </nbprofiledirect> + <profile/> + </target> + <target name="profile-single" if="netbeans.home" depends="profile-init,compile-single" description="Profile a selected class in the IDE."> + <fail unless="profile.class">Must select one file in the IDE or set profile.class</fail> + <nbprofiledirect> + <classpath> + <path path="${run.classpath}"/> + </classpath> + </nbprofiledirect> + <profile classname="${profile.class}"/> + </target> + <!-- + ========================= + APPLET PROFILING SECTION + ========================= + --> + <target name="profile-applet" if="netbeans.home" depends="profile-init,compile-single"> + <nbprofiledirect> + <classpath> + <path path="${run.classpath}"/> + </classpath> + </nbprofiledirect> + <profile classname="sun.applet.AppletViewer"> + <customize> + <arg value="${applet.url}"/> + </customize> + </profile> + </target> + <!-- + ========================= + TESTS PROFILING SECTION + ========================= + --> + <target name="profile-test-single" if="netbeans.home" depends="profile-init,compile-test-single"> + <nbprofiledirect> + <classpath> + <path path="${run.test.classpath}"/> + </classpath> + </nbprofiledirect> + <junit showoutput="true" fork="true" dir="${profiler.info.dir}" jvm="${profiler.info.jvm}" failureproperty="tests.failed" errorproperty="tests.failed"> + <env key="${profiler.info.pathvar}" path="${profiler.info.agentpath}:${profiler.current.path}"/> + <jvmarg value="${profiler.info.jvmargs.agent}"/> + <jvmarg line="${profiler.info.jvmargs}"/> + <test name="${profile.class}"/> + <classpath> + <path path="${run.test.classpath}"/> + </classpath> + <syspropertyset> + <propertyref prefix="test-sys-prop."/> + <mapper type="glob" from="test-sys-prop.*" to="*"/> + </syspropertyset> + <formatter type="brief" usefile="false"/> + <formatter type="xml"/> + </junit> + </target> +</project> diff --git a/src/jbullet/nbproject/project.properties b/src/jbullet/nbproject/project.properties new file mode 100644 index 0000000..660b95a --- /dev/null +++ b/src/jbullet/nbproject/project.properties @@ -0,0 +1,67 @@ +application.title=javabullet +application.vendor=jezek2 +build.classes.dir=${build.dir}/classes +build.classes.excludes=**/*.java,**/*.form +# This directory is removed when the project is cleaned: +build.dir=build +build.generated.dir=${build.dir}/generated +# Only compile against the classpath explicitly listed here: +build.sysclasspath=ignore +build.test.classes.dir=${build.dir}/test/classes +build.test.results.dir=${build.dir}/test/results +debug.classpath=\ + ${run.classpath} +debug.test.classpath=\ + ${run.test.classpath} +# This directory is removed when the project is cleaned: +dist.dir=dist +dist.jar=${dist.dir}/jbullet.jar +dist.javadoc.dir=${dist.dir}/javadoc +excludes= +includes=** +jar.compress=true +javac.classpath=\ + ${libs.vecmath.classpath}:\ + ${jogl.home}/build/jogl.core.jar:${jogl.home}/build/jogl.gles1.jar:${jogl.home}/build/jogl.egl.jar:${jogl.home}/build/newt.jar:\ + ${libs.trove.classpath} +# Space-separated list of extra javac options +javac.compilerargs= +javac.deprecation=false +javac.source=1.5 +javac.target=1.5 +javac.test.classpath=\ + ${javac.classpath}:\ + ${build.classes.dir} +javadoc.additionalparam=-group "Base library" "javabullet:javabullet.linearmath:javabullet.util" -group "Collision library" "javabullet.collision*" -group "Dynamics library" "javabullet.dynamics*" +javadoc.author=false +javadoc.encoding=${source.encoding} +javadoc.noindex=false +javadoc.nonavbar=false +javadoc.notree=false +javadoc.private=false +javadoc.splitindex=true +javadoc.use=true +javadoc.version=false +javadoc.windowtitle=JBullet +jnlp.codebase.type=local +jnlp.codebase.url= +jnlp.enabled=false +jnlp.offline-allowed=false +jnlp.signed=false +main.class=javabullet.demos.basic.BasicDemo +manifest.file=manifest.mf +meta.inf.dir=${src.dir}/META-INF +platform.active=default_platform +run.classpath=\ + ${javac.classpath}:\ + ${build.classes.dir} +# Space-separated list of JVM arguments used when running the project +# (you may also define separate properties like run-sys-prop.name=value instead of -Dname=value +# or test-sys-prop.name=value to set system properties for unit tests): +run.jvmargs=-ea -Djava.library.path=lib/jogl/linux-i586 +run.test.classpath=\ + ${javac.test.classpath}:\ + ${build.test.classes.dir} +source.encoding=UTF-8 +src.dir=src +test.src.dir=test diff --git a/src/jbullet/nbproject/project.xml b/src/jbullet/nbproject/project.xml new file mode 100644 index 0000000..f32ffbd --- /dev/null +++ b/src/jbullet/nbproject/project.xml @@ -0,0 +1,19 @@ +<?xml version="1.0" encoding="UTF-8"?> +<project xmlns="http://www.netbeans.org/ns/project/1"> + <type>org.netbeans.modules.java.j2seproject</type> + <configuration> + <data xmlns="http://www.netbeans.org/ns/j2se-project/3"> + <name>jbullet</name> + <minimum-ant-version>1.6.5</minimum-ant-version> + <source-roots> + <root id="src.dir"/> + </source-roots> + <test-roots> + <root id="test.src.dir"/> + </test-roots> + </data> + <libraries xmlns="http://www.netbeans.org/ns/ant-project-libraries/1"> + <definitions>lib/nblibraries.properties</definitions> + </libraries> + </configuration> +</project> diff --git a/src/jbullet/setenv-jbullet.sh b/src/jbullet/setenv-jbullet.sh new file mode 100644 index 0000000..2464022 --- /dev/null +++ b/src/jbullet/setenv-jbullet.sh @@ -0,0 +1,34 @@ +#! /bin/sh + +JOGLPROF=$1 +shift + +if [ -z "$JOGLPROF" ] ; then + JOGLPROF=JOGL_ES1_MIN +fi +THISHOME=`pwd` + +cd ../.. +. ./setenv-jogl.sh $JOGLPROF + +cd $THISHOME + +THISDIR=`pwd` + +PATH=$JAVA_HOME:$J2RE_HOME:$PATH +export PATH + +export LIBXCB_ALLOW_SLOPPY_LOCK=1 + +LIB=$THISDIR/lib + +JOGL_HOME=$THISDIR/../../../jogl +CLASSPATH_TROVE=$LIB/trove.jar +CLASSPATH_VECM=$LIB/vecmath.jar +CLASSPATH=$CLASSPATH:$THISDIR/build/classes:$CLASSPATH_TROVE:$CLASSPATH_VECM +export JOGL_HOME CLASSPATH_TROVE CLASSPATH_VECM CLASSPATH + +# +# java javabullet.demos.genericjoint.GenericJointDemo +# + diff --git a/src/jbullet/src/javabullet/ArrayPool.java b/src/jbullet/src/javabullet/ArrayPool.java new file mode 100644 index 0000000..d1793e1 --- /dev/null +++ b/src/jbullet/src/javabullet/ArrayPool.java @@ -0,0 +1,149 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +import java.lang.reflect.Array; +import java.util.ArrayList; +import java.util.Collections; +import java.util.Comparator; + +/** + * Object pool for arrays. + * + * @author jezek2 + */ +public class ArrayPool<T> { + + private Class componentType; + private ArrayList list = new ArrayList(); + private Comparator comparator; + private IntValue key = new IntValue(); + + /** + * Creates object pool. + * + * @param componentType + */ + public ArrayPool(Class componentType) { + this.componentType = componentType; + + if (componentType == float.class) { + comparator = floatComparator; + } + else if (!componentType.isPrimitive()) { + comparator = objectComparator; + } + else { + throw new UnsupportedOperationException("unsupported type "+componentType); + } + } + + @SuppressWarnings("unchecked") + private T create(int length) { + return (T)Array.newInstance(componentType, length); + } + + /** + * Returns array of exactly the same length as demanded, or create one if not + * present in the pool. + * + * @param length + * @return array + */ + @SuppressWarnings("unchecked") + public T getFixed(int length) { + key.value = length; + int index = Collections.binarySearch(list, key, comparator); + if (index < 0) { + return create(length); + } + return (T)list.remove(index); + } + + /** + * Returns array that has same or greater length, or create one if not present + * in the pool. + * + * @param length the minimum length required + * @return array + */ + @SuppressWarnings("unchecked") + public T getAtLeast(int length) { + key.value = length; + int index = Collections.binarySearch(list, key, comparator); + if (index < 0) { + index = -index - 1; + if (index < list.size()) { + return (T)list.remove(index); + } + else { + return create(length); + } + } + return (T)list.remove(index); + } + + /** + * Releases array into object pool. + * + * @param array previously obtained array from this pool + */ + @SuppressWarnings("unchecked") + public void release(T array) { + int index = Collections.binarySearch(list, array, comparator); + if (index < 0) index = -index - 1; + list.add(index, array); + + // remove references from object arrays: + if (comparator == objectComparator) { + Object[] objArray = (Object[])array; + for (int i=0; i<objArray.length; i++) { + objArray[i] = null; + } + } + } + + //////////////////////////////////////////////////////////////////////////// + + private static Comparator floatComparator = new Comparator() { + public int compare(Object o1, Object o2) { + int len1 = (o1 instanceof IntValue)? ((IntValue)o1).value : ((float[])o1).length; + int len2 = (o2 instanceof IntValue)? ((IntValue)o2).value : ((float[])o2).length; + return len1 > len2? 1 : len1 < len2 ? -1 : 0; + } + }; + + private static Comparator objectComparator = new Comparator() { + public int compare(Object o1, Object o2) { + int len1 = (o1 instanceof IntValue)? ((IntValue)o1).value : ((Object[])o1).length; + int len2 = (o2 instanceof IntValue)? ((IntValue)o2).value : ((Object[])o2).length; + return len1 > len2? 1 : len1 < len2 ? -1 : 0; + } + }; + + private static class IntValue { + public int value; + } + +} diff --git a/src/jbullet/src/javabullet/BulletGlobals.java b/src/jbullet/src/javabullet/BulletGlobals.java new file mode 100644 index 0000000..91a21ac --- /dev/null +++ b/src/jbullet/src/javabullet/BulletGlobals.java @@ -0,0 +1,143 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.Comparator; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Map.Entry; +import javax.vecmath.Vector3f; + +/** + * Bullet's global variables and constants. + * + * @author jezek2 + */ +public class BulletGlobals { + + public static final boolean DEBUG = true; + public static final boolean ENABLE_PROFILE = false; + + public static final float CONVEX_DISTANCE_MARGIN = 0.04f; + public static final float FLT_EPSILON = 1.19209290e-07f; + public static final float SIMD_EPSILON = FLT_EPSILON; + + public static final float SIMD_2_PI = 6.283185307179586232f; + public static final float SIMD_PI = SIMD_2_PI * 0.5f; + public static final float SIMD_HALF_PI = SIMD_2_PI * 0.25f; + public static final float SIMD_RADS_PER_DEG = SIMD_2_PI / 360f; + public static final float SIMD_DEGS_PER_RAD = 360f / SIMD_2_PI; + public static final float SIMD_INFINITY = Float.MAX_VALUE; + + public static ContactDestroyedCallback gContactDestroyedCallback; + public static ContactAddedCallback gContactAddedCallback; + public static float gContactBreakingThreshold = 0.02f; + + // RigidBody + public static float gDeactivationTime = 2f; + public static boolean gDisableDeactivation = false; + + public static int gTotalContactPoints; + + // GjkPairDetector + // temp globals, to improve GJK/EPA/penetration calculations + public static int gNumDeepPenetrationChecks = 0; + public static int gNumGjkChecks = 0; + + public static int gNumAlignedAllocs; + public static int gNumAlignedFree; + public static int gTotalBytesAlignedAllocs; + + public static int gPickingConstraintId = 0; + public static final Vector3f gOldPickingPos = new Vector3f(); + public static float gOldPickingDist = 0.f; + + public static int gOverlappingPairs = 0; + public static int gRemovePairs = 0; + public static int gAddedPairs = 0; + public static int gFindPairs = 0; + + public static final Vector3f ZERO_VECTOR3 = new Vector3f(0f, 0f, 0f); + + private static final List<ProfileBlock> profileStack = new ArrayList<ProfileBlock>(); + private static final Map<String,Long> profiles = new HashMap<String,Long>(); + + // JAVA NOTE: added for statistics in applet demo + public static long stepSimulationTime; + public static long updateTime; + + public static void pushProfile(String name) { + if (!ENABLE_PROFILE) return; + + ProfileBlock block = new ProfileBlock(); + block.name = name; + block.startTime = System.currentTimeMillis(); + profileStack.add(block); + } + + public static void popProfile() { + if (!ENABLE_PROFILE) return; + + ProfileBlock block = profileStack.remove(profileStack.size() - 1); + long time = System.currentTimeMillis(); + + Long totalTime = profiles.get(block.name); + if (totalTime == null) totalTime = 0L; + totalTime += (time - block.startTime); + profiles.put(block.name, totalTime); + } + + public static void printProfiles() { + ArrayList<Entry<String,Long>> list = new ArrayList<Entry<String,Long>>(profiles.entrySet()); + Collections.sort(list, new Comparator<Entry<String,Long>>() { + public int compare(Entry<String,Long> e1, Entry<String,Long> e2) { + return e1.getValue().compareTo(e2.getValue()); + } + }); + + for (Entry<String,Long> e : /*profiles.entrySet()*/list) { + System.out.println(e.getKey()+" = "+e.getValue()+" ms"); + } + } + + static { + if (ENABLE_PROFILE) { + Runtime.getRuntime().addShutdownHook(new Thread() { + @Override + public void run() { + printProfiles(); + } + }); + } + } + + private static class ProfileBlock { + public String name; + public long startTime; + } + +} diff --git a/src/jbullet/src/javabullet/BulletPool.java b/src/jbullet/src/javabullet/BulletPool.java new file mode 100644 index 0000000..10d720c --- /dev/null +++ b/src/jbullet/src/javabullet/BulletPool.java @@ -0,0 +1,64 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +import java.util.HashMap; +import java.util.Map; + +/** + * Maintains per-thread object pools for different types. + * + * @author jezek2 + */ +public class BulletPool { + + private BulletPool() {} + + private static ThreadLocal<Map> threadLocal = new ThreadLocal<Map>() { + @Override + protected Map initialValue() { + return new HashMap(); + } + }; + + /** + * Returns per-thread object pool for given type, or create one if it doesn't exist. + * + * @param cls type + * @return object pool + */ + @SuppressWarnings("unchecked") + public static <T> ObjectPool<T> get(Class<T> cls) { + Map map = threadLocal.get(); + + ObjectPool<T> pool = (ObjectPool<T>)map.get(cls); + if (pool == null) { + pool = new ObjectPool<T>(cls); + map.put(cls, pool); + } + + return pool; + } + +} diff --git a/src/jbullet/src/javabullet/BulletStack.java b/src/jbullet/src/javabullet/BulletStack.java new file mode 100644 index 0000000..a9152a7 --- /dev/null +++ b/src/jbullet/src/javabullet/BulletStack.java @@ -0,0 +1,82 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +/** + * Per-thread stack based object pools for common types. + * + * @see StackList + * + * @author jezek2 + */ +public class BulletStack { + + private BulletStack() {} + + private static final ThreadLocal<BulletStack> threadLocal = new ThreadLocal<BulletStack>() { + @Override + protected BulletStack initialValue() { + return new BulletStack(); + } + }; + + /** + * Returns stack for current thread, or create one if not present. + * + * @return stack + */ + public static BulletStack get() { + return threadLocal.get(); + } + + // common math: + public final VectorStackList vectors = new VectorStackList(); + public final TransformStackList transforms = new TransformStackList(); + public final MatrixStackList matrices = new MatrixStackList(); + + // others: + public final Vector4StackList vectors4 = new Vector4StackList(); + public final QuatStackList quats = new QuatStackList(); + + public final ArrayPool<float[]> floatArrays = new ArrayPool<float[]>(float.class); + + /** + * Pushes Vector3f, Transform and Matrix3f stacks. + */ + public void pushCommonMath() { + vectors.push(); + transforms.push(); + matrices.push(); + } + + /** + * Pops Vector3f, Transform and Matrix3f stacks. + */ + public void popCommonMath() { + vectors.pop(); + transforms.pop(); + matrices.pop(); + } + +} diff --git a/src/jbullet/src/javabullet/ContactAddedCallback.java b/src/jbullet/src/javabullet/ContactAddedCallback.java new file mode 100644 index 0000000..3745939 --- /dev/null +++ b/src/jbullet/src/javabullet/ContactAddedCallback.java @@ -0,0 +1,37 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +import javabullet.collision.dispatch.CollisionObject; +import javabullet.collision.narrowphase.ManifoldPoint; + +/** + * + * @author jezek2 + */ +public interface ContactAddedCallback { + + public boolean invoke(ManifoldPoint cp, CollisionObject colObj0, int partId0, int index0, CollisionObject colObj1, int partId1, int index1); + +} diff --git a/src/jbullet/src/javabullet/ContactDestroyedCallback.java b/src/jbullet/src/javabullet/ContactDestroyedCallback.java new file mode 100644 index 0000000..8b8498e --- /dev/null +++ b/src/jbullet/src/javabullet/ContactDestroyedCallback.java @@ -0,0 +1,34 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +/** + * + * @author jezek2 + */ +public interface ContactDestroyedCallback { + + public boolean invoke(Object userPersistentData); + +} diff --git a/src/jbullet/src/javabullet/MatrixStackList.java b/src/jbullet/src/javabullet/MatrixStackList.java new file mode 100644 index 0000000..c592095 --- /dev/null +++ b/src/jbullet/src/javabullet/MatrixStackList.java @@ -0,0 +1,51 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +import javax.vecmath.Matrix3f; + +/** + * Stack-based object pool for {@link Matrix3f}. + * + * @author jezek2 + */ +public class MatrixStackList extends StackList<Matrix3f> { + + public Matrix3f get(Matrix3f mat) { + Matrix3f obj = get(); + obj.set(mat); + return obj; + } + + @Override + protected Matrix3f create() { + return new Matrix3f(); + } + + @Override + protected void copy(Matrix3f dest, Matrix3f src) { + dest.set(src); + } + +} diff --git a/src/jbullet/src/javabullet/ObjectPool.java b/src/jbullet/src/javabullet/ObjectPool.java new file mode 100644 index 0000000..00092db --- /dev/null +++ b/src/jbullet/src/javabullet/ObjectPool.java @@ -0,0 +1,77 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +import java.util.ArrayList; + +/** + * Object pool. + * + * @author jezek2 + */ +public class ObjectPool<T> { + + private Class<T> cls; + private ArrayList<T> list = new ArrayList<T>(); + + public ObjectPool(Class<T> cls) { + this.cls = cls; + } + + private T create() { + try { + return cls.newInstance(); + } + catch (InstantiationException e) { + throw new IllegalStateException(e); + } + catch (IllegalAccessException e) { + throw new IllegalStateException(e); + } + } + + /** + * Returns instance from pool, or create one if pool is empty. + * + * @return instance + */ + public T get() { + if (list.size() > 0) { + return list.remove(list.size() - 1); + } + else { + return create(); + } + } + + /** + * Release instance into pool. + * + * @param obj previously obtained instance from pool + */ + public void release(T obj) { + list.add(obj); + } + +} diff --git a/src/jbullet/src/javabullet/ObjectStackList.java b/src/jbullet/src/javabullet/ObjectStackList.java new file mode 100644 index 0000000..2972218 --- /dev/null +++ b/src/jbullet/src/javabullet/ObjectStackList.java @@ -0,0 +1,58 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +/** + * Stack-based object pool for arbitrary objects, returning not supported. + * + * @author jezek2 + */ +public class ObjectStackList<T> extends StackList<T> { + + private Class<T> cls; + + public ObjectStackList(Class<T> cls) { + super(false); + this.cls = cls; + } + + @Override + protected T create() { + try { + return cls.newInstance(); + } + catch (InstantiationException e) { + throw new IllegalStateException(e); + } + catch (IllegalAccessException e) { + throw new IllegalStateException(e); + } + } + + @Override + protected void copy(T dest, T src) { + throw new UnsupportedOperationException(); + } + +} diff --git a/src/jbullet/src/javabullet/QuatStackList.java b/src/jbullet/src/javabullet/QuatStackList.java new file mode 100644 index 0000000..053798e --- /dev/null +++ b/src/jbullet/src/javabullet/QuatStackList.java @@ -0,0 +1,57 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +import javax.vecmath.Quat4f; + +/** + * Stack-based object pool for {@link Quat4f}. + * + * @author jezek2 + */ +public class QuatStackList extends StackList<Quat4f> { + + public Quat4f get(float x, float y, float z, float w) { + Quat4f v = get(); + v.set(x, y, z, w); + return v; + } + + public Quat4f get(Quat4f quat) { + Quat4f obj = get(); + obj.set(quat); + return obj; + } + + @Override + protected Quat4f create() { + return new Quat4f(); + } + + @Override + protected void copy(Quat4f dest, Quat4f src) { + dest.set(src); + } + +} diff --git a/src/jbullet/src/javabullet/StackList.java b/src/jbullet/src/javabullet/StackList.java new file mode 100644 index 0000000..a693ea8 --- /dev/null +++ b/src/jbullet/src/javabullet/StackList.java @@ -0,0 +1,136 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +import java.util.ArrayList; + +/** + * Stack-based object pool, see the example for usage. You must use the {@link #returning} + * method for returning stack-allocated instance.<p> + * + * Example code: + * + * <pre> + * StackList<Vector3f> vectors; + * ... + * + * vectors.push(); + * try { + * Vector3f vec = vectors.get(); + * ... + * return vectors.returning(vec); + * } + * finally { + * vectors.pop(); + * } + * </pre> + * + * @author jezek2 + */ +public abstract class StackList<T> { + + private final ArrayList<T> list = new ArrayList<T>(); + private T returnObj; + + private int[] stack = new int[512]; + private int stackCount = 0; + + private int pos = 0; + + public StackList() { + returnObj = create(); + } + + protected StackList(boolean unused) { + } + + /** + * Pushes the stack. + */ + public final void push() { + /*if (stackCount == stack.length-1) { + resizeStack(); + }*/ + + stack[stackCount++] = pos; + } + + /** + * Pops the stack. + */ + public final void pop() { + pos = stack[--stackCount]; + } + + /** + * Returns instance from stack pool, or create one if not present. The returned + * instance will be automatically reused when {@link #pop} is called. + * + * @return instance + */ + public T get() { + //if (true) return create(); + + if (pos == list.size()) { + expand(); + } + + return list.get(pos++); + } + + /** + * Copies given instance into one slot static instance and returns it. It's + * essential that caller of method (that uses this method for returning instances) + * immediately copies it into own variable before any other usage. + * + * @param obj stack-allocated instance + * @return one slot instance for returning purposes + */ + public final T returning(T obj) { + //if (true) { T ret = create(); copy(ret, obj); return ret; } + + copy(returnObj, obj); + return returnObj; + } + + /** + * Creates a new instance of type. + * + * @return instance + */ + protected abstract T create(); + + /** + * Copies data from one instance to another. + * + * @param dest + * @param src + */ + protected abstract void copy(T dest, T src); + + private void expand() { + list.add(create()); + } + +} diff --git a/src/jbullet/src/javabullet/TransformStackList.java b/src/jbullet/src/javabullet/TransformStackList.java new file mode 100644 index 0000000..085f66d --- /dev/null +++ b/src/jbullet/src/javabullet/TransformStackList.java @@ -0,0 +1,59 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +import javabullet.linearmath.Transform; +import javax.vecmath.Matrix3f; + +/** + * Stack-based object pool for {@link Transform}. + * + * @author jezek2 + */ +public class TransformStackList extends StackList<Transform> { + + public Transform get(Transform tr) { + Transform obj = get(); + obj.set(tr); + return obj; + } + + public Transform get(Matrix3f mat) { + Transform obj = get(); + obj.basis.set(mat); + obj.origin.set(0f, 0f, 0f); + return obj; + } + + @Override + protected Transform create() { + return new Transform(); + } + + @Override + protected void copy(Transform dest, Transform src) { + dest.set(src); + } + +} diff --git a/src/jbullet/src/javabullet/Vector4StackList.java b/src/jbullet/src/javabullet/Vector4StackList.java new file mode 100644 index 0000000..056c05e --- /dev/null +++ b/src/jbullet/src/javabullet/Vector4StackList.java @@ -0,0 +1,57 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +import javax.vecmath.Vector4f; + +/** + * Stack-based object pool for {@link Vector4f}. + * + * @author jezek2 + */ +public class Vector4StackList extends StackList<Vector4f> { + + public Vector4f get(float x, float y, float z, float w) { + Vector4f v = get(); + v.set(x, y, z, w); + return v; + } + + public Vector4f get(Vector4f vec) { + Vector4f v = get(); + v.set(vec); + return v; + } + + @Override + protected Vector4f create() { + return new Vector4f(); + } + + @Override + protected void copy(Vector4f dest, Vector4f src) { + dest.set(src); + } + +} diff --git a/src/jbullet/src/javabullet/VectorStackList.java b/src/jbullet/src/javabullet/VectorStackList.java new file mode 100644 index 0000000..6882932 --- /dev/null +++ b/src/jbullet/src/javabullet/VectorStackList.java @@ -0,0 +1,57 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet; + +import javax.vecmath.Vector3f; + +/** + * Stack-based object pool for {@link Vector3f}. + * + * @author jezek2 + */ +public class VectorStackList extends StackList<Vector3f> { + + public Vector3f get(float x, float y, float z) { + Vector3f v = get(); + v.set(x, y, z); + return v; + } + + public Vector3f get(Vector3f vec) { + Vector3f v = get(); + v.set(vec); + return v; + } + + @Override + protected Vector3f create() { + return new Vector3f(); + } + + @Override + protected void copy(Vector3f dest, Vector3f src) { + dest.set(src); + } + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/BroadphaseInterface.java b/src/jbullet/src/javabullet/collision/broadphase/BroadphaseInterface.java new file mode 100644 index 0000000..131bb56 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/BroadphaseInterface.java @@ -0,0 +1,46 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +import javax.vecmath.Vector3f; + +/** + * BroadphaseInterface for AABB overlapping object pairs. + * + * @author jezek2 + */ +public interface BroadphaseInterface { + + public BroadphaseProxy createProxy(Vector3f aabbMin, Vector3f aabbMax, BroadphaseNativeType shapeType, Object userPtr, short collisionFilterGroup, short collisionFilterMask, Dispatcher dispatcher); + + public void destroyProxy(BroadphaseProxy proxy, Dispatcher dispatcher); + + public void setAabb(BroadphaseProxy proxy, Vector3f aabbMin, Vector3f aabbMax, Dispatcher dispatcher); + + ///calculateOverlappingPairs is optional: incremental algorithms (sweep and prune) might do it during the set aabb + public void calculateOverlappingPairs(Dispatcher dispatcher); + + public OverlappingPairCache getOverlappingPairCache(); + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/BroadphaseNativeType.java b/src/jbullet/src/javabullet/collision/broadphase/BroadphaseNativeType.java new file mode 100644 index 0000000..8d066fe --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/BroadphaseNativeType.java @@ -0,0 +1,102 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +/** + * Dispatcher uses these types.<p> + * + * IMPORTANT NOTE: The types are ordered polyhedral, implicit convex and concave + * to facilitate type checking. + * + * @author jezek2 + */ +public enum BroadphaseNativeType { + + // polyhedral convex shapes: + BOX_SHAPE_PROXYTYPE, + TRIANGLE_SHAPE_PROXYTYPE, + TETRAHEDRAL_SHAPE_PROXYTYPE, + CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE, + CONVEX_HULL_SHAPE_PROXYTYPE, + + // implicit convex shapes: + IMPLICIT_CONVEX_SHAPES_START_HERE, + SPHERE_SHAPE_PROXYTYPE, + MULTI_SPHERE_SHAPE_PROXYTYPE, + CAPSULE_SHAPE_PROXYTYPE, + CONE_SHAPE_PROXYTYPE, + CONVEX_SHAPE_PROXYTYPE, + CYLINDER_SHAPE_PROXYTYPE, + UNIFORM_SCALING_SHAPE_PROXYTYPE, + MINKOWSKI_SUM_SHAPE_PROXYTYPE, + MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE, + + // concave shapes: + CONCAVE_SHAPES_START_HERE, + + // keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy! + TRIANGLE_MESH_SHAPE_PROXYTYPE, + + // used for demo integration FAST/Swift collision library and Bullet: + FAST_CONCAVE_MESH_PROXYTYPE, + + // terrain: + TERRAIN_SHAPE_PROXYTYPE, + + // used for GIMPACT Trimesh integration: + GIMPACT_SHAPE_PROXYTYPE, + EMPTY_SHAPE_PROXYTYPE, + STATIC_PLANE_PROXYTYPE, + CONCAVE_SHAPES_END_HERE, + COMPOUND_SHAPE_PROXYTYPE, + MAX_BROADPHASE_COLLISION_TYPES; + + private static BroadphaseNativeType[] values = values(); + + public static BroadphaseNativeType forValue(int value) { + return values[value]; + } + + public boolean isPolyhedral() { + return (ordinal() < IMPLICIT_CONVEX_SHAPES_START_HERE.ordinal()); + } + + public boolean isConvex() { + return (ordinal() < CONCAVE_SHAPES_START_HERE.ordinal()); + } + + public boolean isConcave() { + return ((ordinal() > CONCAVE_SHAPES_START_HERE.ordinal()) && + (ordinal() < CONCAVE_SHAPES_END_HERE.ordinal())); + } + + public boolean isCompound() { + return (ordinal() == COMPOUND_SHAPE_PROXYTYPE.ordinal()); + } + + public boolean isInfinite() { + return (ordinal() == STATIC_PLANE_PROXYTYPE.ordinal()); + } + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/BroadphasePair.java b/src/jbullet/src/javabullet/collision/broadphase/BroadphasePair.java new file mode 100644 index 0000000..c94c269 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/BroadphasePair.java @@ -0,0 +1,66 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +/** + * + * @author jezek2 + */ +public class BroadphasePair { + + public BroadphaseProxy pProxy0; + public BroadphaseProxy pProxy1; + public CollisionAlgorithm algorithm; + public Object userInfo; + + public BroadphasePair() { + } + + public void set(BroadphaseProxy pProxy0, BroadphaseProxy pProxy1) { + this.pProxy0 = pProxy0; + this.pProxy1 = pProxy1; + this.algorithm = null; + this.userInfo = null; + } + + public void set(BroadphasePair p) { + pProxy0 = p.pProxy0; + pProxy1 = p.pProxy1; + algorithm = p.algorithm; + userInfo = p.userInfo; + } + + @Override + public boolean equals(Object obj) { + if (obj == null || !(obj instanceof BroadphasePair)) return false; + BroadphasePair k = (BroadphasePair)obj; + return (pProxy0 == k.pProxy0 && pProxy1 == k.pProxy1) || (pProxy0 == k.pProxy1 && pProxy1 == k.pProxy0); + } + + @Override + public int hashCode() { + return pProxy0.hashCode() ^ pProxy1.hashCode(); + } + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/BroadphaseProxy.java b/src/jbullet/src/javabullet/collision/broadphase/BroadphaseProxy.java new file mode 100644 index 0000000..d98425d --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/BroadphaseProxy.java @@ -0,0 +1,54 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +/** + * + * @author jezek2 + */ +public class BroadphaseProxy { + + // Usually the client CollisionObject or Rigidbody class + public Object clientObject; + + // TODO: mask + public short collisionFilterGroup; + public short collisionFilterMask; + + public int uniqueId; // uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc. + + public BroadphaseProxy() { + } + + public BroadphaseProxy(Object userPtr, short collisionFilterGroup, short collisionFilterMask) { + this.clientObject = userPtr; + this.collisionFilterGroup = collisionFilterGroup; + this.collisionFilterMask = collisionFilterMask; + } + + public int getUid() { + return uniqueId; + } + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/CollisionAlgorithm.java b/src/jbullet/src/javabullet/collision/broadphase/CollisionAlgorithm.java new file mode 100644 index 0000000..8f6bc1a --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/CollisionAlgorithm.java @@ -0,0 +1,53 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +import javabullet.BulletStack; +import javabullet.collision.dispatch.CollisionObject; +import javabullet.collision.dispatch.ManifoldResult; + +/** + * + * @author jezek2 + */ +public abstract class CollisionAlgorithm { + + protected final BulletStack stack = BulletStack.get(); + + protected Dispatcher dispatcher; + + public CollisionAlgorithm() { + } + + public CollisionAlgorithm(CollisionAlgorithmConstructionInfo ci) { + dispatcher = ci.dispatcher1; + } + + public abstract void destroy(); + + public abstract void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut); + + public abstract float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut); + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/CollisionAlgorithmConstructionInfo.java b/src/jbullet/src/javabullet/collision/broadphase/CollisionAlgorithmConstructionInfo.java new file mode 100644 index 0000000..94f977a --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/CollisionAlgorithmConstructionInfo.java @@ -0,0 +1,39 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +import javabullet.collision.narrowphase.PersistentManifold; + +/** + * + * @author jezek2 + */ +public class CollisionAlgorithmConstructionInfo { + + public Dispatcher dispatcher1; + public PersistentManifold manifold; + + //public int getDispatcherId(); + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/CollisionFilterGroups.java b/src/jbullet/src/javabullet/collision/broadphase/CollisionFilterGroups.java new file mode 100644 index 0000000..49ee387 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/CollisionFilterGroups.java @@ -0,0 +1,39 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +/** + * + * @author jezek2 + */ +public class CollisionFilterGroups { + + public static final int DEFAULT_FILTER = 1; + public static final int STATIC_FILTER = 2; + public static final int KINEMATIC_FILTER = 4; + public static final int DEBRIS_FILTER = 8; + public static final int SENSOR_TRIGGER = 16; + public static final int ALL_FILTER = DEFAULT_FILTER | STATIC_FILTER | KINEMATIC_FILTER | DEBRIS_FILTER | SENSOR_TRIGGER; + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/DispatchFunc.java b/src/jbullet/src/javabullet/collision/broadphase/DispatchFunc.java new file mode 100644 index 0000000..7d8120f --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/DispatchFunc.java @@ -0,0 +1,45 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +/** + * + * @author jezek2 + */ +public enum DispatchFunc { + + DISPATCH_DISCRETE(1), + DISPATCH_CONTINUOUS(2); + + private int value; + + private DispatchFunc(int value) { + this.value = value; + } + + public int getValue() { + return value; + } + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/Dispatcher.java b/src/jbullet/src/javabullet/collision/broadphase/Dispatcher.java new file mode 100644 index 0000000..553f07d --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/Dispatcher.java @@ -0,0 +1,66 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +import java.util.List; +import javabullet.collision.dispatch.CollisionObject; +import javabullet.collision.narrowphase.PersistentManifold; + +/** + * Dispatcher can be used in combination with broadphase to dispatch overlapping pairs. + * For example for pairwise collision detection or user callbacks (game logic). + * + * @author jezek2 + */ +public abstract class Dispatcher { + + public final CollisionAlgorithm findAlgorithm(CollisionObject body0, CollisionObject body1) { + return findAlgorithm(body0, body1, null); + } + + public abstract CollisionAlgorithm findAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold); + + public abstract PersistentManifold getNewManifold(Object body0, Object body1); + + public abstract void releaseManifold(PersistentManifold manifold); + + public abstract void clearManifold(PersistentManifold manifold); + + public abstract boolean needsCollision(CollisionObject body0, CollisionObject body1); + + public abstract boolean needsResponse(CollisionObject body0, CollisionObject body1); + + public abstract void dispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo, Dispatcher dispatcher); + + public abstract int getNumManifolds(); + + public abstract PersistentManifold getManifoldByIndexInternal(int index); + + public abstract List<PersistentManifold> getInternalManifoldPointer(); + + //public abstract Object allocateCollisionAlgorithm(int size); + + //public abstract void freeCollisionAlgorithm(Object ptr); + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/DispatcherInfo.java b/src/jbullet/src/javabullet/collision/broadphase/DispatcherInfo.java new file mode 100644 index 0000000..f37326a --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/DispatcherInfo.java @@ -0,0 +1,49 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +import javabullet.linearmath.IDebugDraw; + +/** + * + * @author jezek2 + */ +public class DispatcherInfo { + + public float timeStep; + public int stepCount; + public DispatchFunc dispatchFunc; + public float timeOfImpact; + public boolean useContinuous; + public IDebugDraw debugDraw; + public boolean enableSatConvex; + public boolean enableSPU; + //btStackAlloc* m_stackAllocator; + + public DispatcherInfo() { + dispatchFunc = DispatchFunc.DISPATCH_DISCRETE; + timeOfImpact = 1f; + } + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/OverlapCallback.java b/src/jbullet/src/javabullet/collision/broadphase/OverlapCallback.java new file mode 100644 index 0000000..73e0208 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/OverlapCallback.java @@ -0,0 +1,35 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +/** + * + * @author jezek2 + */ +public interface OverlapCallback { + + //return true for deletion of the pair + public boolean processOverlap(BroadphasePair pair); + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/OverlapFilterCallback.java b/src/jbullet/src/javabullet/collision/broadphase/OverlapFilterCallback.java new file mode 100644 index 0000000..d288924 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/OverlapFilterCallback.java @@ -0,0 +1,35 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +/** + * + * @author jezek2 + */ +public interface OverlapFilterCallback { + + // return true when pairs need collision + public boolean needBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1); + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/OverlappingPairCache.java b/src/jbullet/src/javabullet/collision/broadphase/OverlappingPairCache.java new file mode 100644 index 0000000..dcfbaca --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/OverlappingPairCache.java @@ -0,0 +1,208 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +import java.util.Collection; +import java.util.Iterator; +import javabullet.BulletGlobals; +import javabullet.BulletPool; +import javabullet.ObjectPool; +import javabullet.util.HashUtil; +import javabullet.util.HashUtil.IMap; +import javabullet.util.HashUtil.IObjectProcedure; + +/** + * + * @author jezek2 + */ +public class OverlappingPairCache { + + private final ObjectPool<BroadphasePair> pairsPool = BulletPool.get(BroadphasePair.class); + + private final IMap<BroadphasePair,BroadphasePair> overlappingPairs = HashUtil.createMap(); + private OverlapFilterCallback overlapFilterCallback; + + public OverlappingPairCache() { + } + + /** + * Add a pair and return the new pair. If the pair already exists, + * no new pair is created and the old one is returned. + */ + public BroadphasePair addOverlappingPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1) { + BulletGlobals.gAddedPairs++; + + if (!needsBroadphaseCollision(proxy0, proxy1)) { + return null; + } + + BroadphasePair pair = pairsPool.get(); + pair.set(proxy0, proxy1); + + BroadphasePair old = overlappingPairs.get(pair); + if (old != null) { + pairsPool.release(pair); + return old; + } + overlappingPairs.put(pair, pair); + return pair; + } + + public Object removeOverlappingPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1, Dispatcher dispatcher) { + BulletGlobals.gRemovePairs++; + + BroadphasePair key = pairsPool.get(); + key.set(proxy0, proxy1); + BroadphasePair pair = overlappingPairs.remove(key); + pairsPool.release(key); + + if (pair == null) { + return null; + } + + cleanOverlappingPair(pair, dispatcher); + pairsPool.release(pair); + + return pair.userInfo; + } + + public boolean needsBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1) { + if (overlapFilterCallback != null) { + return overlapFilterCallback.needBroadphaseCollision(proxy0, proxy1); + } + + boolean collides = (proxy0.collisionFilterGroup & proxy1.collisionFilterMask) != 0; + collides = collides && (proxy1.collisionFilterGroup & proxy0.collisionFilterMask) != 0; + + return collides; + } + + private class ProcessAllOverlappingPairsCallback implements IObjectProcedure<BroadphasePair> { + public OverlapCallback callback; + public Dispatcher dispatcher; + + public boolean execute(BroadphasePair pair) { + if (callback.processOverlap(pair)) { + //removeOverlappingPair(pair.pProxy0, pair.pProxy1, dispatcher); + cleanOverlappingPair(pair, dispatcher); + BulletGlobals.gRemovePairs++; + BulletGlobals.gOverlappingPairs--; + pairsPool.release(pair); + return false; + } + return true; + } + } + + private ProcessAllOverlappingPairsCallback processAllOverlappingPairsCallback = new ProcessAllOverlappingPairsCallback(); + + public void processAllOverlappingPairs(OverlapCallback callback, Dispatcher dispatcher) { + processAllOverlappingPairsCallback.callback = callback; + processAllOverlappingPairsCallback.dispatcher = dispatcher; + overlappingPairs.retainEntries(processAllOverlappingPairsCallback); + } + + public void removeOverlappingPairsContainingProxy(BroadphaseProxy proxy, Dispatcher dispatcher) { + processAllOverlappingPairs(new RemovePairCallback(proxy), dispatcher); + } + + public void cleanProxyFromPairs(BroadphaseProxy proxy, Dispatcher dispatcher) { + processAllOverlappingPairs(new CleanPairCallback(proxy, this, dispatcher), dispatcher); + } + + public IMap<BroadphasePair,BroadphasePair> getOverlappingPairArray() { + return overlappingPairs; + } + + public void cleanOverlappingPair(BroadphasePair pair, Dispatcher dispatcher) { + if (pair.algorithm != null) { + pair.algorithm.destroy(); + // TODO: dispatcher.freeCollisionAlgorithm(pair.m_algorithm); + pair.algorithm = null; + } + } + + public BroadphasePair findPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1) { + BulletGlobals.gFindPairs++; + + BroadphasePair key = pairsPool.get(); + key.set(proxy0, proxy1); + BroadphasePair value = overlappingPairs.get(key); + pairsPool.release(key); + return value; + } + + public int getCount() { + return overlappingPairs.size(); + } + +// btBroadphasePair* GetPairs() { return m_pairs; } + public OverlapFilterCallback getOverlapFilterCallback() { + return overlapFilterCallback; + } + + public void setOverlapFilterCallback(OverlapFilterCallback overlapFilterCallback) { + this.overlapFilterCallback = overlapFilterCallback; + } + + public int getNumOverlappingPairs() { + return overlappingPairs.size(); + } + + //////////////////////////////////////////////////////////////////////////// + + private static class RemovePairCallback implements OverlapCallback { + private BroadphaseProxy obsoleteProxy; + + public RemovePairCallback(BroadphaseProxy obsoleteProxy) { + this.obsoleteProxy = obsoleteProxy; + } + + public boolean processOverlap(BroadphasePair pair) { + return ((pair.pProxy0 == obsoleteProxy) || + (pair.pProxy1 == obsoleteProxy)); + } + } + + private static class CleanPairCallback implements OverlapCallback { + private BroadphaseProxy cleanProxy; + private OverlappingPairCache pairCache; + private Dispatcher dispatcher; + + public CleanPairCallback(BroadphaseProxy cleanProxy, OverlappingPairCache pairCache, Dispatcher dispatcher) { + this.cleanProxy = cleanProxy; + this.pairCache = pairCache; + this.dispatcher = dispatcher; + } + + public boolean processOverlap(BroadphasePair pair) { + if ((pair.pProxy0 == cleanProxy) || + (pair.pProxy1 == cleanProxy)) { + pairCache.cleanOverlappingPair(pair, dispatcher); + } + return false; + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/OverlappingPairCallback.java b/src/jbullet/src/javabullet/collision/broadphase/OverlappingPairCallback.java new file mode 100644 index 0000000..9a832ab --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/OverlappingPairCallback.java @@ -0,0 +1,39 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +/** + * OverlappingPairCallback provides user callback to keep track of overlap between objects, like a collision sensor. + * + * @author jezek2 + */ +public interface OverlappingPairCallback { + + public void addOverlappingPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1); + + public void removeOverlappingPair(BroadphaseProxy proxy0, BroadphaseProxy proxy1); + + public void removeOverlappingPairsContainingProxy(BroadphaseProxy proxy0); + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/SimpleBroadphase.java b/src/jbullet/src/javabullet/collision/broadphase/SimpleBroadphase.java new file mode 100644 index 0000000..be688d9 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/SimpleBroadphase.java @@ -0,0 +1,112 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +import java.util.ArrayList; +import java.util.List; +import javax.vecmath.Vector3f; + +/** + * SimpleBroadphase is a brute force aabb culling broadphase based on O(n^2) aabb checks. + * SimpleBroadphase is just a unit-test implementation to verify and test other broadphases. + * So please don't use this class, but use bt32BitAxisSweep3 or btAxisSweep3 instead! + * + * @author jezek2 + */ +public class SimpleBroadphase implements BroadphaseInterface { + + private final List<SimpleBroadphaseProxy> handles = new ArrayList<SimpleBroadphaseProxy>(); + private int maxHandles; // max number of handles + private OverlappingPairCache pairCache; + private boolean ownsPairCache; + + public SimpleBroadphase() { + this(16384, null); + } + + public SimpleBroadphase(int maxProxies) { + this(maxProxies, null); + } + + public SimpleBroadphase(int maxProxies, OverlappingPairCache overlappingPairCache) { + this.pairCache = overlappingPairCache; + + if (overlappingPairCache == null) { + pairCache = new OverlappingPairCache(); + ownsPairCache = true; + } + } + + public BroadphaseProxy createProxy(Vector3f aabbMin, Vector3f aabbMax, BroadphaseNativeType shapeType, Object userPtr, short collisionFilterGroup, short collisionFilterMask, Dispatcher dispatcher) { + assert (aabbMin.x <= aabbMax.x && aabbMin.y <= aabbMax.y && aabbMin.z <= aabbMax.z); + + SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(aabbMin, aabbMax, shapeType, userPtr, collisionFilterGroup, collisionFilterMask); + proxy.uniqueId = handles.size(); + handles.add(proxy); + return proxy; + } + + public void destroyProxy(BroadphaseProxy proxyOrg, Dispatcher dispatcher) { + handles.remove(proxyOrg); + + pairCache.removeOverlappingPairsContainingProxy(proxyOrg, dispatcher); + } + + public void setAabb(BroadphaseProxy proxy, Vector3f aabbMin, Vector3f aabbMax, Dispatcher dispatcher) { + SimpleBroadphaseProxy sbp = (SimpleBroadphaseProxy)proxy; + sbp.min.set(aabbMin); + sbp.max.set(aabbMax); + } + + private static boolean aabbOverlap(SimpleBroadphaseProxy proxy0, SimpleBroadphaseProxy proxy1) { + return proxy0.min.x <= proxy1.max.x && proxy1.min.x <= proxy0.max.x && + proxy0.min.y <= proxy1.max.y && proxy1.min.y <= proxy0.max.y && + proxy0.min.z <= proxy1.max.z && proxy1.min.z <= proxy0.max.z; + } + + public void calculateOverlappingPairs(Dispatcher dispatcher) { + for (int i=0; i<handles.size(); i++) { + SimpleBroadphaseProxy proxy0 = handles.get(i); + for (int j=0; j<handles.size(); j++) { + SimpleBroadphaseProxy proxy1 = handles.get(j); + if (proxy0 == proxy1) continue; + + if (aabbOverlap(proxy0, proxy1)) { + if (pairCache.findPair(proxy0, proxy1) == null) { + pairCache.addOverlappingPair(proxy0, proxy1); + } + } + else { + if (pairCache.findPair(proxy0, proxy1) != null) { + pairCache.removeOverlappingPair(proxy0, proxy1, dispatcher); + } + } + } + } + } + + public OverlappingPairCache getOverlappingPairCache() { + return pairCache; + } +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/SimpleBroadphaseProxy.java b/src/jbullet/src/javabullet/collision/broadphase/SimpleBroadphaseProxy.java new file mode 100644 index 0000000..1ec73d4 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/SimpleBroadphaseProxy.java @@ -0,0 +1,46 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.broadphase; + +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class SimpleBroadphaseProxy extends BroadphaseProxy { + + protected final Vector3f min = new Vector3f(); + protected final Vector3f max = new Vector3f(); + + public SimpleBroadphaseProxy() { + } + + public SimpleBroadphaseProxy(Vector3f minpt, Vector3f maxpt, BroadphaseNativeType shapeType, Object userPtr, short collisionFilterGroup, short collisionFilterMask) { + super(userPtr, collisionFilterGroup, collisionFilterMask); + this.min.set(minpt); + this.max.set(maxpt); + } + +} diff --git a/src/jbullet/src/javabullet/collision/broadphase/package-info.java b/src/jbullet/src/javabullet/collision/broadphase/package-info.java new file mode 100644 index 0000000..4d78be3 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/broadphase/package-info.java @@ -0,0 +1,28 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +/** + * Broadphase collision code for fast determining of overlapping pairs. + */ +package javabullet.collision.broadphase; + diff --git a/src/jbullet/src/javabullet/collision/dispatch/CollisionAlgorithmCreateFunc.java b/src/jbullet/src/javabullet/collision/dispatch/CollisionAlgorithmCreateFunc.java new file mode 100644 index 0000000..6b3e86c --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/CollisionAlgorithmCreateFunc.java @@ -0,0 +1,40 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import javabullet.collision.broadphase.CollisionAlgorithm; +import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo; + +/** + * Used by the CollisionDispatcher to register and create instances for CollisionAlgorithm. + * + * @author jezek2 + */ +public abstract class CollisionAlgorithmCreateFunc { + + public boolean swapped; + + public abstract CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1); + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/CollisionConfiguration.java b/src/jbullet/src/javabullet/collision/dispatch/CollisionConfiguration.java new file mode 100644 index 0000000..5992624 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/CollisionConfiguration.java @@ -0,0 +1,46 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import javabullet.collision.broadphase.BroadphaseNativeType; + +/** + * CollisionConfiguration allows to configure Bullet collision detection + * stack allocator size, default collision algorithms and persistent manifold pool size + * todo: describe the meaning + * + * @author jezek2 + */ +public abstract class CollisionConfiguration { + + /* + ///memory pools + virtual btPoolAllocator* getPersistentManifoldPool() = 0; + virtual btPoolAllocator* getCollisionAlgorithmPool() = 0; + virtual btStackAlloc* getStackAllocator() = 0; + */ + + public abstract CollisionAlgorithmCreateFunc getCollisionAlgorithmCreateFunc(BroadphaseNativeType proxyType0, BroadphaseNativeType proxyType1); + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/CollisionDispatcher.java b/src/jbullet/src/javabullet/collision/dispatch/CollisionDispatcher.java new file mode 100644 index 0000000..a01ca77 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/CollisionDispatcher.java @@ -0,0 +1,292 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; +import javabullet.BulletPool; +import javabullet.ObjectPool; +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.collision.broadphase.BroadphasePair; +import javabullet.collision.broadphase.CollisionAlgorithm; +import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo; +import javabullet.collision.broadphase.DispatchFunc; +import javabullet.collision.broadphase.Dispatcher; +import javabullet.collision.broadphase.DispatcherInfo; +import javabullet.collision.broadphase.OverlapCallback; +import javabullet.collision.broadphase.OverlappingPairCache; +import javabullet.collision.narrowphase.PersistentManifold; + +/** + * CollisionDispatcher supports algorithms that handle ConvexConvex and ConvexConcave collision pairs. + * Time of Impact, Closest Points and Penetration Depth. + * + * @author jezek2 + */ +public class CollisionDispatcher extends Dispatcher { + + protected final ObjectPool<PersistentManifold> manifoldsPool = BulletPool.get(PersistentManifold.class); + + private static final int MAX_BROADPHASE_COLLISION_TYPES = BroadphaseNativeType.MAX_BROADPHASE_COLLISION_TYPES.ordinal(); + private int count = 0; + private final List<PersistentManifold> manifoldsPtr = new ArrayList<PersistentManifold>(); + private boolean useIslands = true; + private boolean staticWarningReported = false; + private ManifoldResult defaultManifoldResult; + private NearCallback nearCallback; + //private PoolAllocator* m_collisionAlgorithmPoolAllocator; + //private PoolAllocator* m_persistentManifoldPoolAllocator; + private final CollisionAlgorithmCreateFunc[][] doubleDispatch = new CollisionAlgorithmCreateFunc[MAX_BROADPHASE_COLLISION_TYPES][MAX_BROADPHASE_COLLISION_TYPES]; + private CollisionConfiguration collisionConfiguration; + private static int gNumManifold = 0; + + public CollisionDispatcher(CollisionConfiguration collisionConfiguration) { + this.collisionConfiguration = collisionConfiguration; + + setNearCallback(new DefaultNearCallback()); + + //m_collisionAlgorithmPoolAllocator = collisionConfiguration->getCollisionAlgorithmPool(); + //m_persistentManifoldPoolAllocator = collisionConfiguration->getPersistentManifoldPool(); + + for (int i = 0; i < MAX_BROADPHASE_COLLISION_TYPES; i++) { + for (int j = 0; j < MAX_BROADPHASE_COLLISION_TYPES; j++) { + doubleDispatch[i][j] = collisionConfiguration.getCollisionAlgorithmCreateFunc( + BroadphaseNativeType.forValue(i), + BroadphaseNativeType.forValue(j) + ); + assert (doubleDispatch[i][j] != null); + } + } + } + + public void registerCollisionCreateFunc(int proxyType0, int proxyType1, CollisionAlgorithmCreateFunc createFunc) { + doubleDispatch[proxyType0][proxyType1] = createFunc; + } + + public NearCallback getNearCallback() { + return nearCallback; + } + + public void setNearCallback(NearCallback nearCallback) { + this.nearCallback = nearCallback; + } + + public CollisionConfiguration getCollisionConfiguration() { + return collisionConfiguration; + } + + public void setCollisionConfiguration(CollisionConfiguration collisionConfiguration) { + this.collisionConfiguration = collisionConfiguration; + } + + @Override + public CollisionAlgorithm findAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold) { + CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(); + + ci.dispatcher1 = this; + ci.manifold = sharedManifold; + CollisionAlgorithm algo = doubleDispatch[body0.getCollisionShape().getShapeType().ordinal()][body1.getCollisionShape().getShapeType().ordinal()].createCollisionAlgorithm(ci, body0, body1); + + return algo; + } + + @Override + public PersistentManifold getNewManifold(Object b0, Object b1) { + gNumManifold++; + + //btAssert(gNumManifold < 65535); + + CollisionObject body0 = (CollisionObject)b0; + CollisionObject body1 = (CollisionObject)b1; + + /* + void* mem = 0; + + if (m_persistentManifoldPoolAllocator->getFreeCount()) + { + mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); + } else + { + mem = btAlignedAlloc(sizeof(btPersistentManifold),16); + + } + btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0); + manifold->m_index1a = m_manifoldsPtr.size(); + m_manifoldsPtr.push_back(manifold); + */ + + PersistentManifold manifold = manifoldsPool.get(); + manifold.init(body0,body1,0); + + manifold.index1a = manifoldsPtr.size(); + manifoldsPtr.add(manifold); + + return manifold; + } + + @Override + public void releaseManifold(PersistentManifold manifold) { + gNumManifold--; + + //printf("releaseManifold: gNumManifold %d\n",gNumManifold); + clearManifold(manifold); + + // TODO: optimize + int findIndex = manifold.index1a; + assert (findIndex < manifoldsPtr.size()); + Collections.swap(manifoldsPtr, findIndex, manifoldsPtr.size()-1); + manifoldsPtr.get(findIndex).index1a = findIndex; + manifoldsPtr.remove(manifoldsPtr.size()-1); + + manifoldsPool.release(manifold); + /* + manifold->~btPersistentManifold(); + if (m_persistentManifoldPoolAllocator->validPtr(manifold)) + { + m_persistentManifoldPoolAllocator->freeMemory(manifold); + } else + { + btAlignedFree(manifold); + } + */ + } + + @Override + public void clearManifold(PersistentManifold manifold) { + manifold.clearManifold(); + } + + @Override + public boolean needsCollision(CollisionObject body0, CollisionObject body1) { + assert (body0 != null); + assert (body1 != null); + + boolean needsCollision = true; + + //#ifdef BT_DEBUG + if (!staticWarningReported) { + // broadphase filtering already deals with this + if ((body0.isStaticObject() || body0.isKinematicObject()) && + (body1.isStaticObject() || body1.isKinematicObject())) { + staticWarningReported = true; + System.err.println("warning CollisionDispatcher.needsCollision: static-static collision!"); + } + } + //#endif //BT_DEBUG + + if ((!body0.isActive()) && (!body1.isActive())) { + needsCollision = false; + } + else if (!body0.checkCollideWith(body1)) { + needsCollision = false; + } + + return needsCollision; + } + + @Override + public boolean needsResponse(CollisionObject body0, CollisionObject body1) { + //here you can do filtering + boolean hasResponse = (body0.hasContactResponse() && body1.hasContactResponse()); + //no response between two static/kinematic bodies: + hasResponse = hasResponse && ((!body0.isStaticOrKinematicObject()) || (!body1.isStaticOrKinematicObject())); + return hasResponse; + } + + private static class CollisionPairCallback implements OverlapCallback { + private DispatcherInfo dispatchInfo; + private CollisionDispatcher dispatcher; + + public void init(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher) { + this.dispatchInfo = dispatchInfo; + this.dispatcher = dispatcher; + } + + public boolean processOverlap(BroadphasePair pair) { + dispatcher.getNearCallback().invoke(pair, dispatcher, dispatchInfo); + return false; + } + } + + private CollisionPairCallback collisionPairCallback = new CollisionPairCallback(); + + @Override + public void dispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo, Dispatcher dispatcher) { + //m_blockedForChanges = true; + collisionPairCallback.init(dispatchInfo, this); + pairCache.processAllOverlappingPairs(collisionPairCallback, dispatcher); + //m_blockedForChanges = false; + } + + @Override + public int getNumManifolds() { + return manifoldsPtr.size(); + } + + @Override + public PersistentManifold getManifoldByIndexInternal(int index) { + return manifoldsPtr.get(index); + } + + @Override + public List<PersistentManifold> getInternalManifoldPointer() { + return manifoldsPtr; + } + + //////////////////////////////////////////////////////////////////////////// + + private static class DefaultNearCallback implements NearCallback { + private final ManifoldResult contactPointResult = new ManifoldResult(); + + public void invoke(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo) { + CollisionObject colObj0 = (CollisionObject) collisionPair.pProxy0.clientObject; + CollisionObject colObj1 = (CollisionObject) collisionPair.pProxy1.clientObject; + + if (dispatcher.needsCollision(colObj0, colObj1)) { + // dispatcher will keep algorithms persistent in the collision pair + if (collisionPair.algorithm == null) { + collisionPair.algorithm = dispatcher.findAlgorithm(colObj0, colObj1); + } + + if (collisionPair.algorithm != null) { + //ManifoldResult contactPointResult = new ManifoldResult(colObj0, colObj1); + contactPointResult.init(colObj0, colObj1); + + if (dispatchInfo.dispatchFunc == DispatchFunc.DISPATCH_DISCRETE) { + // discrete collision detection query + collisionPair.algorithm.processCollision(colObj0, colObj1, dispatchInfo, contactPointResult); + } + else { + // continuous collision detection query, time of impact (toi) + float toi = collisionPair.algorithm.calculateTimeOfImpact(colObj0, colObj1, dispatchInfo, contactPointResult); + if (dispatchInfo.timeOfImpact > toi) { + dispatchInfo.timeOfImpact = toi; + } + } + } + } + } + } +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/CollisionFlags.java b/src/jbullet/src/javabullet/collision/dispatch/CollisionFlags.java new file mode 100644 index 0000000..9203f08 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/CollisionFlags.java @@ -0,0 +1,37 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +/** + * + * @author jezek2 + */ +public class CollisionFlags { + + public static final int STATIC_OBJECT = 1; + public static final int KINEMATIC_OBJECT = 2; + public static final int NO_CONTACT_RESPONSE = 4; + public static final int CUSTOM_MATERIAL_CALLBACK = 8; // this allows per-triangle material (friction/restitution) + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/CollisionObject.java b/src/jbullet/src/javabullet/collision/dispatch/CollisionObject.java new file mode 100644 index 0000000..483184b --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/CollisionObject.java @@ -0,0 +1,282 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import javabullet.BulletStack; +import javabullet.collision.broadphase.BroadphaseProxy; +import javabullet.collision.shapes.CollisionShape; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * CollisionObject can be used to manage collision detection objects. + * CollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy. + * They can be added to the CollisionWorld. + * + * @author jezek2 + */ +public class CollisionObject { + + protected final BulletStack stack = BulletStack.get(); + + // island management, m_activationState1 + public static final int ACTIVE_TAG = 1; + public static final int ISLAND_SLEEPING = 2; + public static final int WANTS_DEACTIVATION = 3; + public static final int DISABLE_DEACTIVATION = 4; + public static final int DISABLE_SIMULATION = 5; + protected Transform worldTransform = new Transform(); + + ///m_interpolationWorldTransform is used for CCD and interpolation + ///it can be either previous or future (predicted) transform + protected final Transform interpolationWorldTransform = new Transform(); + //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) + //without destroying the continuous interpolated motion (which uses this interpolation velocities) + protected final Vector3f interpolationLinearVelocity = new Vector3f(); + protected final Vector3f interpolationAngularVelocity = new Vector3f(); + protected BroadphaseProxy broadphaseHandle; + protected CollisionShape collisionShape; + protected int collisionFlags; + protected int islandTag1; + protected int companionId; + protected int activationState1; + protected float deactivationTime; + protected float friction; + protected float restitution; + + ///users can point to their objects, m_userPointer is not used by Bullet, see setUserPointer/getUserPointer + protected Object userObjectPointer; + + ///m_internalOwner is reserved to point to Bullet's btRigidBody. Don't use this, use m_userObjectPointer instead. + protected Object internalOwner; + + ///time of impact calculation + protected float hitFraction; + ///Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: + protected float ccdSweptSphereRadius; + + /// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold + protected float ccdSquareMotionThreshold; + /// If some object should have elaborate collision filtering by sub-classes + protected boolean checkCollideWith; + + public CollisionObject() { + this.collisionFlags = CollisionFlags.STATIC_OBJECT; + this.islandTag1 = -1; + this.companionId = -1; + this.activationState1 = 1; + this.friction = 0.5f; + this.hitFraction = 1f; + } + + public boolean checkCollideWithOverride(CollisionObject co) { + return true; + } + + public boolean mergesSimulationIslands() { + ///static objects, kinematic and object without contact response don't merge islands + return ((collisionFlags & (CollisionFlags.STATIC_OBJECT | CollisionFlags.KINEMATIC_OBJECT | CollisionFlags.NO_CONTACT_RESPONSE)) == 0); + } + + public boolean isStaticObject() { + return (collisionFlags & CollisionFlags.STATIC_OBJECT) != 0; + } + + public boolean isKinematicObject() { + return (collisionFlags & CollisionFlags.KINEMATIC_OBJECT) != 0; + } + + public boolean isStaticOrKinematicObject() { + return (collisionFlags & (CollisionFlags.KINEMATIC_OBJECT | CollisionFlags.STATIC_OBJECT)) != 0; + } + + public boolean hasContactResponse() { + return (collisionFlags & CollisionFlags.NO_CONTACT_RESPONSE) == 0; + } + + public CollisionShape getCollisionShape() { + return collisionShape; + } + + public void setCollisionShape(CollisionShape collisionShape) { + this.collisionShape = collisionShape; + } + + public int getActivationState() { + return activationState1; + } + + public void setActivationState(int newState) { + if ((activationState1 != DISABLE_DEACTIVATION) && (activationState1 != DISABLE_SIMULATION)) { + this.activationState1 = newState; + } + } + + public float getDeactivationTime() { + return deactivationTime; + } + + public void setDeactivationTime(float deactivationTime) { + this.deactivationTime = deactivationTime; + } + + public void forceActivationState(int newState) { + this.activationState1 = newState; + } + + public void activate() { + activate(false); + } + + public void activate(boolean forceActivation) { + if (forceActivation || (collisionFlags & (CollisionFlags.STATIC_OBJECT | CollisionFlags.KINEMATIC_OBJECT)) == 0) { + setActivationState(ACTIVE_TAG); + deactivationTime = 0f; + } + } + + public boolean isActive() { + return ((getActivationState() != ISLAND_SLEEPING) && (getActivationState() != DISABLE_SIMULATION)); + } + + public float getRestitution() { + return restitution; + } + + public void setRestitution(float restitution) { + this.restitution = restitution; + } + + public float getFriction() { + return friction; + } + + public void setFriction(float friction) { + this.friction = friction; + } + + // reserved for Bullet internal usage + public Object getInternalOwner() { + return internalOwner; + } + + public Transform getWorldTransform() { + return worldTransform; + } + + public void setWorldTransform(Transform worldTransform) { + this.worldTransform.set(worldTransform); + } + + public BroadphaseProxy getBroadphaseHandle() { + return broadphaseHandle; + } + + public void setBroadphaseHandle(BroadphaseProxy broadphaseHandle) { + this.broadphaseHandle = broadphaseHandle; + } + + public Transform getInterpolationWorldTransform() { + return interpolationWorldTransform; + } + + public void setInterpolationWorldTransform(Transform interpolationWorldTransform) { + this.interpolationWorldTransform.set(interpolationWorldTransform); + } + + public Vector3f getInterpolationLinearVelocity() { + return interpolationLinearVelocity; + } + + public Vector3f getInterpolationAngularVelocity() { + return interpolationAngularVelocity; + } + + public int getIslandTag() { + return islandTag1; + } + + public void setIslandTag(int islandTag) { + this.islandTag1 = islandTag; + } + + public int getCompanionId() { + return companionId; + } + + public void setCompanionId(int companionId) { + this.companionId = companionId; + } + + public float getHitFraction() { + return hitFraction; + } + + public void setHitFraction(float hitFraction) { + this.hitFraction = hitFraction; + } + + public int getCollisionFlags() { + return collisionFlags; + } + + public void setCollisionFlags(int collisionFlags) { + this.collisionFlags = collisionFlags; + } + + // Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: + public float getCcdSweptSphereRadius() { + return ccdSweptSphereRadius; + } + + // Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm:: + public void setCcdSweptSphereRadius(float ccdSweptSphereRadius) { + this.ccdSweptSphereRadius = ccdSweptSphereRadius; + } + + public float getCcdSquareMotionThreshold() { + return ccdSquareMotionThreshold; + } + + // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold + public void setCcdSquareMotionThreshold(float ccdSquareMotionThreshold) { + this.ccdSquareMotionThreshold = ccdSquareMotionThreshold; + } + + public Object getUserPointer() { + return userObjectPointer; + } + + public void setUserPointer(Object userObjectPointer) { + this.userObjectPointer = userObjectPointer; + } + + public boolean checkCollideWith(CollisionObject co) { + if (checkCollideWith) { + return checkCollideWithOverride(co); + } + + return true; + } +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/CollisionWorld.java b/src/jbullet/src/javabullet/collision/dispatch/CollisionWorld.java new file mode 100644 index 0000000..95d3e7d --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/CollisionWorld.java @@ -0,0 +1,567 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import java.util.ArrayList; +import java.util.List; +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javabullet.collision.broadphase.BroadphaseInterface; +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.collision.broadphase.BroadphaseProxy; +import javabullet.collision.broadphase.Dispatcher; +import javabullet.collision.broadphase.DispatcherInfo; +import javabullet.collision.broadphase.OverlappingPairCache; +import javabullet.collision.narrowphase.ConvexCast.CastResult; +import javabullet.collision.narrowphase.SubsimplexConvexCast; +import javabullet.collision.narrowphase.TriangleRaycastCallback; +import javabullet.collision.narrowphase.VoronoiSimplexSolver; +import javabullet.collision.shapes.BvhTriangleMeshShape; +import javabullet.collision.shapes.CollisionShape; +import javabullet.collision.shapes.CompoundShape; +import javabullet.collision.shapes.ConcaveShape; +import javabullet.collision.shapes.ConvexShape; +import javabullet.collision.shapes.SphereShape; +import javabullet.linearmath.AabbUtil2; +import javabullet.linearmath.IDebugDraw; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * CollisionWorld is interface and container for the collision detection. + * + * @author jezek2 + */ +public class CollisionWorld { + + protected final BulletStack stack = BulletStack.get(); + + protected List<CollisionObject> collisionObjects = new ArrayList<CollisionObject>(); + protected Dispatcher dispatcher1; + protected DispatcherInfo dispatchInfo = new DispatcherInfo(); + //protected btStackAlloc* m_stackAlloc; + protected BroadphaseInterface broadphasePairCache; + protected IDebugDraw debugDrawer; + + /** + * This constructor doesn't own the dispatcher and paircache/broadphase. + */ + public CollisionWorld(Dispatcher dispatcher,BroadphaseInterface broadphasePairCache, CollisionConfiguration collisionConfiguration) { + this.dispatcher1 = dispatcher; + this.broadphasePairCache = broadphasePairCache; + } + + public void destroy() { + // clean up remaining objects + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject collisionObject = collisionObjects.get(i); + + BroadphaseProxy bp = collisionObject.getBroadphaseHandle(); + if (bp != null) { + // + // only clear the cached algorithms + // + getBroadphase().getOverlappingPairCache().cleanProxyFromPairs(bp, dispatcher1); + getBroadphase().destroyProxy(bp, dispatcher1); + } + } + } + + public void addCollisionObject(CollisionObject collisionObject) { + addCollisionObject(collisionObject, (short)1, (short)1); + } + + public void addCollisionObject(CollisionObject collisionObject, short collisionFilterGroup, short collisionFilterMask) { + stack.pushCommonMath(); + try { + // check that the object isn't already added + assert (!collisionObjects.contains(collisionObject)); + + collisionObjects.add(collisionObject); + + // calculate new AABB + // TODO: check if it's overwritten or not + Transform trans = stack.transforms.get(collisionObject.getWorldTransform()); + + Vector3f minAabb = stack.vectors.get(); + Vector3f maxAabb = stack.vectors.get(); + collisionObject.getCollisionShape().getAabb(trans, minAabb, maxAabb); + + BroadphaseNativeType type = collisionObject.getCollisionShape().getShapeType(); + collisionObject.setBroadphaseHandle(getBroadphase().createProxy( + minAabb, + maxAabb, + type, + collisionObject, + collisionFilterGroup, + collisionFilterMask, + dispatcher1)); + } + finally { + stack.popCommonMath(); + } + } + + public void performDiscreteCollisionDetection() { + BulletGlobals.pushProfile("performDiscreteCollisionDetection"); + try { + //DispatcherInfo dispatchInfo = getDispatchInfo(); + + updateAabbs(); + + broadphasePairCache.calculateOverlappingPairs(dispatcher1); + + Dispatcher dispatcher = getDispatcher(); + { + BulletGlobals.pushProfile("dispatchAllCollisionPairs"); + try { + if (dispatcher != null) { + dispatcher.dispatchAllCollisionPairs(broadphasePairCache.getOverlappingPairCache(), dispatchInfo, dispatcher1); + } + } + finally { + BulletGlobals.popProfile(); + } + } + } + finally { + BulletGlobals.popProfile(); + } + } + + public void removeCollisionObject(CollisionObject collisionObject) { + //bool removeFromBroadphase = false; + + { + BroadphaseProxy bp = collisionObject.getBroadphaseHandle(); + if (bp != null) { + // + // only clear the cached algorithms + // + getBroadphase().getOverlappingPairCache().cleanProxyFromPairs(bp, dispatcher1); + getBroadphase().destroyProxy(bp, dispatcher1); + collisionObject.setBroadphaseHandle(null); + } + } + + //swapremove + collisionObjects.remove(collisionObject); + } + + public BroadphaseInterface getBroadphase() { + return broadphasePairCache; + } + + public OverlappingPairCache getPairCache() { + return broadphasePairCache.getOverlappingPairCache(); + } + + public Dispatcher getDispatcher() { + return dispatcher1; + } + + public DispatcherInfo getDispatchInfo() { + return dispatchInfo; + } + + private static boolean updateAabbs_reportMe = true; + + public void updateAabbs() { + BulletGlobals.pushProfile("updateAabbs"); + stack.pushCommonMath(); + try { + Transform predictedTrans = stack.transforms.get(); + Vector3f minAabb = stack.vectors.get(), maxAabb = stack.vectors.get(); + Vector3f tmp = stack.vectors.get(); + + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + + // only update aabb of active objects + if (colObj.isActive()) { + colObj.getCollisionShape().getAabb(colObj.getWorldTransform(), minAabb, maxAabb); + BroadphaseInterface bp = broadphasePairCache; + + // moving objects should be moderately sized, probably something wrong if not + tmp.sub(maxAabb, minAabb); // TODO: optimize + if (colObj.isStaticObject() || (tmp.lengthSquared() < 1e12f)) { + bp.setAabb(colObj.getBroadphaseHandle(), minAabb, maxAabb, dispatcher1); + } + else { + // something went wrong, investigate + // this assert is unwanted in 3D modelers (danger of loosing work) + colObj.setActivationState(CollisionObject.DISABLE_SIMULATION); + + if (updateAabbs_reportMe && debugDrawer != null) { + updateAabbs_reportMe = false; + debugDrawer.reportErrorWarning("Overflow in AABB, object removed from simulation"); + debugDrawer.reportErrorWarning("If you can reproduce this, please email [email protected]\n"); + debugDrawer.reportErrorWarning("Please include above information, your Platform, version of OS.\n"); + debugDrawer.reportErrorWarning("Thanks.\n"); + } + } + } + } + } + finally { + stack.popCommonMath(); + BulletGlobals.popProfile(); + } + } + + public IDebugDraw getDebugDrawer() { + return debugDrawer; + } + + public void setDebugDrawer(IDebugDraw debugDrawer) { + this.debugDrawer = debugDrawer; + } + + public int getNumCollisionObjects() { + return collisionObjects.size(); + } + + // TODO + public /*static*/ void rayTestSingle(Transform rayFromTrans, Transform rayToTrans, + CollisionObject collisionObject, + CollisionShape collisionShape, + Transform colObjWorldTransform, + RayResultCallback resultCallback, short collisionFilterMask) { + stack.pushCommonMath(); + try { + SphereShape pointShape = new SphereShape(0f); + pointShape.setMargin(0f); + ConvexShape castShape = pointShape; + + if (collisionShape.isConvex()) { + CastResult castResult = new CastResult(); + castResult.fraction = resultCallback.closestHitFraction; + + ConvexShape convexShape = (ConvexShape) collisionShape; + VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); + + //#define USE_SUBSIMPLEX_CONVEX_CAST 1 + //#ifdef USE_SUBSIMPLEX_CONVEX_CAST + SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(castShape, convexShape, simplexSolver); + //#else + //btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver); + //btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0); + //#endif //#USE_SUBSIMPLEX_CONVEX_CAST + + if (convexCaster.calcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult)) { + //add hit + if (castResult.normal.lengthSquared() > 0.0001f) { + if (castResult.fraction < resultCallback.closestHitFraction) { + //#ifdef USE_SUBSIMPLEX_CONVEX_CAST + //rotate normal into worldspace + rayFromTrans.basis.transform(castResult.normal); + //#endif //USE_SUBSIMPLEX_CONVEX_CAST + + castResult.normal.normalize(); + LocalRayResult localRayResult = new LocalRayResult( + collisionObject, + null, + castResult.normal, + castResult.fraction); + + boolean normalInWorldSpace = true; + resultCallback.addSingleResult(localRayResult, normalInWorldSpace); + } + } + } + } + else { + if (collisionShape.isConcave()) { + if (collisionShape.getShapeType() == BroadphaseNativeType.TRIANGLE_MESH_SHAPE_PROXYTYPE) { + // optimized version for BvhTriangleMeshShape + BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape; + Transform worldTocollisionObject = stack.transforms.get(); + worldTocollisionObject.inverse(colObjWorldTransform); + Vector3f rayFromLocal = stack.vectors.get(rayFromTrans.origin); + worldTocollisionObject.transform(rayFromLocal); + Vector3f rayToLocal = stack.vectors.get(rayToTrans.origin); + worldTocollisionObject.transform(rayToLocal); + + BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh); + rcb.hitFraction = resultCallback.closestHitFraction; + triangleMesh.performRaycast(rcb, rayFromLocal, rayToLocal); + } + else { + ConcaveShape triangleMesh = (ConcaveShape)collisionShape; + + Transform worldTocollisionObject = stack.transforms.get(); + worldTocollisionObject.inverse(colObjWorldTransform); + + Vector3f rayFromLocal = stack.vectors.get(rayFromTrans.origin); + worldTocollisionObject.transform(rayFromLocal); + Vector3f rayToLocal = stack.vectors.get(rayToTrans.origin); + worldTocollisionObject.transform(rayToLocal); + + BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh); + rcb.hitFraction = resultCallback.closestHitFraction; + + Vector3f rayAabbMinLocal = stack.vectors.get(rayFromLocal); + VectorUtil.setMin(rayAabbMinLocal, rayToLocal); + Vector3f rayAabbMaxLocal = stack.vectors.get(rayFromLocal); + VectorUtil.setMax(rayAabbMaxLocal, rayToLocal); + + triangleMesh.processAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal); + } + } + else { + // todo: use AABB tree or other BVH acceleration structure! + if (collisionShape.isCompound()) { + CompoundShape compoundShape = (CompoundShape) collisionShape; + int i = 0; + for (i = 0; i < compoundShape.getNumChildShapes(); i++) { + Transform childTrans = stack.transforms.get(compoundShape.getChildTransform(i)); + CollisionShape childCollisionShape = compoundShape.getChildShape(i); + Transform childWorldTrans = stack.transforms.get(colObjWorldTransform); + childWorldTrans.mul(childTrans); + rayTestSingle(rayFromTrans, rayToTrans, + collisionObject, + childCollisionShape, + childWorldTrans, + resultCallback, collisionFilterMask); + } + } + } + } + } + finally { + stack.popCommonMath(); + } + } + + public void rayTest(Vector3f rayFromWorld, Vector3f rayToWorld, RayResultCallback resultCallback) { + rayTest(rayFromWorld, rayToWorld, resultCallback, (short)-1); + } + + /** + * rayTest performs a raycast on all objects in the CollisionWorld, and calls the resultCallback. + * This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback. + */ + public void rayTest(Vector3f rayFromWorld, Vector3f rayToWorld, RayResultCallback resultCallback, short collisionFilterMask) { + stack.pushCommonMath(); + try { + Transform rayFromTrans = stack.transforms.get(), rayToTrans = stack.transforms.get(); + rayFromTrans.setIdentity(); + rayFromTrans.origin.set(rayFromWorld); + rayToTrans.setIdentity(); + + rayToTrans.origin.set(rayToWorld); + + // go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD) + Vector3f collisionObjectAabbMin = stack.vectors.get(), collisionObjectAabbMax = stack.vectors.get(); + float[] hitLambda = new float[1]; + + for (int i = 0; i < collisionObjects.size(); i++) { + // terminate further ray tests, once the closestHitFraction reached zero + if (resultCallback.closestHitFraction == 0f) { + break; + } + + CollisionObject collisionObject = collisionObjects.get(i); + // only perform raycast if filterMask matches + if ((collisionObject.getBroadphaseHandle().collisionFilterGroup & collisionFilterMask) != 0) { + //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject(); + collisionObject.getCollisionShape().getAabb(collisionObject.getWorldTransform(), collisionObjectAabbMin, collisionObjectAabbMax); + + hitLambda[0] = resultCallback.closestHitFraction; + Vector3f hitNormal = stack.vectors.get(); + if (AabbUtil2.rayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal)) { + rayTestSingle(rayFromTrans, rayToTrans, + collisionObject, + collisionObject.getCollisionShape(), + collisionObject.getWorldTransform(), + resultCallback, + (short) -1); + } + } + + } + } + finally { + stack.popCommonMath(); + } + } + + public List<CollisionObject> getCollisionObjectArray() { + return collisionObjects; + } + + //////////////////////////////////////////////////////////////////////////// + + /** + * LocalShapeInfo gives extra information for complex shapes. + * Currently, only btTriangleMeshShape is available, so it just contains triangleIndex and subpart. + */ + public static class LocalShapeInfo { + public int shapePart; + public int triangleIndex; + //const btCollisionShape* m_shapeTemp; + //const btTransform* m_shapeLocalTransform; + } + + public static class LocalRayResult { + public CollisionObject collisionObject; + public LocalShapeInfo localShapeInfo; + public final Vector3f hitNormalLocal = new Vector3f(); + public float hitFraction; + + public LocalRayResult(CollisionObject collisionObject, LocalShapeInfo localShapeInfo, Vector3f hitNormalLocal, float hitFraction) { + this.collisionObject = collisionObject; + this.localShapeInfo = localShapeInfo; + this.hitNormalLocal.set(hitNormalLocal); + this.hitFraction = hitFraction; + } + } + + /** + * RayResultCallback is used to report new raycast results. + */ + public static abstract class RayResultCallback { + public float closestHitFraction = 1f; + public CollisionObject collisionObject; + + public boolean hasHit() { + return (collisionObject != null); + } + + public abstract float addSingleResult(LocalRayResult rayResult, boolean normalInWorldSpace); + } + + public static class ClosestRayResultCallback extends RayResultCallback { + public final Vector3f rayFromWorld = new Vector3f(); //used to calculate hitPointWorld from hitFraction + public final Vector3f rayToWorld = new Vector3f(); + + public final Vector3f hitNormalWorld = new Vector3f(); + public final Vector3f hitPointWorld = new Vector3f(); + + public ClosestRayResultCallback(Vector3f rayFromWorld, Vector3f rayToWorld) { + this.rayFromWorld.set(rayFromWorld); + this.rayToWorld.set(rayToWorld); + } + + @Override + public float addSingleResult(LocalRayResult rayResult, boolean normalInWorldSpace) { + // caller already does the filter on the closestHitFraction + assert (rayResult.hitFraction <= closestHitFraction); + + closestHitFraction = rayResult.hitFraction; + collisionObject = rayResult.collisionObject; + if (normalInWorldSpace) { + hitNormalWorld.set(rayResult.hitNormalLocal); + } + else { + // need to transform normal into worldspace + hitNormalWorld.set(rayResult.hitNormalLocal); + collisionObject.getWorldTransform().basis.transform(hitNormalWorld); + } + + VectorUtil.setInterpolate3(hitPointWorld, rayFromWorld, rayToWorld, rayResult.hitFraction); + return rayResult.hitFraction; + } + } + + public static class LocalConvexResult { + public CollisionObject hitCollisionObject; + public LocalShapeInfo localShapeInfo; + public final Vector3f hitNormalLocal = new Vector3f(); + public final Vector3f hitPointLocal = new Vector3f(); + public float hitFraction; + + public LocalConvexResult(CollisionObject hitCollisionObject, LocalShapeInfo localShapeInfo, Vector3f hitNormalLocal, Vector3f hitPointLocal, float hitFraction) { + this.hitCollisionObject = hitCollisionObject; + this.localShapeInfo = localShapeInfo; + this.hitNormalLocal.set(hitNormalLocal); + this.hitPointLocal.set(hitPointLocal); + this.hitFraction = hitFraction; + } + } + + public static abstract class ConvexResultCallback { + public float closestHitFraction = 1f; + + public boolean hasHit() { + return (closestHitFraction < 1f); + } + + public abstract float addSingleResult(LocalConvexResult convexResult, boolean normalInWorldSpace); + } + + public static class ClosestConvexResultCallback extends ConvexResultCallback { + public final Vector3f convexFromWorld = new Vector3f(); // used to calculate hitPointWorld from hitFraction + public final Vector3f convexToWorld = new Vector3f(); + public final Vector3f hitNormalWorld = new Vector3f(); + public final Vector3f hitPointWorld = new Vector3f(); + public CollisionObject hitCollisionObject; + + @Override + public float addSingleResult(LocalConvexResult convexResult, boolean normalInWorldSpace) { + // caller already does the filter on the m_closestHitFraction + assert (convexResult.hitFraction <= closestHitFraction); + + closestHitFraction = convexResult.hitFraction; + hitCollisionObject = convexResult.hitCollisionObject; + if (normalInWorldSpace) { + hitNormalWorld.set(convexResult.hitNormalLocal); + } + else { + // need to transform normal into worldspace + hitNormalWorld.set(convexResult.hitNormalLocal); + hitCollisionObject.getWorldTransform().basis.transform(hitNormalWorld); + } + + hitPointWorld.set(convexResult.hitPointLocal); + return convexResult.hitFraction; + } + } + + private static class BridgeTriangleRaycastCallback extends TriangleRaycastCallback { + public RayResultCallback resultCallback; + public CollisionObject collisionObject; + public ConcaveShape triangleMesh; + + public BridgeTriangleRaycastCallback(Vector3f from, Vector3f to, RayResultCallback resultCallback, CollisionObject collisionObject, ConcaveShape triangleMesh) { + super(from, to); + this.resultCallback = resultCallback; + this.collisionObject = collisionObject; + this.triangleMesh = triangleMesh; + } + + public float reportHit(Vector3f hitNormalLocal, float hitFraction, int partId, int triangleIndex) { + LocalShapeInfo shapeInfo = new LocalShapeInfo(); + shapeInfo.shapePart = partId; + shapeInfo.triangleIndex = triangleIndex; + + LocalRayResult rayResult = new LocalRayResult(collisionObject, shapeInfo, hitNormalLocal, hitFraction); + + boolean normalInWorldSpace = false; + return resultCallback.addSingleResult(rayResult, normalInWorldSpace); + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/CompoundCollisionAlgorithm.java b/src/jbullet/src/javabullet/collision/dispatch/CompoundCollisionAlgorithm.java new file mode 100644 index 0000000..7c36d4b --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/CompoundCollisionAlgorithm.java @@ -0,0 +1,196 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import java.util.ArrayList; +import java.util.List; +import javabullet.collision.broadphase.CollisionAlgorithm; +import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo; +import javabullet.collision.broadphase.DispatcherInfo; +import javabullet.collision.shapes.CollisionShape; +import javabullet.collision.shapes.CompoundShape; +import javabullet.linearmath.Transform; + +/** + * CompoundCollisionAlgorithm supports collision between CompoundCollisionShapes and other collision shapes. + * Place holder, not fully implemented yet. + * + * @author jezek2 + */ +public class CompoundCollisionAlgorithm extends CollisionAlgorithm { + + private final List<CollisionAlgorithm> childCollisionAlgorithms = new ArrayList<CollisionAlgorithm>(); + private boolean isSwapped; + + public CompoundCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, boolean isSwapped) { + super(ci); + this.isSwapped = isSwapped; + + CollisionObject colObj = isSwapped ? body1 : body0; + CollisionObject otherObj = isSwapped ? body0 : body1; + assert (colObj.getCollisionShape().isCompound()); + + CompoundShape compoundShape = (CompoundShape) colObj.getCollisionShape(); + int numChildren = compoundShape.getNumChildShapes(); + int i; + + //childCollisionAlgorithms.resize(numChildren); + for (i = 0; i < numChildren; i++) { + CollisionShape childShape = compoundShape.getChildShape(i); + CollisionShape orgShape = colObj.getCollisionShape(); + colObj.setCollisionShape(childShape); + childCollisionAlgorithms.add(ci.dispatcher1.findAlgorithm(colObj, otherObj)); + colObj.setCollisionShape(orgShape); + } + } + + @Override + public void destroy() { + int numChildren = childCollisionAlgorithms.size(); + int i; + for (i = 0; i < numChildren; i++) { + childCollisionAlgorithms.get(i).destroy(); + //m_dispatcher->freeCollisionAlgorithm(m_childCollisionAlgorithms[i]); + } + } + + @Override + public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { + stack.transforms.push(); + try { + CollisionObject colObj = isSwapped ? body1 : body0; + CollisionObject otherObj = isSwapped ? body0 : body1; + + assert (colObj.getCollisionShape().isCompound()); + CompoundShape compoundShape = (CompoundShape) colObj.getCollisionShape(); + + // We will use the OptimizedBVH, AABB tree to cull potential child-overlaps + // If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals + // given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: + // determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 + // then use each overlapping node AABB against Tree0 + // and vise versa. + + Transform tmpTrans = stack.transforms.get(); + Transform orgTrans = stack.transforms.get(); + + int numChildren = childCollisionAlgorithms.size(); + int i; + for (i = 0; i < numChildren; i++) { + // temporarily exchange parent btCollisionShape with childShape, and recurse + CollisionShape childShape = compoundShape.getChildShape(i); + + // backup + orgTrans.set(colObj.getWorldTransform()); + CollisionShape orgShape = colObj.getCollisionShape(); + + Transform childTrans = compoundShape.getChildTransform(i); + //btTransform newChildWorldTrans = orgTrans*childTrans ; + tmpTrans.set(orgTrans); + tmpTrans.mul(childTrans); + colObj.setWorldTransform(tmpTrans); + // the contactpoint is still projected back using the original inverted worldtrans + colObj.setCollisionShape(childShape); + childCollisionAlgorithms.get(i).processCollision(colObj, otherObj, dispatchInfo, resultOut); + // revert back + colObj.setCollisionShape(orgShape); + colObj.setWorldTransform(orgTrans); + } + } + finally { + stack.transforms.pop(); + } + } + + @Override + public float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { + stack.transforms.push(); + try { + CollisionObject colObj = isSwapped ? body1 : body0; + CollisionObject otherObj = isSwapped ? body0 : body1; + + assert (colObj.getCollisionShape().isCompound()); + + CompoundShape compoundShape = (CompoundShape) colObj.getCollisionShape(); + + // We will use the OptimizedBVH, AABB tree to cull potential child-overlaps + // If both proxies are Compound, we will deal with that directly, by performing sequential/parallel tree traversals + // given Proxy0 and Proxy1, if both have a tree, Tree0 and Tree1, this means: + // determine overlapping nodes of Proxy1 using Proxy0 AABB against Tree1 + // then use each overlapping node AABB against Tree0 + // and vise versa. + + Transform tmpTrans = stack.transforms.get(); + Transform orgTrans = stack.transforms.get(); + float hitFraction = 1f; + + int numChildren = childCollisionAlgorithms.size(); + int i; + for (i = 0; i < numChildren; i++) { + // temporarily exchange parent btCollisionShape with childShape, and recurse + CollisionShape childShape = compoundShape.getChildShape(i); + + // backup + orgTrans.set(colObj.getWorldTransform()); + CollisionShape orgShape = colObj.getCollisionShape(); + + Transform childTrans = compoundShape.getChildTransform(i); + //btTransform newChildWorldTrans = orgTrans*childTrans ; + tmpTrans.set(orgTrans); + tmpTrans.mul(childTrans); + colObj.setWorldTransform(tmpTrans); + + colObj.setCollisionShape(childShape); + float frac = childCollisionAlgorithms.get(i).calculateTimeOfImpact(colObj, otherObj, dispatchInfo, resultOut); + if (frac < hitFraction) { + hitFraction = frac; + } + // revert back + colObj.setCollisionShape(orgShape); + colObj.setWorldTransform(orgTrans); + } + return hitFraction; + } + finally { + stack.transforms.pop(); + } + } + + //////////////////////////////////////////////////////////////////////////// + + public static final CollisionAlgorithmCreateFunc createFunc = new CollisionAlgorithmCreateFunc() { + @Override + public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) { + return new CompoundCollisionAlgorithm(ci, body0, body1, false); + } + }; + + public static final CollisionAlgorithmCreateFunc swappedCreateFunc = new CollisionAlgorithmCreateFunc() { + @Override + public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) { + return new CompoundCollisionAlgorithm(ci, body0, body1, true); + } + }; + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/ConvexConcaveCollisionAlgorithm.java b/src/jbullet/src/javabullet/collision/dispatch/ConvexConcaveCollisionAlgorithm.java new file mode 100644 index 0000000..ed2df6d --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/ConvexConcaveCollisionAlgorithm.java @@ -0,0 +1,226 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import javabullet.collision.broadphase.CollisionAlgorithm; +import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo; +import javabullet.collision.broadphase.DispatcherInfo; +import javabullet.collision.narrowphase.ConvexCast.CastResult; +import javabullet.collision.narrowphase.SubsimplexConvexCast; +import javabullet.collision.narrowphase.VoronoiSimplexSolver; +import javabullet.collision.shapes.ConcaveShape; +import javabullet.collision.shapes.SphereShape; +import javabullet.collision.shapes.TriangleCallback; +import javabullet.collision.shapes.TriangleShape; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * ConvexConcaveCollisionAlgorithm supports collision between convex shapes + * and (concave) trianges meshes. + * + * @author jezek2 + */ +public class ConvexConcaveCollisionAlgorithm extends CollisionAlgorithm { + + private boolean isSwapped; + private ConvexTriangleCallback btConvexTriangleCallback; + + public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, boolean isSwapped) { + super(ci); + this.isSwapped = isSwapped; + this.btConvexTriangleCallback = new ConvexTriangleCallback(dispatcher, body0, body1, isSwapped); + } + + @Override + public void destroy() { + btConvexTriangleCallback.destroy(); + } + + @Override + public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { + CollisionObject convexBody = isSwapped ? body1 : body0; + CollisionObject triBody = isSwapped ? body0 : body1; + + if (triBody.getCollisionShape().isConcave()) { + CollisionObject triOb = triBody; + ConcaveShape concaveShape = (ConcaveShape)triOb.getCollisionShape(); + + if (convexBody.getCollisionShape().isConvex()) { + float collisionMarginTriangle = concaveShape.getMargin(); + + resultOut.setPersistentManifold(btConvexTriangleCallback.manifoldPtr); + btConvexTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut); + + // Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. + //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr); + + btConvexTriangleCallback.manifoldPtr.setBodies(convexBody, triBody); + + concaveShape.processAllTriangles(btConvexTriangleCallback, btConvexTriangleCallback.getAabbMin(), btConvexTriangleCallback.getAabbMax()); + + resultOut.refreshContactPoints(); + } + } + } + + @Override + public float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { + stack.pushCommonMath(); + try { + Vector3f tmp = stack.vectors.get(); + + CollisionObject convexbody = isSwapped ? body1 : body0; + CollisionObject triBody = isSwapped ? body0 : body1; + + // quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast) + + // only perform CCD above a certain threshold, this prevents blocking on the long run + // because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame... + tmp.sub(convexbody.getInterpolationWorldTransform().origin, convexbody.getWorldTransform().origin); + float squareMot0 = tmp.lengthSquared(); + if (squareMot0 < convexbody.getCcdSquareMotionThreshold()) { + return 1f; + } + + //const btVector3& from = convexbody->m_worldTransform.getOrigin(); + //btVector3 to = convexbody->m_interpolationWorldTransform.getOrigin(); + //todo: only do if the motion exceeds the 'radius' + + Transform triInv = stack.transforms.get(triBody.getWorldTransform()); + triInv.inverse(); + + Transform convexFromLocal = stack.transforms.get(); + convexFromLocal.mul(triInv, convexbody.getWorldTransform()); + + Transform convexToLocal = stack.transforms.get(); + convexToLocal.mul(triInv, convexbody.getInterpolationWorldTransform()); + + if (triBody.getCollisionShape().isConcave()) { + Vector3f rayAabbMin = stack.vectors.get(convexFromLocal.origin); + VectorUtil.setMin(rayAabbMin, convexToLocal.origin); + + Vector3f rayAabbMax = stack.vectors.get(convexFromLocal.origin); + VectorUtil.setMax(rayAabbMax, convexToLocal.origin); + + float ccdRadius0 = convexbody.getCcdSweptSphereRadius(); + + tmp.set(ccdRadius0, ccdRadius0, ccdRadius0); + rayAabbMin.sub(tmp); + rayAabbMax.add(tmp); + + float curHitFraction = 1f; // is this available? + LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal, convexbody.getCcdSweptSphereRadius(), curHitFraction); + + raycastCallback.hitFraction = convexbody.getHitFraction(); + + CollisionObject concavebody = triBody; + + ConcaveShape triangleMesh = (ConcaveShape)concavebody.getCollisionShape(); + + if (triangleMesh != null) { + triangleMesh.processAllTriangles(raycastCallback, rayAabbMin, rayAabbMax); + } + + if (raycastCallback.hitFraction < convexbody.getHitFraction()) { + convexbody.setHitFraction(raycastCallback.hitFraction); + return raycastCallback.hitFraction; + } + } + + return 1f; + } + finally { + stack.popCommonMath(); + } + } + + public void clearCache() { + btConvexTriangleCallback.clearCache(); + } + + //////////////////////////////////////////////////////////////////////////// + + private static class LocalTriangleSphereCastCallback implements TriangleCallback { + public final Transform ccdSphereFromTrans = new Transform(); + public final Transform ccdSphereToTrans = new Transform(); + public final Transform meshTransform = new Transform(); + + public float ccdSphereRadius; + public float hitFraction; + + private final Transform ident = new Transform(); + + public LocalTriangleSphereCastCallback(Transform from, Transform to, float ccdSphereRadius, float hitFraction) { + this.ccdSphereFromTrans.set(from); + this.ccdSphereToTrans.set(to); + this.ccdSphereRadius = ccdSphereRadius; + this.hitFraction = hitFraction; + + // JAVA NOTE: moved here from processTriangle + ident.setIdentity(); + } + + public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) { + // do a swept sphere for now + + //btTransform ident; + //ident.setIdentity(); + + CastResult castResult = new CastResult(); + castResult.fraction = hitFraction; + SphereShape pointShape = new SphereShape(ccdSphereRadius); + TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); + VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); + SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver); + //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver); + //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0); + //local space? + + if (convexCaster.calcTimeOfImpact(ccdSphereFromTrans, ccdSphereToTrans, ident, ident, castResult)) { + if (hitFraction > castResult.fraction) { + hitFraction = castResult.fraction; + } + } + } + } + + //////////////////////////////////////////////////////////////////////////// + + public static class CreateFunc extends CollisionAlgorithmCreateFunc { + @Override + public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) { + return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false); + } + } + + public static class SwappedCreateFunc extends CollisionAlgorithmCreateFunc { + @Override + public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) { + return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true); + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/ConvexConvexAlgorithm.java b/src/jbullet/src/javabullet/collision/dispatch/ConvexConvexAlgorithm.java new file mode 100644 index 0000000..6b674c3 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/ConvexConvexAlgorithm.java @@ -0,0 +1,254 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import javabullet.BulletPool; +import javabullet.ObjectPool; +import javabullet.collision.broadphase.CollisionAlgorithm; +import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo; +import javabullet.collision.broadphase.DispatcherInfo; +import javabullet.collision.narrowphase.ConvexCast; +import javabullet.collision.narrowphase.ConvexPenetrationDepthSolver; +import javabullet.collision.narrowphase.DiscreteCollisionDetectorInterface.ClosestPointInput; +import javabullet.collision.narrowphase.GjkConvexCast; +import javabullet.collision.narrowphase.GjkPairDetector; +import javabullet.collision.narrowphase.PersistentManifold; +import javabullet.collision.narrowphase.SimplexSolverInterface; +import javabullet.collision.narrowphase.VoronoiSimplexSolver; +import javabullet.collision.shapes.ConvexShape; +import javabullet.collision.shapes.SphereShape; +import javax.vecmath.Vector3f; + +/** + * ConvexConvexAlgorithm collision algorithm implements time of impact, convex closest points and penetration depth calculations. + * + * @author jezek2 + */ +public class ConvexConvexAlgorithm extends CollisionAlgorithm { + + protected final ObjectPool<ClosestPointInput> pointInputsPool = BulletPool.get(ClosestPointInput.class); + + private GjkPairDetector gjkPairDetector; + + public boolean ownManifold = false; + public PersistentManifold manifoldPtr; + public boolean lowLevelOfDetail = false; + + public ConvexConvexAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1, SimplexSolverInterface simplexSolver, ConvexPenetrationDepthSolver pdSolver) { + super(ci); + gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, pdSolver); + this.manifoldPtr = mf; + } + + @Override + public void destroy() { + if (ownManifold) { + if (manifoldPtr != null) { + dispatcher.releaseManifold(manifoldPtr); + } + } + } + + public void setLowLevelOfDetail(boolean useLowLevel) { + this.lowLevelOfDetail = useLowLevel; + } + + /** + * Convex-Convex collision algorithm. + */ + @Override + public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { + if (manifoldPtr == null) { + // swapped? + manifoldPtr = dispatcher.getNewManifold(body0, body1); + ownManifold = true; + } + resultOut.setPersistentManifold(manifoldPtr); + +// #ifdef USE_BT_GJKEPA +// btConvexShape* shape0(static_cast<btConvexShape*>(body0->getCollisionShape())); +// btConvexShape* shape1(static_cast<btConvexShape*>(body1->getCollisionShape())); +// const btScalar radialmargin(0/*shape0->getMargin()+shape1->getMargin()*/); +// btGjkEpaSolver::sResults results; +// if(btGjkEpaSolver::Collide( shape0,body0->getWorldTransform(), +// shape1,body1->getWorldTransform(), +// radialmargin,results)) +// { +// dispatchInfo.m_debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); +// resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); +// } +// #else + + ConvexShape min0 = (ConvexShape) body0.getCollisionShape(); + ConvexShape min1 = (ConvexShape) body1.getCollisionShape(); + + ClosestPointInput input = pointInputsPool.get(); + input.init(); + + // JAVA NOTE: original: TODO: if (dispatchInfo.m_useContinuous) + gjkPairDetector.setMinkowskiA(min0); + gjkPairDetector.setMinkowskiB(min1); + input.maximumDistanceSquared = min0.getMargin() + min1.getMargin() + manifoldPtr.getContactBreakingThreshold(); + input.maximumDistanceSquared *= input.maximumDistanceSquared; + //input.m_stackAlloc = dispatchInfo.m_stackAllocator; + + // input.m_maximumDistanceSquared = btScalar(1e30); + + input.transformA.set(body0.getWorldTransform()); + input.transformB.set(body1.getWorldTransform()); + + gjkPairDetector.getClosestPoints(input, resultOut, dispatchInfo.debugDraw); + + pointInputsPool.release(input); + // #endif + + if (ownManifold) { + resultOut.refreshContactPoints(); + } + } + + private static boolean disableCcd = false; + + @Override + public float calculateTimeOfImpact(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + + // Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold + + // Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold + // col0->m_worldTransform, + float resultFraction = 1f; + + tmp.sub(col0.getInterpolationWorldTransform().origin, col0.getWorldTransform().origin); + float squareMot0 = tmp.lengthSquared(); + + tmp.sub(col1.getInterpolationWorldTransform().origin, col1.getWorldTransform().origin); + float squareMot1 = tmp.lengthSquared(); + + if (squareMot0 < col0.getCcdSquareMotionThreshold() && + squareMot1 < col1.getCcdSquareMotionThreshold()) { + return resultFraction; + } + + if (disableCcd) { + return 1f; + } + + // An adhoc way of testing the Continuous Collision Detection algorithms + // One object is approximated as a sphere, to simplify things + // Starting in penetration should report no time of impact + // For proper CCD, better accuracy and handling of 'allowed' penetration should be added + // also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies) + + // Convex0 against sphere for Convex1 + { + ConvexShape convex0 = (ConvexShape) col0.getCollisionShape(); + + SphereShape sphere1 = new SphereShape(col1.getCcdSweptSphereRadius()); // todo: allow non-zero sphere sizes, for better approximation + ConvexCast.CastResult result = new ConvexCast.CastResult(); + VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + GjkConvexCast ccd1 = new GjkConvexCast(convex0, sphere1, voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + if (ccd1.calcTimeOfImpact(col0.getWorldTransform(), col0.getInterpolationWorldTransform(), + col1.getWorldTransform(), col1.getInterpolationWorldTransform(), result)) { + // store result.m_fraction in both bodies + + if (col0.getHitFraction() > result.fraction) { + col0.setHitFraction(result.fraction); + } + + if (col1.getHitFraction() > result.fraction) { + col1.setHitFraction(result.fraction); + } + + if (resultFraction > result.fraction) { + resultFraction = result.fraction; + } + } + } + + // Sphere (for convex0) against Convex1 + { + ConvexShape convex1 = (ConvexShape) col1.getCollisionShape(); + + SphereShape sphere0 = new SphereShape(col0.getCcdSweptSphereRadius()); // todo: allow non-zero sphere sizes, for better approximation + ConvexCast.CastResult result = new ConvexCast.CastResult(); + VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver(); + //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex); + ///Simplification, one object is simplified as a sphere + GjkConvexCast ccd1 = new GjkConvexCast(sphere0, convex1, voronoiSimplex); + //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0); + if (ccd1.calcTimeOfImpact(col0.getWorldTransform(), col0.getInterpolationWorldTransform(), + col1.getWorldTransform(), col1.getInterpolationWorldTransform(), result)) { + //store result.m_fraction in both bodies + + if (col0.getHitFraction() > result.fraction) { + col0.setHitFraction(result.fraction); + } + + if (col1.getHitFraction() > result.fraction) { + col1.setHitFraction(result.fraction); + } + + if (resultFraction > result.fraction) { + resultFraction = result.fraction; + } + + } + } + + return resultFraction; + } + finally { + stack.vectors.pop(); + } + } + + public PersistentManifold getManifold() { + return manifoldPtr; + } + + //////////////////////////////////////////////////////////////////////////// + + public static class CreateFunc extends CollisionAlgorithmCreateFunc { + public ConvexPenetrationDepthSolver pdSolver; + public SimplexSolverInterface simplexSolver; + + public CreateFunc(SimplexSolverInterface simplexSolver, ConvexPenetrationDepthSolver pdSolver) { + this.simplexSolver = simplexSolver; + this.pdSolver = pdSolver; + } + + @Override + public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) { + //void* mem = ci.dispatcher1.allocateCollisionAlgorithm(sizeof(btConvexConvexAlgorithm)); + return new ConvexConvexAlgorithm(ci.manifold,ci,body0,body1,simplexSolver,pdSolver); + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/ConvexPlaneCollisionAlgorithm.java b/src/jbullet/src/javabullet/collision/dispatch/ConvexPlaneCollisionAlgorithm.java new file mode 100644 index 0000000..9fad739 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/ConvexPlaneCollisionAlgorithm.java @@ -0,0 +1,154 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import javabullet.collision.broadphase.CollisionAlgorithm; +import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo; +import javabullet.collision.broadphase.DispatcherInfo; +import javabullet.collision.narrowphase.PersistentManifold; +import javabullet.collision.shapes.ConvexShape; +import javabullet.collision.shapes.StaticPlaneShape; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class ConvexPlaneCollisionAlgorithm extends CollisionAlgorithm { + + private boolean ownManifold = false; + private PersistentManifold manifoldPtr; + private boolean isSwapped; + + public ConvexPlaneCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0, CollisionObject col1, boolean isSwapped) { + super(ci); + this.manifoldPtr = mf; + this.isSwapped = isSwapped; + + CollisionObject convexObj = isSwapped ? col1 : col0; + CollisionObject planeObj = isSwapped ? col0 : col1; + + if (manifoldPtr == null && dispatcher.needsCollision(convexObj, planeObj)) { + manifoldPtr = dispatcher.getNewManifold(convexObj, planeObj); + ownManifold = true; + } + } + + @Override + public void destroy() { + if (ownManifold) { + if (manifoldPtr != null) { + dispatcher.releaseManifold(manifoldPtr); + } + } + } + + + @Override + public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { + if (manifoldPtr == null) { + return; + } + + stack.pushCommonMath(); + try { + CollisionObject convexObj = isSwapped ? body1 : body0; + CollisionObject planeObj = isSwapped ? body0 : body1; + + ConvexShape convexShape = (ConvexShape) convexObj.getCollisionShape(); + StaticPlaneShape planeShape = (StaticPlaneShape) planeObj.getCollisionShape(); + + boolean hasCollision = false; + Vector3f planeNormal = planeShape.getPlaneNormal(); + float planeConstant = planeShape.getPlaneConstant(); + + Transform planeInConvex = stack.transforms.get(); + planeInConvex.inverse(convexObj.getWorldTransform()); + planeInConvex.mul(planeObj.getWorldTransform()); + + Transform convexInPlaneTrans = stack.transforms.get(); + convexInPlaneTrans.inverse(planeObj.getWorldTransform()); + convexInPlaneTrans.mul(convexObj.getWorldTransform()); + + Vector3f tmp = stack.vectors.get(); + tmp.negate(planeNormal); + planeInConvex.basis.transform(tmp); + + Vector3f vtx = stack.vectors.get(convexShape.localGetSupportingVertexWithoutMargin(tmp)); + Vector3f vtxInPlane = stack.vectors.get(vtx); + convexInPlaneTrans.transform(vtxInPlane); + + float distance = (planeNormal.dot(vtxInPlane) - planeConstant) - convexShape.getMargin(); + + Vector3f vtxInPlaneProjected = stack.vectors.get(); + tmp.scale(distance, planeNormal); + vtxInPlaneProjected.sub(vtxInPlane, tmp); + + Vector3f vtxInPlaneWorld = stack.vectors.get(vtxInPlaneProjected); + planeObj.getWorldTransform().transform(vtxInPlaneWorld); + + hasCollision = distance < manifoldPtr.getContactBreakingThreshold(); + resultOut.setPersistentManifold(manifoldPtr); + if (hasCollision) { + // report a contact. internally this will be kept persistent, and contact reduction is done + Vector3f normalOnSurfaceB = stack.vectors.get(planeNormal); + planeObj.getWorldTransform().basis.transform(normalOnSurfaceB); + + Vector3f pOnB = stack.vectors.get(vtxInPlaneWorld); + resultOut.addContactPoint(normalOnSurfaceB, pOnB, distance); + } + if (ownManifold) { + if (manifoldPtr.getNumContacts() != 0) { + resultOut.refreshContactPoints(); + } + } + } + finally { + stack.popCommonMath(); + } + } + + @Override + public float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { + // not yet + return 1f; + } + + //////////////////////////////////////////////////////////////////////////// + + public static class CreateFunc extends CollisionAlgorithmCreateFunc { + + @Override + public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) { + if (!swapped) { + return new ConvexPlaneCollisionAlgorithm(null, ci, body0, body1, false); + } + else { + return new ConvexPlaneCollisionAlgorithm(null, ci, body0, body1, true); + } + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/ConvexTriangleCallback.java b/src/jbullet/src/javabullet/collision/dispatch/ConvexTriangleCallback.java new file mode 100644 index 0000000..948b813 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/ConvexTriangleCallback.java @@ -0,0 +1,191 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import javabullet.BulletStack; +import javabullet.collision.broadphase.CollisionAlgorithm; +import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo; +import javabullet.collision.broadphase.Dispatcher; +import javabullet.collision.broadphase.DispatcherInfo; +import javabullet.collision.narrowphase.PersistentManifold; +import javabullet.collision.shapes.CollisionShape; +import javabullet.collision.shapes.TriangleCallback; +import javabullet.collision.shapes.TriangleShape; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * For each triangle in the concave mesh that overlaps with the AABB of a convex + * (convexProxy field), processTriangle is called. + * + * @author jezek2 + */ +class ConvexTriangleCallback implements TriangleCallback { + + protected final BulletStack stack = BulletStack.get(); + + private CollisionObject convexBody; + private CollisionObject triBody; + + private final Vector3f aabbMin = new Vector3f(); + private final Vector3f aabbMax = new Vector3f(); + + private ManifoldResult resultOut; + + private Dispatcher dispatcher; + private DispatcherInfo dispatchInfoPtr; + private float collisionMarginTriangle; + + public int triangleCount; + public PersistentManifold manifoldPtr; + + public ConvexTriangleCallback(Dispatcher dispatcher, CollisionObject body0, CollisionObject body1, boolean isSwapped) { + this.dispatcher = dispatcher; + this.dispatchInfoPtr = null; + + convexBody = isSwapped ? body1 : body0; + triBody = isSwapped ? body0 : body1; + + // + // create the manifold from the dispatcher 'manifold pool' + // + manifoldPtr = dispatcher.getNewManifold(convexBody, triBody); + + clearCache(); + } + + public void destroy() { + clearCache(); + dispatcher.releaseManifold(manifoldPtr); + } + + public void setTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { + stack.pushCommonMath(); + try { + this.dispatchInfoPtr = dispatchInfo; + this.collisionMarginTriangle = collisionMarginTriangle; + this.resultOut = resultOut; + + // recalc aabbs + Transform convexInTriangleSpace = stack.transforms.get(); + + convexInTriangleSpace.inverse(triBody.getWorldTransform()); + convexInTriangleSpace.mul(convexBody.getWorldTransform()); + + CollisionShape convexShape = (CollisionShape)convexBody.getCollisionShape(); + //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape); + convexShape.getAabb(convexInTriangleSpace, aabbMin, aabbMax); + float extraMargin = collisionMarginTriangle; + Vector3f extra = stack.vectors.get(extraMargin, extraMargin, extraMargin); + + aabbMax.add(extra); + aabbMin.sub(extra); + } + finally { + stack.popCommonMath(); + } + } + + private CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo(); + private TriangleShape tm = new TriangleShape(); + + public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) { + stack.vectors.push(); + try { + // just for debugging purposes + //printf("triangle %d",m_triangleCount++); + + // aabb filter is already applied! + + ci.dispatcher1 = dispatcher; + + CollisionObject ob = (CollisionObject) triBody; + + // debug drawing of the overlapping triangles + if (dispatchInfoPtr != null && dispatchInfoPtr.debugDraw != null && dispatchInfoPtr.debugDraw.getDebugMode() > 0) { + Vector3f color = stack.vectors.get(255, 255, 0); + Transform tr = ob.getWorldTransform(); + + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + tmp1.set(triangle[0]); tr.transform(tmp1); + tmp2.set(triangle[1]); tr.transform(tmp2); + dispatchInfoPtr.debugDraw.drawLine(tmp1, tmp2, color); + + tmp1.set(triangle[1]); tr.transform(tmp1); + tmp2.set(triangle[2]); tr.transform(tmp2); + dispatchInfoPtr.debugDraw.drawLine(tmp1, tmp2, color); + + tmp1.set(triangle[2]); tr.transform(tmp1); + tmp2.set(triangle[0]); tr.transform(tmp2); + dispatchInfoPtr.debugDraw.drawLine(tmp1, tmp2, color); + + //btVector3 center = triangle[0] + triangle[1]+triangle[2]; + //center *= btScalar(0.333333); + //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color); + //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color); + //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color); + } + + //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject); + + if (convexBody.getCollisionShape().isConvex()) { + tm.init(triangle[0], triangle[1], triangle[2]); + tm.setMargin(collisionMarginTriangle); + + CollisionShape tmpShape = ob.getCollisionShape(); + ob.setCollisionShape(tm); + + CollisionAlgorithm colAlgo = ci.dispatcher1.findAlgorithm(convexBody, triBody, manifoldPtr); + // this should use the btDispatcher, so the actual registered algorithm is used + // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); + + resultOut.setShapeIdentifiers(-1, -1, partId, triangleIndex); + //cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); + //cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); + colAlgo.processCollision(convexBody, triBody, dispatchInfoPtr, resultOut); + colAlgo.destroy(); + //ci.dispatcher1.freeCollisionAlgorithm(colAlgo); + ob.setCollisionShape(tmpShape); + } + } + finally { + stack.vectors.pop(); + } + } + + public void clearCache() { + dispatcher.clearManifold(manifoldPtr); + } + + public Vector3f getAabbMin() { + return aabbMin; + } + + public Vector3f getAabbMax() { + return aabbMax; + } + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/DefaultCollisionConfiguration.java b/src/jbullet/src/javabullet/collision/dispatch/DefaultCollisionConfiguration.java new file mode 100644 index 0000000..f38114e --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/DefaultCollisionConfiguration.java @@ -0,0 +1,197 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.collision.narrowphase.GjkEpaPenetrationDepthSolver; +import javabullet.collision.narrowphase.VoronoiSimplexSolver; +import static javabullet.collision.broadphase.BroadphaseNativeType.*; + +/** + * CollisionConfiguration allows to configure Bullet collision detection + * stack allocator, pool memory allocators + * todo: describe the meaning + * + * @author jezek2 + */ +public class DefaultCollisionConfiguration extends CollisionConfiguration { + + //default simplex/penetration depth solvers + private VoronoiSimplexSolver simplexSolver; + private GjkEpaPenetrationDepthSolver pdSolver; + + //default CreationFunctions, filling the m_doubleDispatch table + private CollisionAlgorithmCreateFunc convexConvexCreateFunc; + private CollisionAlgorithmCreateFunc convexConcaveCreateFunc; + private CollisionAlgorithmCreateFunc swappedConvexConcaveCreateFunc; + private CollisionAlgorithmCreateFunc compoundCreateFunc; + private CollisionAlgorithmCreateFunc swappedCompoundCreateFunc; + private CollisionAlgorithmCreateFunc emptyCreateFunc; + private CollisionAlgorithmCreateFunc sphereSphereCF; + private CollisionAlgorithmCreateFunc sphereBoxCF; + private CollisionAlgorithmCreateFunc boxSphereCF; + private CollisionAlgorithmCreateFunc sphereTriangleCF; + private CollisionAlgorithmCreateFunc triangleSphereCF; + private CollisionAlgorithmCreateFunc planeConvexCF; + private CollisionAlgorithmCreateFunc convexPlaneCF; + + public DefaultCollisionConfiguration() { + simplexSolver = new VoronoiSimplexSolver(); + pdSolver = new GjkEpaPenetrationDepthSolver(); + + /* + //default CreationFunctions, filling the m_doubleDispatch table + */ + convexConvexCreateFunc = new ConvexConvexAlgorithm.CreateFunc(simplexSolver, pdSolver); + convexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.CreateFunc(); + swappedConvexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.SwappedCreateFunc(); + compoundCreateFunc = CompoundCollisionAlgorithm.createFunc; + swappedCompoundCreateFunc = CompoundCollisionAlgorithm.swappedCreateFunc; + emptyCreateFunc = EmptyAlgorithm.createFunc; + + sphereSphereCF = SphereSphereCollisionAlgorithm.createFunc; + /* + m_sphereBoxCF = new(mem) btSphereBoxCollisionAlgorithm::CreateFunc; + m_boxSphereCF = new (mem)btSphereBoxCollisionAlgorithm::CreateFunc; + m_boxSphereCF->m_swapped = true; + m_sphereTriangleCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; + m_triangleSphereCF = new (mem)btSphereTriangleCollisionAlgorithm::CreateFunc; + m_triangleSphereCF->m_swapped = true; + */ + + // convex versus plane + convexPlaneCF = new ConvexPlaneCollisionAlgorithm.CreateFunc(); + planeConvexCF = new ConvexPlaneCollisionAlgorithm.CreateFunc(); + planeConvexCF.swapped = true; + + /* + ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool + int maxSize = sizeof(btConvexConvexAlgorithm); + int maxSize2 = sizeof(btConvexConcaveCollisionAlgorithm); + int maxSize3 = sizeof(btCompoundCollisionAlgorithm); + int maxSize4 = sizeof(btEmptyAlgorithm); + + int collisionAlgorithmMaxElementSize = btMax(maxSize,maxSize2); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize3); + collisionAlgorithmMaxElementSize = btMax(collisionAlgorithmMaxElementSize,maxSize4); + + if (stackAlloc) + { + m_ownsStackAllocator = false; + this->m_stackAlloc = stackAlloc; + } else + { + m_ownsStackAllocator = true; + void* mem = btAlignedAlloc(sizeof(btStackAlloc),16); + m_stackAlloc = new(mem)btStackAlloc(DEFAULT_STACK_ALLOCATOR_SIZE); + } + + if (persistentManifoldPool) + { + m_ownsPersistentManifoldPool = false; + m_persistentManifoldPool = persistentManifoldPool; + } else + { + m_ownsPersistentManifoldPool = true; + void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); + m_persistentManifoldPool = new (mem) btPoolAllocator(sizeof(btPersistentManifold),DEFAULT_MAX_OVERLAPPING_PAIRS); + } + + if (collisionAlgorithmPool) + { + m_ownsCollisionAlgorithmPool = false; + m_collisionAlgorithmPool = collisionAlgorithmPool; + } else + { + m_ownsCollisionAlgorithmPool = true; + void* mem = btAlignedAlloc(sizeof(btPoolAllocator),16); + m_collisionAlgorithmPool = new(mem) btPoolAllocator(collisionAlgorithmMaxElementSize,DEFAULT_MAX_OVERLAPPING_PAIRS); + } + */ + } + + @Override + public CollisionAlgorithmCreateFunc getCollisionAlgorithmCreateFunc(BroadphaseNativeType proxyType0, BroadphaseNativeType proxyType1) { + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1 == SPHERE_SHAPE_PROXYTYPE)) { + return sphereSphereCF; + } + + /* + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE) && (proxyType1==BOX_SHAPE_PROXYTYPE)) + { + return m_sphereBoxCF; + } + + if ((proxyType0 == BOX_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + { + return m_boxSphereCF; + } + + if ((proxyType0 == SPHERE_SHAPE_PROXYTYPE ) && (proxyType1==TRIANGLE_SHAPE_PROXYTYPE)) + { + return m_sphereTriangleCF; + } + + if ((proxyType0 == TRIANGLE_SHAPE_PROXYTYPE ) && (proxyType1==SPHERE_SHAPE_PROXYTYPE)) + { + return m_triangleSphereCF; + } + */ + + if (proxyType0.isConvex() && (proxyType1 == STATIC_PLANE_PROXYTYPE)) + { + return convexPlaneCF; + } + + if (proxyType1.isConvex() && (proxyType0 == STATIC_PLANE_PROXYTYPE)) + { + return planeConvexCF; + } + + if (proxyType0.isConvex() && proxyType1.isConvex()) { + return convexConvexCreateFunc; + } + + if (proxyType0.isConvex() && proxyType1.isConcave()) { + return convexConcaveCreateFunc; + } + + if (proxyType1.isConvex() && proxyType0.isConcave()) { + return swappedConvexConcaveCreateFunc; + } + + if (proxyType0.isCompound()) { + return compoundCreateFunc; + } + else { + if (proxyType1.isCompound()) { + return swappedCompoundCreateFunc; + } + } + + // failed to find an algorithm + return emptyCreateFunc; + } + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/EmptyAlgorithm.java b/src/jbullet/src/javabullet/collision/dispatch/EmptyAlgorithm.java new file mode 100644 index 0000000..470ad21 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/EmptyAlgorithm.java @@ -0,0 +1,62 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import javabullet.collision.broadphase.CollisionAlgorithm; +import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo; +import javabullet.collision.broadphase.DispatcherInfo; + +/** + * + * @author jezek2 + */ +public class EmptyAlgorithm extends CollisionAlgorithm { + + public EmptyAlgorithm(CollisionAlgorithmConstructionInfo ci) { + super(ci); + } + + @Override + public void destroy() { + } + + @Override + public void processCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { + } + + @Override + public float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { + return 1f; + } + + //////////////////////////////////////////////////////////////////////////// + + public static final CollisionAlgorithmCreateFunc createFunc = new CollisionAlgorithmCreateFunc() { + @Override + public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) { + return new EmptyAlgorithm(ci); + } + }; + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/ManifoldResult.java b/src/jbullet/src/javabullet/collision/dispatch/ManifoldResult.java new file mode 100644 index 0000000..38cff45 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/ManifoldResult.java @@ -0,0 +1,186 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import javabullet.BulletGlobals; +import javabullet.BulletPool; +import javabullet.BulletStack; +import javabullet.ObjectPool; +import javabullet.collision.narrowphase.DiscreteCollisionDetectorInterface; +import javabullet.collision.narrowphase.ManifoldPoint; +import javabullet.collision.narrowphase.PersistentManifold; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * ManifoldResult is a helper class to manage contact results. + * + * @author jezek2 + */ +public class ManifoldResult implements DiscreteCollisionDetectorInterface.Result { + + protected final BulletStack stack = BulletStack.get(); + protected final ObjectPool<ManifoldPoint> pointsPool = BulletPool.get(ManifoldPoint.class); + + private PersistentManifold manifoldPtr; + + // we need this for compounds + private final Transform rootTransA = new Transform(); + private final Transform rootTransB = new Transform(); + private CollisionObject body0; + private CollisionObject body1; + private int partId0; + private int partId1; + private int index0; + private int index1; + + public ManifoldResult() { + } + + public ManifoldResult(CollisionObject body0, CollisionObject body1) { + init(body0, body1); + } + + public void init(CollisionObject body0, CollisionObject body1) { + this.body0 = body0; + this.body1 = body1; + this.rootTransA.set(body0.getWorldTransform()); + this.rootTransB.set(body1.getWorldTransform()); + } + + public PersistentManifold getPersistentManifold() { + return manifoldPtr; + } + + public void setPersistentManifold(PersistentManifold manifoldPtr) { + this.manifoldPtr = manifoldPtr; + } + + public void setShapeIdentifiers(int partId0, int index0, int partId1, int index1) { + this.partId0 = partId0; + this.partId1 = partId1; + this.index0 = index0; + this.index1 = index1; + } + + public void addContactPoint(Vector3f normalOnBInWorld, Vector3f pointInWorld, float depth) { + assert (manifoldPtr != null); + //order in manifold needs to match + + if (depth > manifoldPtr.getContactBreakingThreshold()) { + return; + } + + stack.vectors.push(); + try { + boolean isSwapped = manifoldPtr.getBody0() != body0; + + Vector3f pointA = stack.vectors.get(); + pointA.scaleAdd(depth, normalOnBInWorld, pointInWorld); + + Vector3f localA = stack.vectors.get(); + Vector3f localB = stack.vectors.get(); + + if (isSwapped) { + rootTransB.invXform(pointA, localA); + rootTransA.invXform(pointInWorld, localB); + } + else { + rootTransA.invXform(pointA, localA); + rootTransB.invXform(pointInWorld, localB); + } + + ManifoldPoint newPt = pointsPool.get(); + newPt.init(localA, localB, normalOnBInWorld, depth); + + newPt.positionWorldOnA.set(pointA); + newPt.positionWorldOnB.set(pointInWorld); + + int insertIndex = manifoldPtr.getCacheEntry(newPt); + + newPt.combinedFriction = calculateCombinedFriction(body0, body1); + newPt.combinedRestitution = calculateCombinedRestitution(body0, body1); + + + /// todo, check this for any side effects + if (insertIndex >= 0) { + //const btManifoldPoint& oldPoint = m_manifoldPtr->getContactPoint(insertIndex); + manifoldPtr.replaceContactPoint(newPt, insertIndex); + } + else { + manifoldPtr.addManifoldPoint(newPt); + } + + // User can override friction and/or restitution + if (BulletGlobals.gContactAddedCallback != null && + // and if either of the two bodies requires custom material + ((body0.getCollisionFlags() & CollisionFlags.CUSTOM_MATERIAL_CALLBACK) != 0 || + (body1.getCollisionFlags() & CollisionFlags.CUSTOM_MATERIAL_CALLBACK) != 0)) { + //experimental feature info, for per-triangle material etc. + CollisionObject obj0 = isSwapped ? body1 : body0; + CollisionObject obj1 = isSwapped ? body0 : body1; + BulletGlobals.gContactAddedCallback.invoke(newPt, obj0, partId0, index0, obj1, partId1, index1); + } + + pointsPool.release(newPt); + } + finally { + stack.vectors.pop(); + } + } + + ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; + private static float calculateCombinedFriction(CollisionObject body0, CollisionObject body1) { + float friction = body0.getFriction() * body1.getFriction(); + + float MAX_FRICTION = 10f; + if (friction < -MAX_FRICTION) { + friction = -MAX_FRICTION; + } + if (friction > MAX_FRICTION) { + friction = MAX_FRICTION; + } + return friction; + } + + private static float calculateCombinedRestitution(CollisionObject body0, CollisionObject body1) { + return body0.getRestitution() * body1.getRestitution(); + } + + public void refreshContactPoints() { + assert (manifoldPtr != null); + if (manifoldPtr.getNumContacts() == 0) { + return; + } + + boolean isSwapped = manifoldPtr.getBody0() != body0; + + if (isSwapped) { + manifoldPtr.refreshContactPoints(rootTransB, rootTransA); + } + else { + manifoldPtr.refreshContactPoints(rootTransA, rootTransB); + } + } +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/NearCallback.java b/src/jbullet/src/javabullet/collision/dispatch/NearCallback.java new file mode 100644 index 0000000..b695073 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/NearCallback.java @@ -0,0 +1,37 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import javabullet.collision.broadphase.BroadphasePair; +import javabullet.collision.broadphase.DispatcherInfo; + +/** + * + * @author jezek2 + */ +public interface NearCallback { + + public void invoke(BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo); + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/SimulationIslandManager.java b/src/jbullet/src/javabullet/collision/dispatch/SimulationIslandManager.java new file mode 100644 index 0000000..a760bff --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/SimulationIslandManager.java @@ -0,0 +1,335 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import java.util.ArrayList; +import java.util.Comparator; +import java.util.List; +import javabullet.BulletGlobals; +import javabullet.collision.broadphase.BroadphasePair; +import javabullet.collision.broadphase.Dispatcher; +import javabullet.collision.narrowphase.PersistentManifold; +import javabullet.linearmath.MiscUtil; +import javabullet.util.HashUtil.IMap; +import javabullet.util.HashUtil.IObjectProcedure; + +/** + * SimulationIslandManager creates and handles simulation islands, using UnionFind. + * + * @author jezek2 + */ +public class SimulationIslandManager { + + private final UnionFind unionFind = new UnionFind(); + + private final List<PersistentManifold> islandmanifold = new ArrayList<PersistentManifold>(); + private final List<CollisionObject> islandBodies = new ArrayList<CollisionObject>(); + + public void initUnionFind(int n) { + unionFind.reset(n); + } + + public UnionFind getUnionFind() { + return unionFind; + } + + private class FindUnionsCallback implements IObjectProcedure<BroadphasePair> { + public boolean execute(BroadphasePair collisionPair) { + CollisionObject colObj0 = (CollisionObject) collisionPair.pProxy0.clientObject; + CollisionObject colObj1 = (CollisionObject) collisionPair.pProxy1.clientObject; + + if (((colObj0 != null) && ((colObj0).mergesSimulationIslands())) && + ((colObj1 != null) && ((colObj1).mergesSimulationIslands()))) { + unionFind.unite((colObj0).getIslandTag(), (colObj1).getIslandTag()); + } + return true; + } + } + + private FindUnionsCallback findUnionsCallback = new FindUnionsCallback(); + + public void findUnions(Dispatcher dispatcher, CollisionWorld colWorld) { + IMap<BroadphasePair,BroadphasePair> pairPtr = colWorld.getPairCache().getOverlappingPairArray(); + pairPtr.forEachValue(findUnionsCallback); + } + + public void updateActivationState(CollisionWorld colWorld, Dispatcher dispatcher) { + initUnionFind(colWorld.getCollisionObjectArray().size()); + + // put the index into m_controllers into m_tag + { + int index = 0; + int i; + for (i = 0; i < colWorld.getCollisionObjectArray().size(); i++) { + CollisionObject collisionObject = colWorld.getCollisionObjectArray().get(i); + collisionObject.setIslandTag(index); + collisionObject.setCompanionId(-1); + collisionObject.setHitFraction(1f); + index++; + } + } + // do the union find + + findUnions(dispatcher, colWorld); + } + + public void storeIslandActivationState(CollisionWorld colWorld) { + // put the islandId ('find' value) into m_tag + { + int index = 0; + int i; + for (i = 0; i < colWorld.getCollisionObjectArray().size(); i++) { + CollisionObject collisionObject = colWorld.getCollisionObjectArray().get(i); + if (collisionObject.mergesSimulationIslands()) { + collisionObject.setIslandTag(unionFind.find(index)); + collisionObject.setCompanionId(-1); + } + else { + collisionObject.setIslandTag(-1); + collisionObject.setCompanionId(-2); + } + index++; + } + } + } + + private static int getIslandId(PersistentManifold lhs) { + int islandId; + CollisionObject rcolObj0 = (CollisionObject) lhs.getBody0(); + CollisionObject rcolObj1 = (CollisionObject) lhs.getBody1(); + islandId = rcolObj0.getIslandTag() >= 0? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); + return islandId; + } + + public void buildAndProcessIslands(Dispatcher dispatcher, List<CollisionObject> collisionObjects, IslandCallback callback) { + BulletGlobals.pushProfile("islandUnionFindAndHeapSort"); + try { + // we are going to sort the unionfind array, and store the element id in the size + // afterwards, we clean unionfind, to make sure no-one uses it anymore + + getUnionFind().sortIslands(); + int numElem = getUnionFind().getNumElements(); + + int endIslandIndex = 1; + int startIslandIndex; + + // update the sleeping state for bodies, if all are sleeping + for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) { + int islandId = getUnionFind().getElement(startIslandIndex).id; + for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).id == islandId); endIslandIndex++) { + } + + //int numSleeping = 0; + + boolean allSleeping = true; + + int idx; + for (idx = startIslandIndex; idx < endIslandIndex; idx++) { + int i = getUnionFind().getElement(idx).sz; + + CollisionObject colObj0 = collisionObjects.get(i); + if ((colObj0.getIslandTag() != islandId) && (colObj0.getIslandTag() != -1)) { + System.err.println("error in island management\n"); + } + + assert ((colObj0.getIslandTag() == islandId) || (colObj0.getIslandTag() == -1)); + if (colObj0.getIslandTag() == islandId) { + if (colObj0.getActivationState() == CollisionObject.ACTIVE_TAG) { + allSleeping = false; + } + if (colObj0.getActivationState() == CollisionObject.DISABLE_DEACTIVATION) { + allSleeping = false; + } + } + } + + + if (allSleeping) { + //int idx; + for (idx = startIslandIndex; idx < endIslandIndex; idx++) { + int i = getUnionFind().getElement(idx).sz; + CollisionObject colObj0 = collisionObjects.get(i); + if ((colObj0.getIslandTag() != islandId) && (colObj0.getIslandTag() != -1)) { + System.err.println("error in island management\n"); + } + + assert ((colObj0.getIslandTag() == islandId) || (colObj0.getIslandTag() == -1)); + + if (colObj0.getIslandTag() == islandId) { + colObj0.setActivationState(CollisionObject.ISLAND_SLEEPING); + } + } + } + else { + + //int idx; + for (idx = startIslandIndex; idx < endIslandIndex; idx++) { + int i = getUnionFind().getElement(idx).sz; + + CollisionObject colObj0 = collisionObjects.get(i); + if ((colObj0.getIslandTag() != islandId) && (colObj0.getIslandTag() != -1)) { + System.err.println("error in island management\n"); + } + + assert ((colObj0.getIslandTag() == islandId) || (colObj0.getIslandTag() == -1)); + + if (colObj0.getIslandTag() == islandId) { + if (colObj0.getActivationState() == CollisionObject.ISLAND_SLEEPING) { + colObj0.setActivationState(CollisionObject.WANTS_DEACTIVATION); + } + } + } + } + } + + + int i; + int maxNumManifolds = dispatcher.getNumManifolds(); + + //#define SPLIT_ISLANDS 1 + //#ifdef SPLIT_ISLANDS + //#endif //SPLIT_ISLANDS + + for (i = 0; i < maxNumManifolds; i++) { + PersistentManifold manifold = dispatcher.getManifoldByIndexInternal(i); + + CollisionObject colObj0 = (CollisionObject) manifold.getBody0(); + CollisionObject colObj1 = (CollisionObject) manifold.getBody1(); + + // todo: check sleeping conditions! + if (((colObj0 != null) && colObj0.getActivationState() != CollisionObject.ISLAND_SLEEPING) || + ((colObj1 != null) && colObj1.getActivationState() != CollisionObject.ISLAND_SLEEPING)) { + + // kinematic objects don't merge islands, but wake up all connected objects + if (colObj0.isKinematicObject() && colObj0.getActivationState() != CollisionObject.ISLAND_SLEEPING) { + colObj1.activate(); + } + if (colObj1.isKinematicObject() && colObj1.getActivationState() != CollisionObject.ISLAND_SLEEPING) { + colObj0.activate(); + } + //#ifdef SPLIT_ISLANDS + //filtering for response + if (dispatcher.needsResponse(colObj0, colObj1)) { + islandmanifold.add(manifold); + } + //#endif //SPLIT_ISLANDS + } + } + + //#ifndef SPLIT_ISLANDS + //btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); + // + //callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); + //#else + // Sort manifolds, based on islands + // Sort the vector using predicate and std::sort + //std::sort(islandmanifold.begin(), islandmanifold.end(), btPersistentManifoldSortPredicate); + + int numManifolds = islandmanifold.size(); + + // we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) + //islandmanifold.heapSort(btPersistentManifoldSortPredicate()); + + // JAVA NOTE: memory optimized sorting with caching of temporary array + //Collections.sort(islandmanifold, persistentManifoldComparator); + MiscUtil.heapSort(islandmanifold, persistentManifoldComparator); + + // now process all active islands (sets of manifolds for now) + + int startManifoldIndex = 0; + int endManifoldIndex = 1; + + //int islandId; + + //printf("Start Islands\n"); + + // traverse the simulation islands, and call the solver, unless all objects are sleeping/deactivated + for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex) { + int islandId = getUnionFind().getElement(startIslandIndex).id; + boolean islandSleeping = false; + + for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (getUnionFind().getElement(endIslandIndex).id == islandId); endIslandIndex++) { + /*int*/ i = getUnionFind().getElement(endIslandIndex).sz; + CollisionObject colObj0 = collisionObjects.get(i); + islandBodies.add(colObj0); + if (!colObj0.isActive()) { + islandSleeping = true; + } + } + + + // find the accompanying contact manifold for this islandId + int numIslandManifolds = 0; + //List<PersistentManifold> startManifold = null; + int startManifold_idx = -1; + + if (startManifoldIndex < numManifolds) { + int curIslandId = getIslandId(islandmanifold.get(startManifoldIndex)); + if (curIslandId == islandId) { + //startManifold = &m_islandmanifold[startManifoldIndex]; + //startManifold = islandmanifold.subList(startManifoldIndex, islandmanifold.size()); + startManifold_idx = startManifoldIndex; + + for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == getIslandId(islandmanifold.get(endManifoldIndex))); endManifoldIndex++) { + + } + // Process the actual simulation, only if not sleeping/deactivated + numIslandManifolds = endManifoldIndex - startManifoldIndex; + } + + } + + if (!islandSleeping) { + callback.processIsland(islandBodies, islandBodies.size(), islandmanifold, startManifold_idx, numIslandManifolds, islandId); + //printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); + } + + if (numIslandManifolds != 0) { + startManifoldIndex = endManifoldIndex; + } + + islandBodies.clear(); + } + //#endif //SPLIT_ISLANDS + + islandmanifold.clear(); + } + finally { + BulletGlobals.popProfile(); + } + } + + //////////////////////////////////////////////////////////////////////////// + + public interface IslandCallback { + public void processIsland(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifolds, int manifolds_offset, int numManifolds, int islandId); + } + + private static final Comparator<PersistentManifold> persistentManifoldComparator = new Comparator<PersistentManifold>() { + public int compare(PersistentManifold lhs, PersistentManifold rhs) { + return getIslandId(lhs) < getIslandId(rhs)? -1 : +1; + } + }; + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/SphereSphereCollisionAlgorithm.java b/src/jbullet/src/javabullet/collision/dispatch/SphereSphereCollisionAlgorithm.java new file mode 100644 index 0000000..66f73a2 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/SphereSphereCollisionAlgorithm.java @@ -0,0 +1,137 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javabullet.collision.broadphase.CollisionAlgorithm; +import javabullet.collision.broadphase.CollisionAlgorithmConstructionInfo; +import javabullet.collision.broadphase.DispatcherInfo; +import javabullet.collision.narrowphase.PersistentManifold; +import javabullet.collision.shapes.SphereShape; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class SphereSphereCollisionAlgorithm extends CollisionAlgorithm { + + private boolean ownManifold; + private PersistentManifold manifoldPtr; + + public SphereSphereCollisionAlgorithm(PersistentManifold mf, CollisionAlgorithmConstructionInfo ci, CollisionObject col0, CollisionObject col1) { + super(ci); + manifoldPtr = mf; + + if (manifoldPtr == null) { + manifoldPtr = dispatcher.getNewManifold(col0, col1); + ownManifold = true; + } + } + + public SphereSphereCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci) { + super(ci); + } + + @Override + public void destroy() { + if (ownManifold) { + if (manifoldPtr != null) { + dispatcher.releaseManifold(manifoldPtr); + } + } + } + + @Override + public void processCollision(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { + if (manifoldPtr == null) { + return; + } + + stack.vectors.push(); + try { + resultOut.setPersistentManifold(manifoldPtr); + + SphereShape sphere0 = (SphereShape) col0.getCollisionShape(); + SphereShape sphere1 = (SphereShape) col1.getCollisionShape(); + + Vector3f diff = stack.vectors.get(); + diff.sub(col0.getWorldTransform().origin, col1.getWorldTransform().origin); + + float len = diff.length(); + float radius0 = sphere0.getRadius(); + float radius1 = sphere1.getRadius(); + + manifoldPtr.clearManifold(); + + // if distance positive, don't generate a new contact + if (len > (radius0 + radius1)) { + return; + } + // distance (negative means penetration) + float dist = len - (radius0 + radius1); + + Vector3f normalOnSurfaceB = stack.vectors.get(1f, 0f, 0f); + if (len > BulletGlobals.FLT_EPSILON) { + normalOnSurfaceB.scale(1f / len, diff); + } + + Vector3f tmp = stack.vectors.get(); + + // point on A (worldspace) + Vector3f pos0 = stack.vectors.get(); + tmp.scale(radius0, normalOnSurfaceB); + pos0.sub(col0.getWorldTransform().origin, tmp); + + // point on B (worldspace) + Vector3f pos1 = stack.vectors.get(); + tmp.scale(radius1, normalOnSurfaceB); + pos1.add(col1.getWorldTransform().origin, tmp); + + // report a contact. internally this will be kept persistent, and contact reduction is done + resultOut.addContactPoint(normalOnSurfaceB, pos1, dist); + + //no resultOut->refreshContactPoints(); needed, because of clearManifold (all points are new) + } + finally { + stack.vectors.pop(); + } + } + + @Override + public float calculateTimeOfImpact(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { + return 1f; + } + + //////////////////////////////////////////////////////////////////////////// + + public static final CollisionAlgorithmCreateFunc createFunc = new CollisionAlgorithmCreateFunc() { + @Override + public CollisionAlgorithm createCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1) { + return new SphereSphereCollisionAlgorithm(null, ci, body0, body1); + } + }; + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/UnionFind.java b/src/jbullet/src/javabullet/collision/dispatch/UnionFind.java new file mode 100644 index 0000000..a4419ed --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/UnionFind.java @@ -0,0 +1,149 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.dispatch; + +import java.util.ArrayList; +import java.util.Comparator; +import java.util.List; +import javabullet.linearmath.MiscUtil; + +/** + * UnionFind calculates connected subsets. + * Implements weighted Quick Union with path compression. + * Optimization: could use short ints instead of ints (halving memory, would limit the number of rigid bodies to 64k, sounds reasonable). + * + * @author jezek2 + */ +public class UnionFind { + + private final List<Element> elements = new ArrayList<Element>(); + + /** + * This is a special operation, destroying the content of UnionFind. + * It sorts the elements, based on island id, in order to make it easy to iterate over islands. + */ + public void sortIslands() { + // first store the original body index, and islandId + int numElements = elements.size(); + + for (int i = 0; i < numElements; i++) { + elements.get(i).id = find(i); + elements.get(i).sz = i; + } + + // Sort the vector using predicate and std::sort + //std::sort(m_elements.begin(), m_elements.end(), btUnionFindElementSortPredicate); + //perhaps use radix sort? + //elements.heapSort(btUnionFindElementSortPredicate()); + + //Collections.sort(elements); + MiscUtil.heapSort(elements, elementComparator); + } + + public void reset(int N) { + allocate(N); + + for (int i = 0; i < N; i++) { + elements.get(i).id = i; + elements.get(i).sz = 1; + } + } + + public int getNumElements() { + return elements.size(); + } + + public boolean isRoot(int x) { + return (x == elements.get(x).id); + } + + public Element getElement(int index) { + return elements.get(index); + } + + public void allocate(int N) { + MiscUtil.resize(elements, N, Element.class); + } + + public void free() { + elements.clear(); + } + + public int find(int p, int q) { + return (find(p) == find(q))? 1 : 0; + } + + public void unite(int p, int q) { + int i = find(p), j = find(q); + if (i == j) { + return; + } + + //#ifndef USE_PATH_COMPRESSION + ////weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) ) + //if (m_elements[i].m_sz < m_elements[j].m_sz) + //{ + // m_elements[i].m_id = j; m_elements[j].m_sz += m_elements[i].m_sz; + //} + //else + //{ + // m_elements[j].m_id = i; m_elements[i].m_sz += m_elements[j].m_sz; + //} + //#else + elements.get(i).id = j; + elements.get(j).sz += elements.get(i).sz; + //#endif //USE_PATH_COMPRESSION + } + + public int find(int x) { + //assert(x < m_N); + //assert(x >= 0); + + while (x != elements.get(x).id) { + // not really a reason not to use path compression, and it flattens the trees/improves find performance dramatically + + //#ifdef USE_PATH_COMPRESSION + elements.get(x).id = elements.get(elements.get(x).id).id; + //#endif // + x = elements.get(x).id; + //assert(x < m_N); + //assert(x >= 0); + } + return x; + } + + //////////////////////////////////////////////////////////////////////////// + + public static class Element { + public int id; + public int sz; + } + + private static final Comparator<Element> elementComparator = new Comparator<Element>() { + public int compare(Element o1, Element o2) { + return o1.id < o2.id? -1 : +1; + } + }; + +} diff --git a/src/jbullet/src/javabullet/collision/dispatch/package-info.java b/src/jbullet/src/javabullet/collision/dispatch/package-info.java new file mode 100644 index 0000000..683537c --- /dev/null +++ b/src/jbullet/src/javabullet/collision/dispatch/package-info.java @@ -0,0 +1,28 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +/** + * Dispatching code for collisions between various shapes. + */ +package javabullet.collision.dispatch; + diff --git a/src/jbullet/src/javabullet/collision/narrowphase/ConvexCast.java b/src/jbullet/src/javabullet/collision/narrowphase/ConvexCast.java new file mode 100644 index 0000000..fdf63e3 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/ConvexCast.java @@ -0,0 +1,60 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javabullet.linearmath.IDebugDraw; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * ConvexCast is an interface for Casting. + * + * @author jezek2 + */ +public interface ConvexCast { + + /** + * Cast a convex against another convex object. + */ + public boolean calcTimeOfImpact(Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result); + + //////////////////////////////////////////////////////////////////////////// + + /** + * RayResult stores the closest result + * alternatively, add a callback method to decide about closest/all results + */ + public static class CastResult { + public final Vector3f normal = new Vector3f(); + public final Vector3f hitPoint = new Vector3f(); + public float fraction = 1e30f; + public final Transform hitTransformA = new Transform(); + public final Transform hitTransformB = new Transform(); + public IDebugDraw debugDrawer; + + public void debugDraw(float fraction) {} + public void drawCoordSystem(Transform trans) {} + } + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/ConvexPenetrationDepthSolver.java b/src/jbullet/src/javabullet/collision/narrowphase/ConvexPenetrationDepthSolver.java new file mode 100644 index 0000000..2b1ae15 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/ConvexPenetrationDepthSolver.java @@ -0,0 +1,44 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javabullet.collision.shapes.ConvexShape; +import javabullet.linearmath.IDebugDraw; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * ConvexPenetrationDepthSolver provides an interface for penetration depth calculation. + * + * @author jezek2 + */ +public interface ConvexPenetrationDepthSolver { + + public boolean calcPenDepth(SimplexSolverInterface simplexSolver, + ConvexShape convexA, ConvexShape convexB, + Transform transA, Transform transB, + Vector3f v, Vector3f pa, Vector3f pb, + IDebugDraw debugDraw/*, btStackAlloc* stackAlloc*/); + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/DiscreteCollisionDetectorInterface.java b/src/jbullet/src/javabullet/collision/narrowphase/DiscreteCollisionDetectorInterface.java new file mode 100644 index 0000000..fcd453c --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/DiscreteCollisionDetectorInterface.java @@ -0,0 +1,69 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javabullet.linearmath.IDebugDraw; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * This interface is made to be used by an iterative approach to do TimeOfImpact calculations. + * This interface allows to query for closest points and penetration depth between two (convex) objects + * the closest point is on the second object (B), and the normal points from the surface on B towards A. + * distance is between closest points on B and closest point on A. So you can calculate closest point on A + * by taking closestPointInA = closestPointInB + m_distance * m_normalOnSurfaceB + * + * @author jezek2 + */ +public interface DiscreteCollisionDetectorInterface { + + public interface Result { + ///setShapeIdentifiers provides experimental support for per-triangle material / custom material combiner + public void setShapeIdentifiers(int partId0, int index0, int partId1, int index1); + + public void addContactPoint(Vector3f normalOnBInWorld, Vector3f pointInWorld, float depth); + } + + public static class ClosestPointInput { + public final Transform transformA = new Transform(); + public final Transform transformB = new Transform(); + public float maximumDistanceSquared; + //btStackAlloc* m_stackAlloc; + + public ClosestPointInput() { + init(); + } + + public void init() { + maximumDistanceSquared = Float.MAX_VALUE; + } + } + + /** + * Give either closest points (distance > 0) or penetration (distance) + * the normal always points from B towards A. + */ + public void getClosestPoints(ClosestPointInput input,Result output, IDebugDraw debugDraw); + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/GjkConvexCast.java b/src/jbullet/src/javabullet/collision/narrowphase/GjkConvexCast.java new file mode 100644 index 0000000..a5daad0 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/GjkConvexCast.java @@ -0,0 +1,207 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javabullet.BulletGlobals; +import javabullet.BulletPool; +import javabullet.BulletStack; +import javabullet.ObjectPool; +import javabullet.collision.narrowphase.DiscreteCollisionDetectorInterface.ClosestPointInput; +import javabullet.collision.shapes.ConvexShape; +import javabullet.collision.shapes.MinkowskiSumShape; +import javabullet.collision.shapes.SphereShape; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * GjkConvexCast performs a raycast on a convex object using support mapping. + * + * @author jezek2 + */ +public class GjkConvexCast implements ConvexCast { + + protected final BulletStack stack = BulletStack.get(); + protected final ObjectPool<ClosestPointInput> pointInputsPool = BulletPool.get(ClosestPointInput.class); + + private SimplexSolverInterface simplexSolver; + private ConvexShape convexA; + private ConvexShape convexB; + + public GjkConvexCast(ConvexShape convexA, ConvexShape convexB, SimplexSolverInterface simplexSolver) { + this.simplexSolver = simplexSolver; + this.convexA = convexA; + this.convexB = convexB; + } + + public boolean calcTimeOfImpact(Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result) { + stack.pushCommonMath(); + try { + MinkowskiSumShape combi = new MinkowskiSumShape(convexA, convexB); + MinkowskiSumShape convex = combi; + + Transform rayFromLocalA = stack.transforms.get(); + Transform rayToLocalA = stack.transforms.get(); + + rayFromLocalA.inverse(fromA); + rayFromLocalA.mul(fromB); + + rayToLocalA.inverse(toA); + rayToLocalA.mul(toB); + + Transform trA = stack.transforms.get(), trB = stack.transforms.get(); + trA.set(fromA); + trB.set(fromB); + trA.origin.set(0f, 0f, 0f); + trB.origin.set(0f, 0f, 0f); + + convex.setTransformA(trA); + convex.setTransformB(trB); + + float radius = 0.01f; + + float lambda = 0f; + Vector3f s = stack.vectors.get(rayFromLocalA.origin); + Vector3f r = stack.vectors.get(); + r.sub(rayToLocalA.origin, rayFromLocalA.origin); + Vector3f x = stack.vectors.get(s); + Vector3f n = stack.vectors.get(); + n.set(0f, 0f, 0f); + boolean hasResult = false; + Vector3f c = stack.vectors.get(); + + float lastLambda = lambda; + + // first solution, using GJK + + // no penetration support for now, perhaps pass a pointer when we really want it + ConvexPenetrationDepthSolver penSolverPtr = null; + + Transform identityTrans = stack.transforms.get(); + identityTrans.setIdentity(); + + SphereShape raySphere = new SphereShape(0f); + raySphere.setMargin(0f); + + Transform sphereTr = stack.transforms.get(); + sphereTr.setIdentity(); + sphereTr.origin.set(rayFromLocalA.origin); + + result.drawCoordSystem(sphereTr); + { + PointCollector pointCollector1 = new PointCollector(); + GjkPairDetector gjk = new GjkPairDetector(raySphere, convex, simplexSolver, penSolverPtr); + + ClosestPointInput input = pointInputsPool.get(); + input.init(); + + input.transformA.set(sphereTr); + input.transformB.set(identityTrans); + gjk.getClosestPoints(input, pointCollector1, null); + + pointInputsPool.release(input); + + hasResult = pointCollector1.hasResult; + c.set(pointCollector1.pointInWorld); + n.set(pointCollector1.normalOnBInWorld); + } + + if (hasResult) { + Vector3f tmp = stack.vectors.get(); + + float dist; + tmp.sub(c, x); + dist = tmp.length(); + if (dist < radius) { + // penetration + lastLambda = 1f; + } + + // not close enough + while (dist > radius) { + + n.sub(x, c); + float nDotr = n.dot(r); + + if (nDotr >= -(BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) { + return false; + } + + lambda = lambda - n.dot(n) / nDotr; + if (lambda <= lastLambda) { + break; + } + + lastLambda = lambda; + + x.scaleAdd(lambda, r, s); + + sphereTr.origin.set(x); + result.drawCoordSystem(sphereTr); + PointCollector pointCollector = new PointCollector(); + GjkPairDetector gjk = new GjkPairDetector(raySphere, convex, simplexSolver, penSolverPtr); + + ClosestPointInput input = pointInputsPool.get(); + input.init(); + + input.transformA.set(sphereTr); + input.transformB.set(identityTrans); + gjk.getClosestPoints(input, pointCollector, null); + + pointInputsPool.release(input); + + if (pointCollector.hasResult) { + if (pointCollector.distance < 0f) { + // degeneracy, report a hit + result.fraction = lastLambda; + result.normal.set(n); + result.hitPoint.set(pointCollector.pointInWorld); + return true; + } + c.set(pointCollector.pointInWorld); + tmp.sub(c, x); + dist = tmp.length(); + } + else { + // ?? + return false; + } + + } + + if (lastLambda < 1f) { + result.fraction = lastLambda; + result.normal.set(n); + result.hitPoint.set(c); + return true; + } + } + + return false; + } + finally { + stack.popCommonMath(); + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/GjkEpaPenetrationDepthSolver.java b/src/jbullet/src/javabullet/collision/narrowphase/GjkEpaPenetrationDepthSolver.java new file mode 100644 index 0000000..e8b74c0 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/GjkEpaPenetrationDepthSolver.java @@ -0,0 +1,59 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javabullet.collision.shapes.ConvexShape; +import javabullet.linearmath.IDebugDraw; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class GjkEpaPenetrationDepthSolver implements ConvexPenetrationDepthSolver { + + public boolean calcPenDepth(SimplexSolverInterface simplexSolver, + ConvexShape pConvexA, ConvexShape pConvexB, + Transform transformA, Transform transformB, + Vector3f v, Vector3f wWitnessOnA, Vector3f wWitnessOnB, + IDebugDraw debugDraw/*, btStackAlloc* stackAlloc*/) + { + float radialmargin = 0f; + + GjkEpaSolver.Results results = new GjkEpaSolver.Results(); + if (GjkEpaSolver.collide(pConvexA, transformA, + pConvexB, transformB, + radialmargin/*,stackAlloc*/, results)) { + //debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0)); + //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth); + wWitnessOnA.set(results.witnesses[0]); + wWitnessOnB.set(results.witnesses[1]); + return true; + } + + return false; + } + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/GjkEpaSolver.java b/src/jbullet/src/javabullet/collision/narrowphase/GjkEpaSolver.java new file mode 100644 index 0000000..989e38f --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/GjkEpaSolver.java @@ -0,0 +1,947 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import java.util.Arrays; +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javabullet.ObjectStackList; +import javabullet.collision.narrowphase.GjkEpaSolver.EPA.Face; +import javabullet.collision.narrowphase.GjkEpaSolver.GJK.He; +import javabullet.collision.narrowphase.GjkEpaSolver.GJK.Mkv; +import javabullet.collision.shapes.ConvexShape; +import javabullet.linearmath.MatrixUtil; +import javabullet.linearmath.QuaternionUtil; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Matrix3f; +import javax.vecmath.Quat4f; +import javax.vecmath.Vector3f; + +/* +GJK-EPA collision solver by Nathanael Presson +Nov.2006 +*/ + +/** + * GjkEpaSolver contributed under zlib by Nathanael Presson. + * + * @author jezek2 + */ +public class GjkEpaSolver { + + protected static ObjectStackList<Mkv> stackMkv = new ObjectStackList<Mkv>(Mkv.class); + protected static ObjectStackList<He> stackHe = new ObjectStackList<He>(He.class); + protected static ObjectStackList<Face> stackFace = new ObjectStackList<Face>(Face.class); + + protected static void pushStack() { + stackMkv.push(); + stackHe.push(); + stackFace.push(); + } + + protected static void popStack() { + stackMkv.pop(); + stackHe.pop(); + stackFace.pop(); + } + + public enum ResultsStatus { + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed, /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } + + public static class Results { + public ResultsStatus status; + public final Vector3f[] witnesses/*[2]*/ = new Vector3f[] { new Vector3f(), new Vector3f() }; + public final Vector3f normal = new Vector3f(); + public float depth; + public int epa_iterations; + public int gjk_iterations; + } + + //////////////////////////////////////////////////////////////////////////// + + private static final float cstInf = BulletGlobals.SIMD_INFINITY; + private static final float cstPi = BulletGlobals.SIMD_PI; + private static final float cst2Pi = BulletGlobals.SIMD_2_PI; + private static final int GJK_maxiterations = 128; + private static final int GJK_hashsize = 1 << 6; + private static final int GJK_hashmask = GJK_hashsize - 1; + private static final float GJK_insimplex_eps = 0.0001f; + private static final float GJK_sqinsimplex_eps = GJK_insimplex_eps * GJK_insimplex_eps; + private static final int EPA_maxiterations = 256; + private static final float EPA_inface_eps = 0.01f; + private static final float EPA_accuracy = 0.001f; + + //////////////////////////////////////////////////////////////////////////// + + protected static class GJK { + protected final BulletStack stack = BulletStack.get(); + + public static class Mkv { + public final Vector3f w = new Vector3f(); // Minkowski vertice + public final Vector3f r = new Vector3f(); // Ray + + public void set(Mkv m) { + w.set(m.w); + r.set(m.r); + } + } + + public static class He { + public final Vector3f v = new Vector3f(); + public He n; + } + + //public btStackAlloc sa; + //public Block sablock; + public final He[] table = new He[GJK_hashsize]; + public final Matrix3f[] wrotations/*[2]*/ = new Matrix3f[] { new Matrix3f(), new Matrix3f() }; + public final Vector3f[] positions/*[2]*/ = new Vector3f[] { new Vector3f(), new Vector3f() }; + public final ConvexShape[] shapes = new ConvexShape[2]; + public final Mkv[] simplex = new Mkv[5]; + public final Vector3f ray = new Vector3f(); + public /*unsigned*/ int order; + public /*unsigned*/ int iterations; + public float margin; + public boolean failed; + + { + for (int i=0; i<simplex.length; i++) simplex[i] = new Mkv(); + } + + public GJK() { + } + + public GJK(/*StackAlloc psa,*/ + Matrix3f wrot0, Vector3f pos0, ConvexShape shape0, + Matrix3f wrot1, Vector3f pos1, ConvexShape shape1) { + this(wrot0, pos0, shape0, wrot1, pos1, shape1, 0f); + } + + public GJK(/*StackAlloc psa,*/ + Matrix3f wrot0, Vector3f pos0, ConvexShape shape0, + Matrix3f wrot1, Vector3f pos1, ConvexShape shape1, + float pmargin) { + init(wrot0, pos0, shape0, wrot1, pos1, shape1, pmargin); + } + + public void init(/*StackAlloc psa,*/ + Matrix3f wrot0, Vector3f pos0, ConvexShape shape0, + Matrix3f wrot1, Vector3f pos1, ConvexShape shape1, + float pmargin) { + pushStack(); + wrotations[0].set(wrot0); + positions[0].set(pos0); + shapes[0] = shape0; + wrotations[1].set(wrot1); + positions[1].set(pos1); + shapes[1] = shape1; + //sa =psa; + //sablock =sa->beginBlock(); + margin = pmargin; + failed = false; + } + + public void destroy() { + popStack(); + } + + // vdh: very dummy hash + public static /*unsigned*/ int Hash(Vector3f v) { + int h = (int)(v.x * 15461) ^ (int)(v.y * 83003) ^ (int)(v.z * 15473); + return (h * 169639) & GJK_hashmask; + } + + public Vector3f LocalSupport(Vector3f d, /*unsigned*/ int i) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + MatrixUtil.transposeTransform(tmp, d, wrotations[i]); + + Vector3f tmp2 = stack.vectors.get(shapes[i].localGetSupportingVertex(tmp)); + wrotations[i].transform(tmp2); + tmp2.add(positions[i]); + + return stack.vectors.returning(tmp2); + } + finally { + stack.vectors.pop(); + } + } + + public void Support(Vector3f d, Mkv v) { + stack.vectors.push(); + try { + v.r.set(d); + + Vector3f tmp1 = stack.vectors.get(LocalSupport(d, 0)); + + Vector3f tmp = stack.vectors.get(); + tmp.set(d); + tmp.negate(); + Vector3f tmp2 = stack.vectors.get(LocalSupport(tmp, 1)); + + v.w.sub(tmp1, tmp2); + v.w.scaleAdd(margin, d, v.w); + } + finally { + stack.vectors.pop(); + } + } + + public boolean FetchSupport() { + int h = Hash(ray); + He e = table[h]; + while (e != null) { + if (e.v.equals(ray)) { + --order; + return false; + } + else { + e = e.n; + } + } + //e = (He*)sa->allocate(sizeof(He)); + //e = new He(); + e = stackHe.get(); + e.v.set(ray); + e.n = table[h]; + table[h] = e; + Support(ray, simplex[++order]); + return (ray.dot(simplex[order].w) > 0); + } + + public boolean SolveSimplex2(Vector3f ao, Vector3f ab) { + stack.vectors.push(); + try { + if (ab.dot(ao) >= 0) { + Vector3f cabo = stack.vectors.get(); + cabo.cross(ab, ao); + if (cabo.lengthSquared() > GJK_sqinsimplex_eps) { + ray.cross(cabo, ab); + } + else { + return true; + } + } + else { + order = 0; + simplex[0].set(simplex[1]); + ray.set(ao); + } + return (false); + } + finally { + stack.vectors.pop(); + } + } + + public boolean SolveSimplex3(Vector3f ao, Vector3f ab, Vector3f ac) + { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + tmp.cross(ab, ac); + return (SolveSimplex3a(ao,ab,ac,tmp)); + } + finally { + stack.vectors.pop(); + } + } + + public boolean SolveSimplex3a(Vector3f ao, Vector3f ab, Vector3f ac, Vector3f cabc) { + stack.vectors.push(); + try { + // TODO: optimize + + Vector3f tmp = stack.vectors.get(); + tmp.cross(cabc, ab); + + Vector3f tmp2 = stack.vectors.get(); + tmp2.cross(cabc, ac); + + if (tmp.dot(ao) < -GJK_insimplex_eps) { + order = 1; + simplex[0].set(simplex[1]); + simplex[1].set(simplex[2]); + return SolveSimplex2(ao, ab); + } + else if (tmp2.dot(ao) > +GJK_insimplex_eps) { + order = 1; + simplex[1].set(simplex[2]); + return SolveSimplex2(ao, ac); + } + else { + float d = cabc.dot(ao); + if (Math.abs(d) > GJK_insimplex_eps) { + if (d > 0) { + ray.set(cabc); + } + else { + ray.negate(cabc); + + Mkv swapTmp = new Mkv(); + swapTmp.set(simplex[0]); + simplex[0].set(simplex[1]); + simplex[1].set(swapTmp); + } + return false; + } + else { + return true; + } + } + } + finally { + stack.vectors.pop(); + } + } + + public boolean SolveSimplex4(Vector3f ao, Vector3f ab, Vector3f ac, Vector3f ad) { + stack.vectors.push(); + try { + // TODO: optimize + + Vector3f crs = stack.vectors.get(); + + Vector3f tmp = stack.vectors.get(); + tmp.cross(ab, ac); + + Vector3f tmp2 = stack.vectors.get(); + tmp2.cross(ac, ad); + + Vector3f tmp3 = stack.vectors.get(); + tmp3.cross(ad, ab); + + if (tmp.dot(ao) > GJK_insimplex_eps) { + crs.set(tmp); + order = 2; + simplex[0].set(simplex[1]); + simplex[1].set(simplex[2]); + simplex[2].set(simplex[3]); + return SolveSimplex3a(ao, ab, ac, crs); + } + else if (tmp2.dot(ao) > GJK_insimplex_eps) { + crs.set(tmp2); + order = 2; + simplex[2].set(simplex[3]); + return SolveSimplex3a(ao, ac, ad, crs); + } + else if (tmp3.dot(ao) > GJK_insimplex_eps) { + crs.set(tmp3); + order = 2; + simplex[1].set(simplex[0]); + simplex[0].set(simplex[2]); + simplex[2].set(simplex[3]); + return SolveSimplex3a(ao, ad, ab, crs); + } + else { + return (true); + } + } + finally { + stack.vectors.pop(); + } + } + + public boolean SearchOrigin() { + stack.vectors.push(); + try { + return SearchOrigin(stack.vectors.get(1f, 0f, 0f)); + } + finally { + stack.vectors.pop(); + } + } + + public boolean SearchOrigin(Vector3f initray) { + stack.vectors.push(); + try { + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + Vector3f tmp3 = stack.vectors.get(); + Vector3f tmp4 = stack.vectors.get(); + + iterations = 0; + order = -1; + failed = false; + ray.set(initray); + ray.normalize(); + + Arrays.fill(table, null); + + FetchSupport(); + ray.negate(simplex[0].w); + for (; iterations < GJK_maxiterations; ++iterations) { + float rl = ray.length(); + ray.scale(1f / (rl > 0f ? rl : 1f)); + if (FetchSupport()) { + boolean found = false; + switch (order) { + case 1: { + tmp1.negate(simplex[1].w); + tmp2.sub(simplex[0].w, simplex[1].w); + found = SolveSimplex2(tmp1, tmp2); + break; + } + case 2: { + tmp1.negate(simplex[2].w); + tmp2.sub(simplex[1].w, simplex[2].w); + tmp3.sub(simplex[0].w, simplex[2].w); + found = SolveSimplex3(tmp1, tmp2, tmp3); + break; + } + case 3: { + tmp1.negate(simplex[3].w); + tmp2.sub(simplex[2].w, simplex[3].w); + tmp3.sub(simplex[1].w, simplex[3].w); + tmp4.sub(simplex[0].w, simplex[3].w); + found = SolveSimplex4(tmp1, tmp2, tmp3, tmp4); + break; + } + } + if (found) { + return true; + } + } + else { + return false; + } + } + failed = true; + return false; + } + finally { + stack.vectors.pop(); + } + } + + public boolean EncloseOrigin() { + stack.pushCommonMath(); + stack.quats.push(); + try { + Vector3f tmp = stack.vectors.get(); + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + switch (order) { + // Point + case 0: + break; + // Line + case 1: { + Vector3f ab = stack.vectors.get(); + ab.sub(simplex[1].w, simplex[0].w); + + Vector3f[] b = new Vector3f[] { stack.vectors.get(1f, 0f, 0f), stack.vectors.get(0f, 1f, 0f), stack.vectors.get(0, 0, 1f) }; + b[0].cross(ab, b[0]); + b[1].cross(ab, b[1]); + b[2].cross(ab, b[2]); + + float m[] = new float[] { b[0].lengthSquared(), b[1].lengthSquared(), b[2].lengthSquared() }; + + Quat4f tmpQuat = stack.quats.get(); + tmp.normalize(ab); + QuaternionUtil.setRotation(tmpQuat, tmp, cst2Pi / 3f); + + Matrix3f r = stack.matrices.get(); + MatrixUtil.setRotation(r, tmpQuat); + + Vector3f w = stack.vectors.get(b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2]); + + tmp.normalize(w); + Support(tmp, simplex[4]); r.transform(w); + tmp.normalize(w); + Support(tmp, simplex[2]); r.transform(w); + tmp.normalize(w); + Support(tmp, simplex[3]); r.transform(w); + order = 4; + return (true); + } + // Triangle + case 2: { + tmp1.sub(simplex[1].w, simplex[0].w); + tmp2.sub(simplex[2].w, simplex[0].w); + Vector3f n = stack.vectors.get(); + n.cross(tmp1, tmp2); + n.normalize(); + + Support(n, simplex[3]); + + tmp.negate(n); + Support(tmp, simplex[4]); + order = 4; + return (true); + } + // Tetrahedron + case 3: + return (true); + // Hexahedron + case 4: + return (true); + } + return (false); + } + finally { + stack.popCommonMath(); + stack.quats.pop(); + } + } + + } + + //////////////////////////////////////////////////////////////////////////// + + protected static class EPA { + protected final BulletStack stack = BulletStack.get(); + + public static class Face { + public final GJK.Mkv[] v = new GJK.Mkv[3]; + public final Face[] f = new Face[3]; + public final int[] e = new int[3]; + public final Vector3f n = new Vector3f(); + public float d; + public int mark; + public Face prev; + public Face next; + } + + public GJK gjk; + //public btStackAlloc* sa; + public Face root; + public int nfaces; + public int iterations; + public final Vector3f[][] features = new Vector3f[2][3]; + public final Vector3f[] nearest/*[2]*/ = new Vector3f[] { new Vector3f(), new Vector3f() }; + public final Vector3f normal = new Vector3f(); + public float depth; + public boolean failed; + + { + for (int i=0; i<features.length; i++) { + for (int j=0; j<features[i].length; j++) { + features[i][j] = new Vector3f(); + } + } + } + + public EPA(GJK pgjk) { + gjk = pgjk; + //sa = pgjk->sa; + } + + public Vector3f GetCoordinates(Face face) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + Vector3f o = stack.vectors.get(); + o.scale(-face.d, face.n); + + float[] a = stack.floatArrays.getFixed(3); + + tmp1.sub(face.v[0].w, o); + tmp2.sub(face.v[1].w, o); + tmp.cross(tmp1, tmp2); + a[0] = tmp.length(); + + tmp1.sub(face.v[1].w, o); + tmp2.sub(face.v[2].w, o); + tmp.cross(tmp1, tmp2); + a[1] = tmp.length(); + + tmp1.sub(face.v[2].w, o); + tmp2.sub(face.v[0].w, o); + tmp.cross(tmp1, tmp2); + a[2] = tmp.length(); + + float sm = a[0] + a[1] + a[2]; + + Vector3f result = stack.vectors.get(a[1], a[2], a[0]); + result.scale(1f / (sm > 0f ? sm : 1f)); + + stack.floatArrays.release(a); + + return stack.vectors.returning(result); + } + finally { + stack.vectors.pop(); + } + } + + public Face FindBest() { + Face bf = null; + if (root != null) { + Face cf = root; + float bd = cstInf; + do { + if (cf.d < bd) { + bd = cf.d; + bf = cf; + } + } + while (null != (cf = cf.next)); + } + return bf; + } + + public boolean Set(Face f, GJK.Mkv a, GJK.Mkv b, GJK.Mkv c) { + stack.vectors.push(); + try { + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + Vector3f tmp3 = stack.vectors.get(); + + Vector3f nrm = stack.vectors.get(); + tmp1.sub(b.w, a.w); + tmp2.sub(c.w, a.w); + nrm.cross(tmp1, tmp2); + + float len = nrm.length(); + + tmp1.cross(a.w, b.w); + tmp2.cross(b.w, c.w); + tmp3.cross(c.w, a.w); + + boolean valid = (tmp1.dot(nrm) >= -EPA_inface_eps) && + (tmp2.dot(nrm) >= -EPA_inface_eps) && + (tmp3.dot(nrm) >= -EPA_inface_eps); + + f.v[0] = a; + f.v[1] = b; + f.v[2] = c; + f.mark = 0; + f.n.scale(1f / (len > 0f ? len : cstInf), nrm); + f.d = Math.max(0, -f.n.dot(a.w)); + return valid; + } + finally { + stack.vectors.pop(); + } + } + + public Face NewFace(GJK.Mkv a, GJK.Mkv b, GJK.Mkv c) { + //Face pf = new Face(); + Face pf = stackFace.get(); + if (Set(pf, a, b, c)) { + if (root != null) { + root.prev = pf; + } + pf.prev = null; + pf.next = root; + root = pf; + ++nfaces; + } + else { + pf.prev = pf.next = null; + } + return (pf); + } + + public void Detach(Face face) { + if (face.prev != null || face.next != null) { + --nfaces; + if (face == root) { + root = face.next; + root.prev = null; + } + else { + if (face.next == null) { + face.prev.next = null; + } + else { + face.prev.next = face.next; + face.next.prev = face.prev; + } + } + face.prev = face.next = null; + } + } + + public void Link(Face f0, int e0, Face f1, int e1) { + f0.f[e0] = f1; f1.e[e1] = e0; + f1.f[e1] = f0; f0.e[e0] = e1; + } + + public Mkv Support(Vector3f w) { + //Mkv v = new Mkv(); + Mkv v = stackMkv.get(); + gjk.Support(w, v); + return v; + } + + private static int[] mod3 = new int[] { 0, 1, 2, 0, 1 }; + + public int BuildHorizon(int markid, GJK.Mkv w, Face f, int e, Face[] cf, Face[] ff) { + int ne = 0; + if (f.mark != markid) { + int e1 = mod3[e + 1]; + if ((f.n.dot(w.w) + f.d) > 0) { + Face nf = NewFace(f.v[e1], f.v[e], w); + Link(nf, 0, f, e); + if (cf[0] != null) { + Link(cf[0], 1, nf, 2); + } + else { + ff[0] = nf; + } + cf[0] = nf; + ne = 1; + } + else { + int e2 = mod3[e + 2]; + Detach(f); + f.mark = markid; + ne += BuildHorizon(markid, w, f.f[e1], f.e[e1], cf, ff); + ne += BuildHorizon(markid, w, f.f[e2], f.e[e2], cf, ff); + } + } + return (ne); + } + + public float EvaluatePD() { + return EvaluatePD(EPA_accuracy); + } + + private static final int[][] tetrahedron_fidx/*[4][3]*/ = new int[][] {{2,1,0},{3,0,1},{3,1,2},{3,2,0}}; + private static final int[][] tetrahedron_eidx/*[6][4]*/ = new int[][] {{0,0,2,1},{0,1,1,1},{0,2,3,1},{1,0,3,2},{2,0,1,2},{3,0,2,2}}; + + private static final int[][] hexahedron_fidx/*[6][3]*/ = new int[][] {{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}}; + private static final int[][] hexahedron_eidx/*[9][4]*/ = new int[][] {{0,0,4,0},{0,1,2,1},{0,2,1,2},{1,1,5,2},{1,0,2,0},{2,2,3,2},{3,1,5,0},{3,0,4,2},{5,1,4,1}}; + + public float EvaluatePD(float accuracy) { + stack.vectors.push(); + pushStack(); + try { + Vector3f tmp = stack.vectors.get(); + + //btBlock* sablock = sa->beginBlock(); + Face bestface = null; + int markid = 1; + depth = -cstInf; + normal.set(0f, 0f, 0f); + root = null; + nfaces = 0; + iterations = 0; + failed = false; + /* Prepare hull */ + if (gjk.EncloseOrigin()) { + //const U* pfidx = 0; + int[][] pfidx_ptr = null; + int pfidx_index = 0; + + int nfidx = 0; + //const U* peidx = 0; + int[][] peidx_ptr = null; + int peidx_index = 0; + + int neidx = 0; + GJK.Mkv[] basemkv = new GJK.Mkv[5]; + Face[] basefaces = new Face[6]; + switch (gjk.order) { + // Tetrahedron + case 3: + { + //pfidx=(const U*)fidx; + pfidx_ptr = tetrahedron_fidx; + pfidx_index = 0; + + nfidx = 4; + + //peidx=(const U*)eidx; + peidx_ptr = tetrahedron_eidx; + peidx_index = 0; + + neidx = 6; + } + break; + // Hexahedron + case 4: + { + //pfidx=(const U*)fidx; + pfidx_ptr = hexahedron_fidx; + pfidx_index = 0; + + nfidx = 6; + + //peidx=(const U*)eidx; + peidx_ptr = hexahedron_eidx; + peidx_index = 0; + + neidx = 9; + } + break; + } + int i; + + for (i = 0; i <= gjk.order; ++i) { + basemkv[i] = new GJK.Mkv(); + basemkv[i].set(gjk.simplex[i]); + } + for (i = 0; i < nfidx; ++i, pfidx_index++) { + basefaces[i] = NewFace(basemkv[pfidx_ptr[pfidx_index][0]], basemkv[pfidx_ptr[pfidx_index][1]], basemkv[pfidx_ptr[pfidx_index][2]]); + } + for (i = 0; i < neidx; ++i, peidx_index++) { + Link(basefaces[peidx_ptr[peidx_index][0]], peidx_ptr[peidx_index][1], basefaces[peidx_ptr[peidx_index][2]], peidx_ptr[peidx_index][3]); + } + } + if (0 == nfaces) { + //sa->endBlock(sablock); + return (depth); + } + /* Expand hull */ + for (; iterations < EPA_maxiterations; ++iterations) { + Face bf = FindBest(); + if (bf != null) { + tmp.negate(bf.n); + GJK.Mkv w = Support(tmp); + float d = bf.n.dot(w.w) + bf.d; + bestface = bf; + if (d < -accuracy) { + Face[] cf = new Face[]{null}; + Face[] ff = new Face[]{null}; + int nf = 0; + Detach(bf); + bf.mark = ++markid; + for (int i = 0; i < 3; ++i) { + nf += BuildHorizon(markid, w, bf.f[i], bf.e[i], cf, ff); + } + if (nf <= 2) { + break; + } + Link(cf[0], 1, ff[0], 2); + } + else { + break; + } + } + else { + break; + } + } + /* Extract contact */ + if (bestface != null) { + Vector3f b = stack.vectors.get(GetCoordinates(bestface)); + normal.set(bestface.n); + depth = Math.max(0, bestface.d); + for (int i = 0; i < 2; ++i) { + float s = i != 0 ? -1f : 1f; + for (int j = 0; j < 3; ++j) { + tmp.scale(s, bestface.v[j].r); + features[i][j].set(gjk.LocalSupport(tmp, i)); + } + } + + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + Vector3f tmp3 = stack.vectors.get(); + + tmp1.scale(b.x, features[0][0]); + tmp2.scale(b.y, features[0][1]); + tmp3.scale(b.z, features[0][2]); + VectorUtil.add(nearest[0], tmp1, tmp2, tmp3); + + tmp1.scale(b.x, features[1][0]); + tmp2.scale(b.y, features[1][1]); + tmp3.scale(b.z, features[1][2]); + VectorUtil.add(nearest[1], tmp1, tmp2, tmp3); + } + else { + failed = true; + } + //sa->endBlock(sablock); + return (depth); + } + finally { + stack.vectors.pop(); + popStack(); + } + } + + } + + //////////////////////////////////////////////////////////////////////////// + + private static GJK gjk = new GJK(); + + public static boolean collide(ConvexShape shape0, Transform wtrs0, + ConvexShape shape1, Transform wtrs1, + float radialmargin/*, + btStackAlloc* stackAlloc*/, + Results results) { + + // Initialize + results.witnesses[0].set(0f, 0f, 0f); + results.witnesses[1].set(0f, 0f, 0f); + results.normal.set(0f, 0f, 0f); + results.depth = 0; + results.status = ResultsStatus.Separated; + results.epa_iterations = 0; + results.gjk_iterations = 0; + /* Use GJK to locate origin */ + gjk.init(/*stackAlloc,*/ + wtrs0.basis, wtrs0.origin, shape0, + wtrs1.basis, wtrs1.origin, shape1, + radialmargin + EPA_accuracy); + try { + boolean collide = gjk.SearchOrigin(); + results.gjk_iterations = gjk.iterations + 1; + if (collide) { + /* Then EPA for penetration depth */ + EPA epa = new EPA(gjk); + float pd = epa.EvaluatePD(); + results.epa_iterations = epa.iterations + 1; + if (pd > 0) { + results.status = ResultsStatus.Penetrating; + results.normal.set(epa.normal); + results.depth = pd; + results.witnesses[0].set(epa.nearest[0]); + results.witnesses[1].set(epa.nearest[1]); + return (true); + } + else { + if (epa.failed) { + results.status = ResultsStatus.EPA_Failed; + } + } + } + else { + if (gjk.failed) { + results.status = ResultsStatus.GJK_Failed; + } + } + return (false); + } + finally { + gjk.destroy(); + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/GjkPairDetector.java b/src/jbullet/src/javabullet/collision/narrowphase/GjkPairDetector.java new file mode 100644 index 0000000..a70a2cd --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/GjkPairDetector.java @@ -0,0 +1,337 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javabullet.collision.shapes.ConvexShape; +import javabullet.linearmath.IDebugDraw; +import javabullet.linearmath.MatrixUtil; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * GjkPairDetector uses GJK to implement the DiscreteCollisionDetectorInterface. + * + * @author jezek2 + */ +public class GjkPairDetector implements DiscreteCollisionDetectorInterface { + + protected final BulletStack stack = BulletStack.get(); + + // must be above the machine epsilon + private static final float REL_ERROR2 = 1.0e-6f; + + private final Vector3f cachedSeparatingAxis = new Vector3f(0f, 0f, 1f); + private ConvexPenetrationDepthSolver penetrationDepthSolver; + private SimplexSolverInterface simplexSolver; + private ConvexShape minkowskiA; + private ConvexShape minkowskiB; + private boolean ignoreMargin = false; + + //some debugging to fix degeneracy problems + public int lastUsedMethod = -1; + public int curIter; + public int degenerateSimplex; + public int catchDegeneracies = 1; + + public GjkPairDetector(ConvexShape objectA, ConvexShape objectB, SimplexSolverInterface simplexSolver, ConvexPenetrationDepthSolver penetrationDepthSolver) { + this.penetrationDepthSolver = penetrationDepthSolver; + this.simplexSolver = simplexSolver; + this.minkowskiA = objectA; + this.minkowskiB = objectB; + } + + public void getClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw) { + stack.pushCommonMath(); + try { + Vector3f tmp = stack.vectors.get(); + + float distance = 0f; + Vector3f normalInB = stack.vectors.get(0f, 0f, 0f); + Vector3f pointOnA = stack.vectors.get(), pointOnB = stack.vectors.get(); + Transform localTransA = stack.transforms.get(input.transformA); + Transform localTransB = stack.transforms.get(input.transformB); + Vector3f positionOffset = stack.vectors.get(); + positionOffset.add(localTransA.origin, localTransB.origin); + positionOffset.scale(0.5f); + localTransA.origin.sub(positionOffset); + localTransB.origin.sub(positionOffset); + + float marginA = minkowskiA.getMargin(); + float marginB = minkowskiB.getMargin(); + + BulletGlobals.gNumGjkChecks++; + + // for CCD we don't use margins + if (ignoreMargin) { + marginA = 0f; + marginB = 0f; + } + + curIter = 0; + int gGjkMaxIter = 1000; // this is to catch invalid input, perhaps check for #NaN? + cachedSeparatingAxis.set(0f, 1f, 0f); + + boolean isValid = false; + boolean checkSimplex = false; + boolean checkPenetration = true; + degenerateSimplex = 0; + + lastUsedMethod = -1; + + { + float squaredDistance = BulletGlobals.SIMD_INFINITY; + float delta = 0f; + + float margin = marginA + marginB; + + simplexSolver.reset(); + + for (;;) //while (true) + { + Vector3f seperatingAxisInA = stack.vectors.get(); + seperatingAxisInA.negate(cachedSeparatingAxis); + MatrixUtil.transposeTransform(seperatingAxisInA, seperatingAxisInA, input.transformA.basis); + + Vector3f seperatingAxisInB = stack.vectors.get(); + seperatingAxisInB.set(cachedSeparatingAxis); + MatrixUtil.transposeTransform(seperatingAxisInB, seperatingAxisInB, input.transformB.basis); + + Vector3f pInA = stack.vectors.get(minkowskiA.localGetSupportingVertexWithoutMargin(seperatingAxisInA)); + Vector3f qInB = stack.vectors.get(minkowskiB.localGetSupportingVertexWithoutMargin(seperatingAxisInB)); + + Vector3f pWorld = stack.vectors.get(pInA); + localTransA.transform(pWorld); + + Vector3f qWorld = stack.vectors.get(qInB); + localTransB.transform(qWorld); + + Vector3f w = stack.vectors.get(); + w.sub(pWorld, qWorld); + + delta = cachedSeparatingAxis.dot(w); + + // potential exit, they don't overlap + if ((delta > 0f) && (delta * delta > squaredDistance * input.maximumDistanceSquared)) { + checkPenetration = false; + break; + } + + // exit 0: the new point is already in the simplex, or we didn't come any closer + if (simplexSolver.inSimplex(w)) { + degenerateSimplex = 1; + checkSimplex = true; + break; + } + // are we getting any closer ? + float f0 = squaredDistance - delta; + float f1 = squaredDistance * REL_ERROR2; + + if (f0 <= f1) { + if (f0 <= 0f) { + degenerateSimplex = 2; + } + checkSimplex = true; + break; + } + // add current vertex to simplex + simplexSolver.addVertex(w, pWorld, qWorld); + + // calculate the closest point to the origin (update vector v) + if (!simplexSolver.closest(cachedSeparatingAxis)) { + degenerateSimplex = 3; + checkSimplex = true; + break; + } + + float previousSquaredDistance = squaredDistance; + squaredDistance = cachedSeparatingAxis.lengthSquared(); + + // redundant m_simplexSolver->compute_points(pointOnA, pointOnB); + + // are we getting any closer ? + if (previousSquaredDistance - squaredDistance <= BulletGlobals.FLT_EPSILON * previousSquaredDistance) { + simplexSolver.backup_closest(cachedSeparatingAxis); + checkSimplex = true; + break; + } + + // degeneracy, this is typically due to invalid/uninitialized worldtransforms for a CollisionObject + if (curIter++ > gGjkMaxIter) { + //#if defined(DEBUG) || defined (_DEBUG) + if (BulletGlobals.DEBUG) { + System.err.printf("btGjkPairDetector maxIter exceeded:%i\n", curIter); + System.err.printf("sepAxis=(%f,%f,%f), squaredDistance = %f, shapeTypeA=%i,shapeTypeB=%i\n", + cachedSeparatingAxis.x, + cachedSeparatingAxis.y, + cachedSeparatingAxis.z, + squaredDistance, + minkowskiA.getShapeType(), + minkowskiB.getShapeType()); + } + //#endif + break; + + } + + boolean check = (!simplexSolver.fullSimplex()); + //bool check = (!m_simplexSolver->fullSimplex() && squaredDistance > SIMD_EPSILON * m_simplexSolver->maxVertex()); + + if (!check) { + // do we need this backup_closest here ? + simplexSolver.backup_closest(cachedSeparatingAxis); + break; + } + } + + if (checkSimplex) { + simplexSolver.compute_points(pointOnA, pointOnB); + normalInB.sub(pointOnA, pointOnB); + float lenSqr = cachedSeparatingAxis.lengthSquared(); + // valid normal + if (lenSqr < 0.0001f) { + degenerateSimplex = 5; + } + if (lenSqr > BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) { + float rlen = 1f / (float) Math.sqrt(lenSqr); + normalInB.scale(rlen); // normalize + float s = (float) Math.sqrt(squaredDistance); + + assert (s > 0f); + + tmp.scale((marginA / s), cachedSeparatingAxis); + pointOnA.sub(tmp); + + tmp.scale((marginB / s), cachedSeparatingAxis); + pointOnB.add(tmp); + + distance = ((1f / rlen) - margin); + isValid = true; + + lastUsedMethod = 1; + } + else { + lastUsedMethod = 2; + } + } + + boolean catchDegeneratePenetrationCase = + (catchDegeneracies != 0 && penetrationDepthSolver != null && degenerateSimplex != 0 && ((distance + margin) < 0.01f)); + + //if (checkPenetration && !isValid) + if (checkPenetration && (!isValid || catchDegeneratePenetrationCase)) { + // penetration case + + // if there is no way to handle penetrations, bail out + if (penetrationDepthSolver != null) { + // Penetration depth case. + Vector3f tmpPointOnA = stack.vectors.get(), tmpPointOnB = stack.vectors.get(); + + BulletGlobals.gNumDeepPenetrationChecks++; + + boolean isValid2 = penetrationDepthSolver.calcPenDepth( + simplexSolver, + minkowskiA, minkowskiB, + localTransA, localTransB, + cachedSeparatingAxis, tmpPointOnA, tmpPointOnB, + debugDraw/*,input.stackAlloc*/); + + if (isValid2) { + Vector3f tmpNormalInB = stack.vectors.get(); + tmpNormalInB.sub(tmpPointOnB, tmpPointOnA); + + float lenSqr = tmpNormalInB.lengthSquared(); + if (lenSqr > (BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) { + tmpNormalInB.scale(1f / (float) Math.sqrt(lenSqr)); + tmp.sub(tmpPointOnA, tmpPointOnB); + float distance2 = -tmp.length(); + // only replace valid penetrations when the result is deeper (check) + if (!isValid || (distance2 < distance)) { + distance = distance2; + pointOnA.set(tmpPointOnA); + pointOnB.set(tmpPointOnB); + normalInB.set(tmpNormalInB); + isValid = true; + lastUsedMethod = 3; + } + else { + + } + } + else { + //isValid = false; + lastUsedMethod = 4; + } + } + else { + lastUsedMethod = 5; + } + + } + } + } + + if (isValid) { + //#ifdef __SPU__ + // //spu_printf("distance\n"); + //#endif //__CELLOS_LV2__ + + tmp.add(pointOnB, positionOffset); + output.addContactPoint( + normalInB, + tmp, + distance); + //printf("gjk add:%f",distance); + } + } + finally { + stack.popCommonMath(); + } + } + + public void setMinkowskiA(ConvexShape minkA) { + minkowskiA = minkA; + } + + public void setMinkowskiB(ConvexShape minkB) { + minkowskiB = minkB; + } + + public void setCachedSeperatingAxis(Vector3f seperatingAxis) { + cachedSeparatingAxis.set(seperatingAxis); + } + + public void setPenetrationDepthSolver(ConvexPenetrationDepthSolver penetrationDepthSolver) { + this.penetrationDepthSolver = penetrationDepthSolver; + } + + /** + * Don't use setIgnoreMargin, it's for Bullet's internal use. + */ + public void setIgnoreMargin(boolean ignoreMargin) { + this.ignoreMargin = ignoreMargin; + } + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/ManifoldPoint.java b/src/jbullet/src/javabullet/collision/narrowphase/ManifoldPoint.java new file mode 100644 index 0000000..beb5a83 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/ManifoldPoint.java @@ -0,0 +1,98 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javax.vecmath.Vector3f; + +/** + * ManifoldContactPoint collects and maintains persistent contactpoints. + * Used to improve stability and performance of rigidbody dynamics response. + * + * @author jezek2 + */ +public class ManifoldPoint { + + public final Vector3f localPointA = new Vector3f(); + public final Vector3f localPointB = new Vector3f(); + public final Vector3f positionWorldOnB = new Vector3f(); + ///m_positionWorldOnA is redundant information, see getPositionWorldOnA(), but for clarity + public final Vector3f positionWorldOnA = new Vector3f(); + public final Vector3f normalWorldOnB = new Vector3f(); + + public float distance1; + public float combinedFriction; + public float combinedRestitution; + + public Object userPersistentData; + public int lifeTime; //lifetime of the contactpoint in frames + + public ManifoldPoint() { + } + + public ManifoldPoint(Vector3f pointA, Vector3f pointB, Vector3f normal, float distance) { + init(pointA, pointB, normal, distance); + } + + public void init(Vector3f pointA, Vector3f pointB, Vector3f normal, float distance) { + this.localPointA.set(pointA); + this.localPointB.set(pointB); + this.normalWorldOnB.set(normal); + this.distance1 = distance; + } + + public float getDistance() { + return distance1; + } + + public int getLifeTime() { + return lifeTime; + } + + public void set(ManifoldPoint p) { + localPointA.set(p.localPointA); + localPointB.set(p.localPointB); + positionWorldOnA.set(p.positionWorldOnA); + positionWorldOnB.set(p.positionWorldOnB); + normalWorldOnB.set(p.normalWorldOnB); + distance1 = p.distance1; + combinedFriction = p.combinedFriction; + combinedRestitution = p.combinedRestitution; + userPersistentData = p.userPersistentData; + lifeTime = p.lifeTime; + } + + public Vector3f getPositionWorldOnA() { + return positionWorldOnA; + //return m_positionWorldOnB + m_normalWorldOnB * m_distance1; + } + + public Vector3f getPositionWorldOnB() { + return positionWorldOnB; + } + + public void setDistance(float dist) { + distance1 = dist; + } + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/PersistentManifold.java b/src/jbullet/src/javabullet/collision/narrowphase/PersistentManifold.java new file mode 100644 index 0000000..93dfb4b --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/PersistentManifold.java @@ -0,0 +1,373 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; +import javax.vecmath.Vector4f; + + +/** + * btPersistentManifold is a contact point cache, it stays persistent as long as objects are overlapping in the broadphase. + * Those contact points are created by the collision narrow phase. + * The cache can be empty, or hold 1,2,3 or 4 points. Some collision algorithms (GJK) might only add one point at a time. + * updates/refreshes old contact points, and throw them away if necessary (distance becomes too large) + * reduces the cache to 4 points, when more then 4 points are added, using following rules: + * the contact point with deepest penetration is always kept, and it tries to maximuze the area covered by the points + * note that some pairs of objects might have more then one contact manifold. + * + * @author jezek2 + */ +public class PersistentManifold { + + protected final BulletStack stack = BulletStack.get(); + + public static final int MANIFOLD_CACHE_SIZE = 4; + + private final ManifoldPoint[] pointCache = new ManifoldPoint[MANIFOLD_CACHE_SIZE]; + /// this two body pointers can point to the physics rigidbody class. + /// void* will allow any rigidbody class + private Object body0; + private Object body1; + private int cachedPoints; + + public int index1a; + + { + for (int i=0; i<pointCache.length; i++) pointCache[i] = new ManifoldPoint(); + } + + public PersistentManifold() { + } + + public PersistentManifold(Object body0, Object body1, int bla) { + init(body0, body1, bla); + } + + public void init(Object body0, Object body1, int bla) { + this.body0 = body0; + this.body1 = body1; + cachedPoints = 0; + index1a = 0; + } + + /// sort cached points so most isolated points come first + private int sortCachedPoints(ManifoldPoint pt) { + stack.vectors.push(); + stack.vectors4.push(); + try { + //calculate 4 possible cases areas, and take biggest area + //also need to keep 'deepest' + + int maxPenetrationIndex = -1; + //#define KEEP_DEEPEST_POINT 1 + //#ifdef KEEP_DEEPEST_POINT + float maxPenetration = pt.getDistance(); + for (int i = 0; i < 4; i++) { + if (pointCache[i].getDistance() < maxPenetration) { + maxPenetrationIndex = i; + maxPenetration = pointCache[i].getDistance(); + } + } + //#endif //KEEP_DEEPEST_POINT + + float res0 = 0f, res1 = 0f, res2 = 0f, res3 = 0f; + if (maxPenetrationIndex != 0) { + Vector3f a0 = stack.vectors.get(pt.localPointA); + a0.sub(pointCache[1].localPointA); + + Vector3f b0 = stack.vectors.get(pointCache[3].localPointA); + b0.sub(pointCache[2].localPointA); + + Vector3f cross = stack.vectors.get(); + cross.cross(a0, b0); + + res0 = cross.lengthSquared(); + } + + if (maxPenetrationIndex != 1) { + Vector3f a1 = stack.vectors.get(pt.localPointA); + a1.sub(pointCache[0].localPointA); + + Vector3f b1 = stack.vectors.get(pointCache[3].localPointA); + b1.sub(pointCache[2].localPointA); + + Vector3f cross = stack.vectors.get(); + cross.cross(a1, b1); + res1 = cross.lengthSquared(); + } + + if (maxPenetrationIndex != 2) { + Vector3f a2 = stack.vectors.get(pt.localPointA); + a2.sub(pointCache[0].localPointA); + + Vector3f b2 = stack.vectors.get(pointCache[3].localPointA); + b2.sub(pointCache[1].localPointA); + + Vector3f cross = stack.vectors.get(); + cross.cross(a2, b2); + + res2 = cross.lengthSquared(); + } + + if (maxPenetrationIndex != 3) { + Vector3f a3 = stack.vectors.get(pt.localPointA); + a3.sub(pointCache[0].localPointA); + + Vector3f b3 = stack.vectors.get(pointCache[2].localPointA); + b3.sub(pointCache[1].localPointA); + + Vector3f cross = stack.vectors.get(); + cross.cross(a3, b3); + res3 = cross.lengthSquared(); + } + + Vector4f maxvec = stack.vectors4.get(res0, res1, res2, res3); + int biggestarea = VectorUtil.closestAxis4(maxvec); + return biggestarea; + } + finally { + stack.vectors.pop(); + stack.vectors4.pop(); + } + } + + //private int findContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt); + + public Object getBody0() { + return body0; + } + + public Object getBody1() { + return body1; + } + + public void setBodies(Object body0, Object body1) { + this.body0 = body0; + this.body1 = body1; + } + + public void clearUserCache(ManifoldPoint pt) { + Object oldPtr = pt.userPersistentData; + if (oldPtr != null) { +//#ifdef DEBUG_PERSISTENCY +// int i; +// int occurance = 0; +// for (i = 0; i < cachedPoints; i++) { +// if (pointCache[i].userPersistentData == oldPtr) { +// occurance++; +// if (occurance > 1) { +// throw new InternalError(); +// } +// } +// } +// assert (occurance <= 0); +//#endif //DEBUG_PERSISTENCY + + if (pt.userPersistentData != null && BulletGlobals.gContactDestroyedCallback != null) { + BulletGlobals.gContactDestroyedCallback.invoke(pt.userPersistentData); + pt.userPersistentData = null; + } + +//#ifdef DEBUG_PERSISTENCY +// DebugPersistency(); +//#endif + } + } + + public int getNumContacts() { + return cachedPoints; + } + + public ManifoldPoint getContactPoint(int index) { + return pointCache[index]; + } + + // todo: get this margin from the current physics / collision environment + public float getContactBreakingThreshold() { + return BulletGlobals.gContactBreakingThreshold; + } + + public int getCacheEntry(ManifoldPoint newPoint) { + stack.vectors.push(); + try { + float shortestDist = getContactBreakingThreshold() * getContactBreakingThreshold(); + int size = getNumContacts(); + int nearestPoint = -1; + Vector3f diffA = stack.vectors.get(); + for (int i = 0; i < size; i++) { + ManifoldPoint mp = pointCache[i]; + + diffA.sub(mp.localPointA, newPoint.localPointA); + + float distToManiPoint = diffA.dot(diffA); + if (distToManiPoint < shortestDist) { + shortestDist = distToManiPoint; + nearestPoint = i; + } + } + return nearestPoint; + } + finally { + stack.vectors.pop(); + } + } + + public void addManifoldPoint(ManifoldPoint newPoint) { + assert (validContactDistance(newPoint)); + + int insertIndex = getNumContacts(); + if (insertIndex == MANIFOLD_CACHE_SIZE) { + //#if MANIFOLD_CACHE_SIZE >= 4 + if (MANIFOLD_CACHE_SIZE >= 4) { + //sort cache so best points come first, based on area + insertIndex = sortCachedPoints(newPoint); + } + else { + //#else + insertIndex = 0; + } + //#endif + } + else { + cachedPoints++; + } + replaceContactPoint(newPoint, insertIndex); + } + + public void removeContactPoint(int index) { + clearUserCache(pointCache[index]); + + int lastUsedIndex = getNumContacts() - 1; +// m_pointCache[index] = m_pointCache[lastUsedIndex]; + if (index != lastUsedIndex) { + // TODO: possible bug + pointCache[index].set(pointCache[lastUsedIndex]); + //get rid of duplicated userPersistentData pointer + pointCache[lastUsedIndex].userPersistentData = null; + pointCache[lastUsedIndex].lifeTime = 0; + } + + assert (pointCache[lastUsedIndex].userPersistentData == null); + cachedPoints--; + } + + public void replaceContactPoint(ManifoldPoint newPoint, int insertIndex) { + assert (validContactDistance(newPoint)); + +//#define MAINTAIN_PERSISTENCY 1 +//#ifdef MAINTAIN_PERSISTENCY + int lifeTime = pointCache[insertIndex].getLifeTime(); + assert (lifeTime >= 0); + Object cache = pointCache[insertIndex].userPersistentData; + + pointCache[insertIndex].set(newPoint); + + pointCache[insertIndex].userPersistentData = cache; + pointCache[insertIndex].lifeTime = lifeTime; +//#else +// clearUserCache(m_pointCache[insertIndex]); +// m_pointCache[insertIndex] = newPoint; +//#endif + } + + private boolean validContactDistance(ManifoldPoint pt) { + return pt.distance1 <= getContactBreakingThreshold(); + } + + /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin + public void refreshContactPoints(Transform trA, Transform trB) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + int i; + //#ifdef DEBUG_PERSISTENCY + // printf("refreshContactPoints posA = (%f,%f,%f) posB = (%f,%f,%f)\n", + // trA.getOrigin().getX(), + // trA.getOrigin().getY(), + // trA.getOrigin().getZ(), + // trB.getOrigin().getX(), + // trB.getOrigin().getY(), + // trB.getOrigin().getZ()); + //#endif //DEBUG_PERSISTENCY + // first refresh worldspace positions and distance + for (i = getNumContacts() - 1; i >= 0; i--) { + ManifoldPoint manifoldPoint = pointCache[i]; + + manifoldPoint.positionWorldOnA.set(manifoldPoint.localPointA); + trA.transform(manifoldPoint.positionWorldOnA); + + manifoldPoint.positionWorldOnB.set(manifoldPoint.localPointB); + trB.transform(manifoldPoint.positionWorldOnB); + + tmp.set(manifoldPoint.positionWorldOnA); + tmp.sub(manifoldPoint.positionWorldOnB); + manifoldPoint.distance1 = tmp.dot(manifoldPoint.normalWorldOnB); + + manifoldPoint.lifeTime++; + } + + // then + float distance2d; + Vector3f projectedDifference = stack.vectors.get(), projectedPoint = stack.vectors.get(); + + for (i = getNumContacts() - 1; i >= 0; i--) { + + ManifoldPoint manifoldPoint = pointCache[i]; + // contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction) + if (!validContactDistance(manifoldPoint)) { + removeContactPoint(i); + } + else { + // contact also becomes invalid when relative movement orthogonal to normal exceeds margin + tmp.scale(manifoldPoint.distance1, manifoldPoint.normalWorldOnB); + projectedPoint.sub(manifoldPoint.positionWorldOnA, tmp); + projectedDifference.sub(manifoldPoint.positionWorldOnB, projectedPoint); + distance2d = projectedDifference.dot(projectedDifference); + if (distance2d > getContactBreakingThreshold() * getContactBreakingThreshold()) { + removeContactPoint(i); + } + } + } + //#ifdef DEBUG_PERSISTENCY + // DebugPersistency(); + //#endif // + } + finally { + stack.vectors.pop(); + } + } + + public void clearManifold() { + int i; + for (i = 0; i < cachedPoints; i++) { + clearUserCache(pointCache[i]); + } + cachedPoints = 0; + } + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/PointCollector.java b/src/jbullet/src/javabullet/collision/narrowphase/PointCollector.java new file mode 100644 index 0000000..a754515 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/PointCollector.java @@ -0,0 +1,54 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class PointCollector implements DiscreteCollisionDetectorInterface.Result { + + public final Vector3f normalOnBInWorld = new Vector3f(); + public final Vector3f pointInWorld = new Vector3f(); + public float distance = 1e30f; // negative means penetration + + public boolean hasResult = false; + + public void setShapeIdentifiers(int partId0, int index0, int partId1, int index1) { + // ?? + } + + public void addContactPoint(Vector3f normalOnBInWorld, Vector3f pointInWorld, float depth) { + if (depth < distance) { + hasResult = true; + this.normalOnBInWorld.set(normalOnBInWorld); + this.pointInWorld.set(pointInWorld); + // negative means penetration + distance = depth; + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/SimplexSolverInterface.java b/src/jbullet/src/javabullet/collision/narrowphase/SimplexSolverInterface.java new file mode 100644 index 0000000..13c1a54 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/SimplexSolverInterface.java @@ -0,0 +1,59 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javax.vecmath.Vector3f; + +/** + * SimplexSolverInterface can incrementally calculate distance between origin and up to 4 vertices + * Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on + * voronoi regions or barycentric coordinates. + * + * @author jezek2 + */ +public interface SimplexSolverInterface { + + public void reset(); + + public void addVertex(Vector3f w, Vector3f p, Vector3f q); + + public boolean closest(Vector3f v); + + public float maxVertex(); + + public boolean fullSimplex(); + + public int getSimplex(Vector3f[] pBuf, Vector3f[] qBuf, Vector3f[] yBuf); + + public boolean inSimplex(Vector3f w); + + public void backup_closest(Vector3f v); + + public boolean emptySimplex(); + + public void compute_points(Vector3f p1, Vector3f p2); + + public int numVertices(); + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/SubsimplexConvexCast.java b/src/jbullet/src/javabullet/collision/narrowphase/SubsimplexConvexCast.java new file mode 100644 index 0000000..17a7aa8 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/SubsimplexConvexCast.java @@ -0,0 +1,169 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javabullet.collision.shapes.ConvexShape; +import javabullet.collision.shapes.MinkowskiSumShape; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * SubsimplexConvexCast implements Gino van den Bergens' paper + * "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection" + * GJK based Ray Cast, optimized version + * Objects should not start in overlap, otherwise results are not defined. + * + * @author jezek2 + */ +public class SubsimplexConvexCast implements ConvexCast { + + protected final BulletStack stack = BulletStack.get(); + + // Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases. + // See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565 + //#ifdef BT_USE_DOUBLE_PRECISION + //#define MAX_ITERATIONS 64 + //#else + //#define MAX_ITERATIONS 32 + //#endif + + private static final int MAX_ITERATIONS = 32; + + private SimplexSolverInterface simplexSolver; + private ConvexShape convexA; + private ConvexShape convexB; + + public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, SimplexSolverInterface simplexSolver) { + this.convexA = shapeA; + this.convexB = shapeB; + this.simplexSolver = simplexSolver; + } + + public boolean calcTimeOfImpact(Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result) { + stack.pushCommonMath(); + try { + MinkowskiSumShape combi = new MinkowskiSumShape(convexA, convexB); + MinkowskiSumShape convex = combi; + + Transform rayFromLocalA = stack.transforms.get(); + Transform rayToLocalA = stack.transforms.get(); + + rayFromLocalA.inverse(fromA); + rayFromLocalA.mul(fromB); + + rayToLocalA.inverse(toA); + rayToLocalA.mul(toB); + + simplexSolver.reset(); + + convex.setTransformB(stack.transforms.get(rayFromLocalA.basis)); + + //btScalar radius = btScalar(0.01); + + float lambda = 0f; + // todo: need to verify this: + // because of minkowski difference, we need the inverse direction + + Vector3f s = stack.vectors.get(); + s.negate(rayFromLocalA.origin); + + //Vector3f r = -(rayToLocalA.getOrigin()-rayFromLocalA.getOrigin()); + Vector3f r = stack.vectors.get(); + r.sub(rayToLocalA.origin, rayFromLocalA.origin); + r.negate(); + + Vector3f x = stack.vectors.get(s); + Vector3f v = stack.vectors.get(); + Vector3f arbitraryPoint = stack.vectors.get(convex.localGetSupportingVertex(r)); + + v.sub(x, arbitraryPoint); + + int maxIter = MAX_ITERATIONS; + + Vector3f n = stack.vectors.get(0f, 0f, 0f); + boolean hasResult = false; + Vector3f c = stack.vectors.get(); + + float lastLambda = lambda; + + float dist2 = v.lengthSquared(); + //#ifdef BT_USE_DOUBLE_PRECISION + // btScalar epsilon = btScalar(0.0001); + //#else + float epsilon = 0.0001f; + //#endif + Vector3f w = stack.vectors.get(), p = stack.vectors.get(); + float VdotR; + + while ((dist2 > epsilon) && (maxIter--) != 0) { + p.set(convex.localGetSupportingVertex(v)); + w.sub(x, p); + + float VdotW = v.dot(w); + + if (VdotW > 0f) { + VdotR = v.dot(r); + + if (VdotR >= -(BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) { + return false; + } + else { + lambda = lambda - VdotW / VdotR; + x.scaleAdd(lambda, r, s); + simplexSolver.reset(); + // check next line + w.sub(x, p); + lastLambda = lambda; + n.set(v); + hasResult = true; + } + } + simplexSolver.addVertex(w, x, p); + if (simplexSolver.closest(v)) { + dist2 = v.lengthSquared(); + hasResult = true; + //printf("V=%f , %f, %f\n",v[0],v[1],v[2]); + //printf("DIST2=%f\n",dist2); + //printf("numverts = %i\n",m_simplexSolver->numVertices()); + } + else { + dist2 = 0f; + } + } + + //int numiter = MAX_ITERATIONS - maxIter; + // printf("number of iterations: %d", numiter); + result.fraction = lambda; + result.normal.set(n); + + return true; + } + finally { + stack.popCommonMath(); + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/TriangleConvexcastCallback.java b/src/jbullet/src/javabullet/collision/narrowphase/TriangleConvexcastCallback.java new file mode 100644 index 0000000..430d3a5 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/TriangleConvexcastCallback.java @@ -0,0 +1,92 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javabullet.collision.narrowphase.ConvexCast.CastResult; +import javabullet.collision.shapes.ConvexShape; +import javabullet.collision.shapes.TriangleCallback; +import javabullet.collision.shapes.TriangleShape; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public abstract class TriangleConvexcastCallback implements TriangleCallback { + + public ConvexShape convexShape; + public final Transform convexShapeFrom = new Transform(); + public final Transform convexShapeTo = new Transform(); + public final Transform triangleToWorld = new Transform(); + public float hitFraction; + + public TriangleConvexcastCallback(ConvexShape convexShape, Transform convexShapeFrom, Transform convexShapeTo, Transform triangleToWorld) { + this.convexShape = convexShape; + this.convexShapeFrom.set(convexShapeFrom); + this.convexShapeTo.set(convexShapeTo); + this.triangleToWorld.set(triangleToWorld); + this.hitFraction = 1f; + } + + public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) { + TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]); + + VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver(); + + //#define USE_SUBSIMPLEX_CONVEX_CAST 1 + //#ifdef USE_SUBSIMPLEX_CONVEX_CAST + // TODO: implement ContinuousConvexCollision + SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(convexShape, triangleShape, simplexSolver); + //#else + // //btGjkConvexCast convexCaster(m_convexShape,&triangleShape,&simplexSolver); + //btContinuousConvexCollision convexCaster(m_convexShape,&triangleShape,&simplexSolver,NULL); + //#endif //#USE_SUBSIMPLEX_CONVEX_CAST + + CastResult castResult = new CastResult(); + castResult.fraction = 1f; + if (convexCaster.calcTimeOfImpact(convexShapeFrom, convexShapeTo, triangleToWorld, triangleToWorld, castResult)) { + // add hit + if (castResult.normal.lengthSquared() > 0.0001f) { + if (castResult.fraction < hitFraction) { + + //#ifdef USE_SUBSIMPLEX_CONVEX_CAST + // rotate normal into worldspace + convexShapeFrom.basis.transform(castResult.normal); + //#endif //USE_SUBSIMPLEX_CONVEX_CAST + castResult.normal.normalize(); + + reportHit(castResult.normal, + castResult.hitPoint, + castResult.fraction, + partId, + triangleIndex); + } + } + } + } + + public abstract float reportHit(Vector3f hitNormalLocal, Vector3f hitPointLocal, float hitFraction, int partId, int triangleIndex); + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/TriangleRaycastCallback.java b/src/jbullet/src/javabullet/collision/narrowphase/TriangleRaycastCallback.java new file mode 100644 index 0000000..a5ccf60 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/TriangleRaycastCallback.java @@ -0,0 +1,128 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javabullet.BulletStack; +import javabullet.collision.shapes.TriangleCallback; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public abstract class TriangleRaycastCallback implements TriangleCallback { + + protected final BulletStack stack = BulletStack.get(); + + public final Vector3f from = new Vector3f(); + public final Vector3f to = new Vector3f(); + + public float hitFraction; + + public TriangleRaycastCallback(Vector3f from, Vector3f to) { + this.from.set(from); + this.to.set(to); + this.hitFraction = 1f; + } + + public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) { + stack.vectors.push(); + try { + Vector3f vert0 = triangle[0]; + Vector3f vert1 = triangle[1]; + Vector3f vert2 = triangle[2]; + + Vector3f v10 = stack.vectors.get(); + v10.sub(vert1, vert0); + + Vector3f v20 = stack.vectors.get(); + v20.sub(vert2, vert0); + + Vector3f triangleNormal = stack.vectors.get(); + triangleNormal.cross(v10, v20); + + float dist = vert0.dot(triangleNormal); + float dist_a = triangleNormal.dot(from); + dist_a -= dist; + float dist_b = triangleNormal.dot(to); + dist_b -= dist; + + if (dist_a * dist_b >= 0f) { + return; // same sign + } + + float proj_length = dist_a - dist_b; + float distance = (dist_a) / (proj_length); + // Now we have the intersection point on the plane, we'll see if it's inside the triangle + // Add an epsilon as a tolerance for the raycast, + // in case the ray hits exacly on the edge of the triangle. + // It must be scaled for the triangle size. + + if (distance < hitFraction) { + float edge_tolerance = triangleNormal.lengthSquared(); + edge_tolerance *= -0.0001f; + Vector3f point = new Vector3f(); + VectorUtil.setInterpolate3(point, from, to, distance); + { + Vector3f v0p = stack.vectors.get(); + v0p.sub(vert0, point); + Vector3f v1p = stack.vectors.get(); + v1p.sub(vert1, point); + Vector3f cp0 = stack.vectors.get(); + cp0.cross(v0p, v1p); + + if (cp0.dot(triangleNormal) >= edge_tolerance) { + Vector3f v2p = stack.vectors.get(); + v2p.sub(vert2, point); + Vector3f cp1 = stack.vectors.get(); + cp1.cross(v1p, v2p); + if (cp1.dot(triangleNormal) >= edge_tolerance) { + Vector3f cp2 = stack.vectors.get(); + cp2.cross(v2p, v0p); + + if (cp2.dot(triangleNormal) >= edge_tolerance) { + + if (dist_a > 0f) { + hitFraction = reportHit(triangleNormal, distance, partId, triangleIndex); + } + else { + Vector3f tmp = stack.vectors.get(); + tmp.negate(triangleNormal); + hitFraction = reportHit(tmp, distance, partId, triangleIndex); + } + } + } + } + } + } + } + finally { + stack.vectors.pop(); + } + } + + public abstract float reportHit(Vector3f hitNormalLocal, float hitFraction, int partId, int triangleIndex ); + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/VoronoiSimplexSolver.java b/src/jbullet/src/javabullet/collision/narrowphase/VoronoiSimplexSolver.java new file mode 100644 index 0000000..ec504e4 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/VoronoiSimplexSolver.java @@ -0,0 +1,739 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.narrowphase; + +import javabullet.BulletPool; +import javabullet.BulletStack; +import javabullet.ObjectPool; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class VoronoiSimplexSolver implements SimplexSolverInterface { + + protected final BulletStack stack = BulletStack.get(); + protected final ObjectPool<SubSimplexClosestResult> subsimplexResultsPool = BulletPool.get(SubSimplexClosestResult.class); + + private static final int VORONOI_SIMPLEX_MAX_VERTS = 5; + + private static final int VERTA = 0; + private static final int VERTB = 1; + private static final int VERTC = 2; + private static final int VERTD = 3; + + public int numVertices; + + public final Vector3f[] simplexVectorW = new Vector3f[VORONOI_SIMPLEX_MAX_VERTS]; + public final Vector3f[] simplexPointsP = new Vector3f[VORONOI_SIMPLEX_MAX_VERTS]; + public final Vector3f[] simplexPointsQ = new Vector3f[VORONOI_SIMPLEX_MAX_VERTS]; + + public final Vector3f cachedP1 = new Vector3f(); + public final Vector3f cachedP2 = new Vector3f(); + public final Vector3f cachedV = new Vector3f(); + public final Vector3f lastW = new Vector3f(); + public boolean cachedValidClosest; + + public final SubSimplexClosestResult cachedBC = new SubSimplexClosestResult(); + + public boolean needsUpdate; + + { + for (int i=0; i<VORONOI_SIMPLEX_MAX_VERTS; i++) { + simplexVectorW[i] = new Vector3f(); + simplexPointsP[i] = new Vector3f(); + simplexPointsQ[i] = new Vector3f(); + } + } + + public void removeVertex(int index) { + assert(numVertices>0); + numVertices--; + simplexVectorW[index].set(simplexVectorW[numVertices]); + simplexPointsP[index].set(simplexPointsP[numVertices]); + simplexPointsQ[index].set(simplexPointsQ[numVertices]); + } + + public void reduceVertices(UsageBitfield usedVerts) { + if ((numVertices() >= 4) && (!usedVerts.usedVertexD)) + removeVertex(3); + + if ((numVertices() >= 3) && (!usedVerts.usedVertexC)) + removeVertex(2); + + if ((numVertices() >= 2) && (!usedVerts.usedVertexB)) + removeVertex(1); + + if ((numVertices() >= 1) && (!usedVerts.usedVertexA)) + removeVertex(0); + } + + public boolean updateClosestVectorAndPoints() { + stack.vectors.push(); + try { + if (needsUpdate) + { + Vector3f tmp = stack.vectors.get(); + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + Vector3f tmp3 = stack.vectors.get(); + Vector3f tmp4 = stack.vectors.get(); + + cachedBC.reset(); + + needsUpdate = false; + + switch (numVertices()) + { + case 0: + cachedValidClosest = false; + break; + case 1: + { + cachedP1.set(simplexPointsP[0]); + cachedP2.set(simplexPointsQ[0]); + cachedV.sub(cachedP1, cachedP2); //== m_simplexVectorW[0] + cachedBC.reset(); + cachedBC.setBarycentricCoordinates(1f, 0f, 0f, 0f); + cachedValidClosest = cachedBC.isValid(); + break; + } + case 2: + { + //closest point origin from line segment + Vector3f from = simplexVectorW[0]; + Vector3f to = simplexVectorW[1]; + Vector3f nearest = stack.vectors.get(); + + Vector3f p = stack.vectors.get(0f, 0f, 0f); + Vector3f diff = stack.vectors.get(); + diff.sub(p, from); + + Vector3f v = stack.vectors.get(); + v.sub(to, from); + + float t = v.dot(diff); + + if (t > 0) { + float dotVV = v.dot(v); + if (t < dotVV) { + t /= dotVV; + tmp.scale(t, v); + diff.sub(tmp); + cachedBC.usedVertices.usedVertexA = true; + cachedBC.usedVertices.usedVertexB = true; + } else { + t = 1; + diff.sub(v); + // reduce to 1 point + cachedBC.usedVertices.usedVertexB = true; + } + } else + { + t = 0; + //reduce to 1 point + cachedBC.usedVertices.usedVertexA = true; + } + cachedBC.setBarycentricCoordinates(1f-t, t, 0f, 0f); + tmp.scale(t, v); + nearest.add(from, tmp); + + tmp.sub(simplexPointsP[1], simplexPointsP[0]); + tmp.scale(t); + cachedP1.add(simplexPointsP[0], tmp); + + tmp.sub(simplexPointsQ[1], simplexPointsQ[0]); + tmp.scale(t); + cachedP2.add(simplexPointsQ[0], tmp); + + cachedV.sub(cachedP1, cachedP2); + + reduceVertices(cachedBC.usedVertices); + + cachedValidClosest = cachedBC.isValid(); + break; + } + case 3: + { + // closest point origin from triangle + Vector3f p = stack.vectors.get(0f, 0f, 0f); + + Vector3f a = simplexVectorW[0]; + Vector3f b = simplexVectorW[1]; + Vector3f c = simplexVectorW[2]; + + closestPtPointTriangle(p,a,b,c,cachedBC); + + tmp1.scale(cachedBC.barycentricCoords[0], simplexPointsP[0]); + tmp2.scale(cachedBC.barycentricCoords[1], simplexPointsP[1]); + tmp3.scale(cachedBC.barycentricCoords[2], simplexPointsP[2]); + VectorUtil.add(cachedP1, tmp1, tmp2, tmp3); + + tmp1.scale(cachedBC.barycentricCoords[0], simplexPointsQ[0]); + tmp2.scale(cachedBC.barycentricCoords[1], simplexPointsQ[1]); + tmp3.scale(cachedBC.barycentricCoords[2], simplexPointsQ[2]); + VectorUtil.add(cachedP2, tmp1, tmp2, tmp3); + + cachedV.sub(cachedP1, cachedP2); + + reduceVertices(cachedBC.usedVertices); + cachedValidClosest = cachedBC.isValid(); + + break; + } + case 4: + { + Vector3f p = stack.vectors.get(0f, 0f, 0f); + + Vector3f a = simplexVectorW[0]; + Vector3f b = simplexVectorW[1]; + Vector3f c = simplexVectorW[2]; + Vector3f d = simplexVectorW[3]; + + boolean hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,cachedBC); + + if (hasSeperation) + { + tmp1.scale(cachedBC.barycentricCoords[0], simplexPointsP[0]); + tmp2.scale(cachedBC.barycentricCoords[1], simplexPointsP[1]); + tmp3.scale(cachedBC.barycentricCoords[2], simplexPointsP[2]); + tmp4.scale(cachedBC.barycentricCoords[3], simplexPointsP[3]); + VectorUtil.add(cachedP1, tmp1, tmp2, tmp3, tmp4); + + tmp1.scale(cachedBC.barycentricCoords[0], simplexPointsQ[0]); + tmp2.scale(cachedBC.barycentricCoords[1], simplexPointsQ[1]); + tmp3.scale(cachedBC.barycentricCoords[2], simplexPointsQ[2]); + tmp4.scale(cachedBC.barycentricCoords[3], simplexPointsQ[3]); + VectorUtil.add(cachedP2, tmp1, tmp2, tmp3, tmp4); + + cachedV.sub(cachedP1, cachedP2); + reduceVertices (cachedBC.usedVertices); + } else + { + // printf("sub distance got penetration\n"); + + if (cachedBC.degenerate) + { + cachedValidClosest = false; + } else + { + cachedValidClosest = true; + //degenerate case == false, penetration = true + zero + cachedV.set(0f, 0f, 0f); + } + break; + } + + cachedValidClosest = cachedBC.isValid(); + + //closest point origin from tetrahedron + break; + } + default: + { + cachedValidClosest = false; + } + } + } + + return cachedValidClosest; + } + finally { + stack.vectors.pop(); + } + } + + public boolean closestPtPointTriangle(Vector3f p, Vector3f a, Vector3f b, Vector3f c, SubSimplexClosestResult result) { + stack.vectors.push(); + try { + result.usedVertices.reset(); + + // Check if P in vertex region outside A + Vector3f ab = stack.vectors.get(); + ab.sub(b, a); + + Vector3f ac = stack.vectors.get(); + ac.sub(c, a); + + Vector3f ap = stack.vectors.get(); + ap.sub(p, a); + + float d1 = ab.dot(ap); + float d2 = ac.dot(ap); + + if (d1 <= 0f && d2 <= 0f) + { + result.closestPointOnSimplex.set(a); + result.usedVertices.usedVertexA = true; + result.setBarycentricCoordinates(1f, 0f, 0f, 0f); + return true; // a; // barycentric coordinates (1,0,0) + } + + // Check if P in vertex region outside B + Vector3f bp = stack.vectors.get(); + bp.sub(p, b); + + float d3 = ab.dot(bp); + float d4 = ac.dot(bp); + + if (d3 >= 0f && d4 <= d3) + { + result.closestPointOnSimplex.set(b); + result.usedVertices.usedVertexB = true; + result.setBarycentricCoordinates(0, 1f, 0f, 0f); + + return true; // b; // barycentric coordinates (0,1,0) + } + + // Check if P in edge region of AB, if so return projection of P onto AB + float vc = d1*d4 - d3*d2; + if (vc <= 0f && d1 >= 0f && d3 <= 0f) { + float v = d1 / (d1 - d3); + result.closestPointOnSimplex.scaleAdd(v, ab, a); + result.usedVertices.usedVertexA = true; + result.usedVertices.usedVertexB = true; + result.setBarycentricCoordinates(1f-v, v, 0f, 0f); + return true; + //return a + v * ab; // barycentric coordinates (1-v,v,0) + } + + // Check if P in vertex region outside C + Vector3f cp = stack.vectors.get(); + cp.sub(p, c); + + float d5 = ab.dot(cp); + float d6 = ac.dot(cp); + + if (d6 >= 0f && d5 <= d6) + { + result.closestPointOnSimplex.set(c); + result.usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(0f, 0f, 1f, 0f); + return true;//c; // barycentric coordinates (0,0,1) + } + + // Check if P in edge region of AC, if so return projection of P onto AC + float vb = d5*d2 - d1*d6; + if (vb <= 0f && d2 >= 0f && d6 <= 0f) { + float w = d2 / (d2 - d6); + result.closestPointOnSimplex.scaleAdd(w, ac, a); + result.usedVertices.usedVertexA = true; + result.usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(1f-w, 0f, w, 0f); + return true; + //return a + w * ac; // barycentric coordinates (1-w,0,w) + } + + // Check if P in edge region of BC, if so return projection of P onto BC + float va = d3*d6 - d5*d4; + if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f) { + float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + + Vector3f tmp = stack.vectors.get(); + tmp.sub(c, b); + result.closestPointOnSimplex.scaleAdd(w, tmp, b); + + result.usedVertices.usedVertexB = true; + result.usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(0, 1f-w, w, 0f); + return true; + // return b + w * (c - b); // barycentric coordinates (0,1-w,w) + } + + // P inside face region. Compute Q through its barycentric coordinates (u,v,w) + float denom = 1f / (va + vb + vc); + float v = vb * denom; + float w = vc * denom; + + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + tmp1.scale(v, ab); + tmp2.scale(w, ac); + VectorUtil.add(result.closestPointOnSimplex, a, tmp1, tmp2); + result.usedVertices.usedVertexA = true; + result.usedVertices.usedVertexB = true; + result.usedVertices.usedVertexC = true; + result.setBarycentricCoordinates(1f-v-w, v, w, 0f); + + return true; + // return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w + } + finally { + stack.vectors.pop(); + } + } + + /// Test if point p and d lie on opposite sides of plane through abc + public /*static*/ int pointOutsideOfPlane(Vector3f p, Vector3f a, Vector3f b, Vector3f c, Vector3f d) + { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + + Vector3f normal = stack.vectors.get(); + normal.sub(b, a); + tmp.sub(c, a); + normal.cross(normal, tmp); + + tmp.sub(p, a); + float signp = tmp.dot(normal); // [AP AB AC] + + tmp.sub(d, a); + float signd = tmp.dot(normal); // [AD AB AC] + + //#ifdef CATCH_DEGENERATE_TETRAHEDRON + // #ifdef BT_USE_DOUBLE_PRECISION + // if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) + // { + // return -1; + // } + // #else + if (signd * signd < ((1e-4f) * (1e-4f))) + { + // printf("affine dependent/degenerate\n");// + return -1; + } + //#endif + + //#endif + // Points on opposite sides if expression signs are opposite + return (signp * signd < 0f)? 1 : 0; + } + finally { + stack.vectors.pop(); + } + } + + public boolean closestPtPointTetrahedron(Vector3f p, Vector3f a, Vector3f b, Vector3f c, Vector3f d, SubSimplexClosestResult finalResult) { + stack.vectors.push(); + SubSimplexClosestResult tempResult = subsimplexResultsPool.get(); + tempResult.reset(); + try { + Vector3f tmp = stack.vectors.get(); + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + // Start out assuming point inside all halfspaces, so closest to itself + finalResult.closestPointOnSimplex.set(p); + finalResult.usedVertices.reset(); + finalResult.usedVertices.usedVertexA = true; + finalResult.usedVertices.usedVertexB = true; + finalResult.usedVertices.usedVertexC = true; + finalResult.usedVertices.usedVertexD = true; + + int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d); + int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b); + int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c); + int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a); + + if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) + { + finalResult.degenerate = true; + return false; + } + + if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0) + { + return false; + } + + + float bestSqDist = Float.MAX_VALUE; + // If point outside face abc then compute closest point on abc + if (pointOutsideABC != 0) + { + closestPtPointTriangle(p, a, b, c,tempResult); + Vector3f q = stack.vectors.get(tempResult.closestPointOnSimplex); + + tmp.sub(q, p); + float sqDist = tmp.dot(tmp); + // Update best closest point if (squared) distance is less than current best + if (sqDist < bestSqDist) { + bestSqDist = sqDist; + finalResult.closestPointOnSimplex.set(q); + //convert result bitmask! + finalResult.usedVertices.reset(); + finalResult.usedVertices.usedVertexA = tempResult.usedVertices.usedVertexA; + finalResult.usedVertices.usedVertexB = tempResult.usedVertices.usedVertexB; + finalResult.usedVertices.usedVertexC = tempResult.usedVertices.usedVertexC; + finalResult.setBarycentricCoordinates( + tempResult.barycentricCoords[VERTA], + tempResult.barycentricCoords[VERTB], + tempResult.barycentricCoords[VERTC], + 0 + ); + + } + } + + + // Repeat test for face acd + if (pointOutsideACD != 0) + { + closestPtPointTriangle(p, a, c, d,tempResult); + Vector3f q = stack.vectors.get(tempResult.closestPointOnSimplex); + //convert result bitmask! + + tmp.sub(q, p); + float sqDist = tmp.dot(tmp); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.closestPointOnSimplex.set(q); + finalResult.usedVertices.reset(); + finalResult.usedVertices.usedVertexA = tempResult.usedVertices.usedVertexA; + + finalResult.usedVertices.usedVertexC = tempResult.usedVertices.usedVertexB; + finalResult.usedVertices.usedVertexD = tempResult.usedVertices.usedVertexC; + finalResult.setBarycentricCoordinates( + tempResult.barycentricCoords[VERTA], + 0, + tempResult.barycentricCoords[VERTB], + tempResult.barycentricCoords[VERTC] + ); + + } + } + // Repeat test for face adb + + + if (pointOutsideADB != 0) + { + closestPtPointTriangle(p, a, d, b,tempResult); + Vector3f q = stack.vectors.get(tempResult.closestPointOnSimplex); + //convert result bitmask! + + tmp.sub(q, p); + float sqDist = tmp.dot(tmp); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.closestPointOnSimplex.set(q); + finalResult.usedVertices.reset(); + finalResult.usedVertices.usedVertexA = tempResult.usedVertices.usedVertexA; + finalResult.usedVertices.usedVertexB = tempResult.usedVertices.usedVertexC; + + finalResult.usedVertices.usedVertexD = tempResult.usedVertices.usedVertexB; + finalResult.setBarycentricCoordinates( + tempResult.barycentricCoords[VERTA], + tempResult.barycentricCoords[VERTC], + 0, + tempResult.barycentricCoords[VERTB] + ); + + } + } + // Repeat test for face bdc + + + if (pointOutsideBDC != 0) + { + closestPtPointTriangle(p, b, d, c,tempResult); + Vector3f q = stack.vectors.get(tempResult.closestPointOnSimplex); + //convert result bitmask! + tmp.sub(q, p); + float sqDist = tmp.dot(tmp); + if (sqDist < bestSqDist) + { + bestSqDist = sqDist; + finalResult.closestPointOnSimplex.set(q); + finalResult.usedVertices.reset(); + // + finalResult.usedVertices.usedVertexB = tempResult.usedVertices.usedVertexA; + finalResult.usedVertices.usedVertexC = tempResult.usedVertices.usedVertexC; + finalResult.usedVertices.usedVertexD = tempResult.usedVertices.usedVertexB; + + finalResult.setBarycentricCoordinates( + 0, + tempResult.barycentricCoords[VERTA], + tempResult.barycentricCoords[VERTC], + tempResult.barycentricCoords[VERTB] + ); + + } + } + + //help! we ended up full ! + + if (finalResult.usedVertices.usedVertexA && + finalResult.usedVertices.usedVertexB && + finalResult.usedVertices.usedVertexC && + finalResult.usedVertices.usedVertexD) + { + return true; + } + + return true; + } + finally { + stack.vectors.pop(); + subsimplexResultsPool.release(tempResult); + } + } + + /** + * Clear the simplex, remove all the vertices. + */ + public void reset() { + cachedValidClosest = false; + numVertices = 0; + needsUpdate = true; + lastW.set(1e30f, 1e30f, 1e30f); + cachedBC.reset(); + } + + public void addVertex(Vector3f w, Vector3f p, Vector3f q) { + lastW.set(w); + needsUpdate = true; + + simplexVectorW[numVertices].set(w); + simplexPointsP[numVertices].set(p); + simplexPointsQ[numVertices].set(q); + + numVertices++; + } + + /** + * Return/calculate the closest vertex. + */ + public boolean closest(Vector3f v) { + boolean succes = updateClosestVectorAndPoints(); + v.set(cachedV); + return succes; + } + + public float maxVertex() { + int i, numverts = numVertices(); + float maxV = 0f; + for (i = 0; i < numverts; i++) { + float curLen2 = simplexVectorW[i].lengthSquared(); + if (maxV < curLen2) { + maxV = curLen2; + } + } + return maxV; + } + + public boolean fullSimplex() { + return (numVertices == 4); + } + + public int getSimplex(Vector3f[] pBuf, Vector3f[] qBuf, Vector3f[] yBuf) { + for (int i = 0; i < numVertices(); i++) { + yBuf[i].set(simplexVectorW[i]); + pBuf[i].set(simplexPointsP[i]); + qBuf[i].set(simplexPointsQ[i]); + } + return numVertices(); + } + + public boolean inSimplex(Vector3f w) { + boolean found = false; + int i, numverts = numVertices(); + //btScalar maxV = btScalar(0.); + + //w is in the current (reduced) simplex + for (i = 0; i < numverts; i++) { + if (simplexVectorW[i].equals(w)) { + found = true; + } + } + + //check in case lastW is already removed + if (w.equals(lastW)) { + return true; + } + + return found; + } + + public void backup_closest(Vector3f v) { + v.set(cachedV); + } + + public boolean emptySimplex() { + return (numVertices() == 0); + } + + public void compute_points(Vector3f p1, Vector3f p2) { + updateClosestVectorAndPoints(); + p1.set(cachedP1); + p2.set(cachedP2); + } + + public int numVertices() { + return numVertices; + } + + //////////////////////////////////////////////////////////////////////////// + + public static class UsageBitfield { + public boolean usedVertexA; + public boolean usedVertexB; + public boolean usedVertexC; + public boolean usedVertexD; + + public void reset() { + usedVertexA = false; + usedVertexB = false; + usedVertexC = false; + usedVertexD = false; + } + } + + public static class SubSimplexClosestResult { + public final Vector3f closestPointOnSimplex = new Vector3f(); + //MASK for m_usedVertices + //stores the simplex vertex-usage, using the MASK, + // if m_usedVertices & MASK then the related vertex is used + public final UsageBitfield usedVertices = new UsageBitfield(); + public final float[] barycentricCoords = new float[4]; + public boolean degenerate; + + public void reset() { + degenerate = false; + setBarycentricCoordinates(0f, 0f, 0f, 0f); + usedVertices.reset(); + } + + public boolean isValid() { + boolean valid = (barycentricCoords[0] >= 0f) && + (barycentricCoords[1] >= 0f) && + (barycentricCoords[2] >= 0f) && + (barycentricCoords[3] >= 0f); + return valid; + } + + public void setBarycentricCoordinates(float a, float b, float c, float d) { + barycentricCoords[0] = a; + barycentricCoords[1] = b; + barycentricCoords[2] = c; + barycentricCoords[3] = d; + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/narrowphase/package-info.java b/src/jbullet/src/javabullet/collision/narrowphase/package-info.java new file mode 100644 index 0000000..cafa61a --- /dev/null +++ b/src/jbullet/src/javabullet/collision/narrowphase/package-info.java @@ -0,0 +1,28 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +/** + * Narrow-phase collision. + */ +package javabullet.collision.narrowphase; + diff --git a/src/jbullet/src/javabullet/collision/shapes/BoxShape.java b/src/jbullet/src/javabullet/collision/shapes/BoxShape.java new file mode 100644 index 0000000..68dd46d --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/BoxShape.java @@ -0,0 +1,400 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.linearmath.MatrixUtil; +import javabullet.linearmath.ScalarUtil; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Matrix3f; +import javax.vecmath.Vector3f; +import javax.vecmath.Vector4f; + +/** + * BoxShape implements both a feature based (vertex/edge/plane) and implicit (getSupportingVertex) Box. + * + * @author jezek2 + */ +public class BoxShape extends PolyhedralConvexShape { + + public BoxShape(Vector3f boxHalfExtents) { + Vector3f margin = new Vector3f(getMargin(), getMargin(), getMargin()); + VectorUtil.mul(implicitShapeDimensions, boxHalfExtents, localScaling); + implicitShapeDimensions.sub(margin); + } + + public Vector3f getHalfExtentsWithMargin() { + stack.vectors.push(); + try { + Vector3f halfExtents = stack.vectors.get(getHalfExtentsWithoutMargin()); + Vector3f margin = stack.vectors.get(getMargin(), getMargin(), getMargin()); + halfExtents.add(margin); + return stack.vectors.returning(halfExtents); + } + finally { + stack.vectors.pop(); + } + } + + public Vector3f getHalfExtentsWithoutMargin() { + return implicitShapeDimensions; // changed in Bullet 2.63: assume the scaling and margin are included + } + + @Override + public BroadphaseNativeType getShapeType() { + return BroadphaseNativeType.BOX_SHAPE_PROXYTYPE; + } + + @Override + public Vector3f localGetSupportingVertex(Vector3f vec) { + stack.vectors.push(); + try { + Vector3f halfExtents = stack.vectors.get(getHalfExtentsWithoutMargin()); + Vector3f margin = stack.vectors.get(getMargin(), getMargin(), getMargin()); + halfExtents.add(margin); + + return stack.vectors.returning(stack.vectors.get( + ScalarUtil.fsel(vec.x, halfExtents.x, -halfExtents.x), + ScalarUtil.fsel(vec.y, halfExtents.y, -halfExtents.y), + ScalarUtil.fsel(vec.z, halfExtents.z, -halfExtents.z))); + } + finally { + stack.vectors.pop(); + } + } + + @Override + public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) { + stack.vectors.push(); + try { + Vector3f halfExtents = stack.vectors.get(getHalfExtentsWithoutMargin()); + + return stack.vectors.returning(stack.vectors.get( + ScalarUtil.fsel(vec.x, halfExtents.x, -halfExtents.x), + ScalarUtil.fsel(vec.y, halfExtents.y, -halfExtents.y), + ScalarUtil.fsel(vec.z, halfExtents.z, -halfExtents.z))); + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) { + stack.vectors.push(); + try { + Vector3f halfExtents = stack.vectors.get(getHalfExtentsWithoutMargin()); + + for (int i = 0; i < numVectors; i++) { + Vector3f vec = vectors[i]; + supportVerticesOut[i].set(ScalarUtil.fsel(vec.x, halfExtents.x, -halfExtents.x), + ScalarUtil.fsel(vec.y, halfExtents.y, -halfExtents.y), + ScalarUtil.fsel(vec.z, halfExtents.z, -halfExtents.z)); + } + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void setMargin(float margin) { + stack.vectors.push(); + try { + // correct the implicitShapeDimensions for the margin + Vector3f oldMargin = stack.vectors.get(getMargin(), getMargin(), getMargin()); + Vector3f implicitShapeDimensionsWithMargin = stack.vectors.get(); + implicitShapeDimensionsWithMargin.add(implicitShapeDimensions, oldMargin); + + super.setMargin(margin); + Vector3f newMargin = stack.vectors.get(getMargin(), getMargin(), getMargin()); + implicitShapeDimensions.sub(implicitShapeDimensionsWithMargin, newMargin); + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void setLocalScaling(Vector3f scaling) { + stack.vectors.push(); + try { + Vector3f oldMargin = stack.vectors.get(getMargin(), getMargin(), getMargin()); + Vector3f implicitShapeDimensionsWithMargin = stack.vectors.get(); + implicitShapeDimensionsWithMargin.add(implicitShapeDimensions, oldMargin); + Vector3f unScaledImplicitShapeDimensionsWithMargin = stack.vectors.get(); + VectorUtil.div(unScaledImplicitShapeDimensionsWithMargin, implicitShapeDimensionsWithMargin, localScaling); + + super.setLocalScaling(scaling); + + VectorUtil.mul(implicitShapeDimensions, unScaledImplicitShapeDimensionsWithMargin, localScaling); + implicitShapeDimensions.sub(oldMargin); + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) { + stack.pushCommonMath(); + try { + Vector3f halfExtents = getHalfExtentsWithoutMargin(); + + Matrix3f abs_b = stack.matrices.get(t.basis); + MatrixUtil.absolute(abs_b); + + Vector3f tmp = stack.vectors.get(); + + Vector3f center = stack.vectors.get(t.origin); + Vector3f extent = stack.vectors.get(); + abs_b.getRow(0, tmp); + extent.x = tmp.dot(halfExtents); + abs_b.getRow(1, tmp); + extent.y = tmp.dot(halfExtents); + abs_b.getRow(2, tmp); + extent.z = tmp.dot(halfExtents); + + extent.add(stack.vectors.get(getMargin(), getMargin(), getMargin())); + + aabbMin.sub(center, extent); + aabbMax.add(center, extent); + } + finally { + stack.popCommonMath(); + } + } + + @Override + public void calculateLocalInertia(float mass, Vector3f inertia) { + stack.vectors.push(); + try { + //btScalar margin = btScalar(0.); + Vector3f halfExtents = stack.vectors.get(getHalfExtentsWithMargin()); + + float lx = 2f * halfExtents.x; + float ly = 2f * halfExtents.y; + float lz = 2f * halfExtents.z; + + inertia.set(mass / 12f * (ly * ly + lz * lz), + mass / 12f * (lx * lx + lz * lz), + mass / 12f * (lx * lx + ly * ly)); + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void getPlane(Vector3f planeNormal, Vector3f planeSupport, int i) { + stack.vectors.push(); + stack.vectors4.push(); + try { + // this plane might not be aligned... + Vector4f plane = stack.vectors4.get(); + getPlaneEquation(plane, i); + planeNormal.set(plane.x, plane.y, plane.z); + Vector3f tmp = stack.vectors.get(); + tmp.negate(planeNormal); + planeSupport.set(localGetSupportingVertex(tmp)); + } + finally { + stack.vectors.pop(); + stack.vectors4.pop(); + } + } + + @Override + public int getNumPlanes() { + return 6; + } + + @Override + public int getNumVertices() { + return 8; + } + + @Override + public int getNumEdges() { + return 12; + } + + @Override + public void getVertex(int i, Vector3f vtx) { + // JAVA NOTE: against stack usage, but safe with code below + Vector3f halfExtents = getHalfExtentsWithoutMargin(); + + vtx.set(halfExtents.x * (1 - (i & 1)) - halfExtents.x * (i & 1), + halfExtents.y * (1 - ((i & 2) >> 1)) - halfExtents.y * ((i & 2) >> 1), + halfExtents.z * (1 - ((i & 4) >> 2)) - halfExtents.z * ((i & 4) >> 2)); + } + + public void getPlaneEquation(Vector4f plane, int i) { + // JAVA NOTE: against stack usage, but safe with code below + Vector3f halfExtents = getHalfExtentsWithoutMargin(); + + switch (i) { + case 0: + plane.set(1f, 0f, 0f, -halfExtents.x); + break; + case 1: + plane.set(-1f, 0f, 0f, -halfExtents.x); + break; + case 2: + plane.set(0f, 1f, 0f, -halfExtents.y); + break; + case 3: + plane.set(0f, -1f, 0f, -halfExtents.y); + break; + case 4: + plane.set(0f, 0f, 1f, -halfExtents.z); + break; + case 5: + plane.set(0f, 0f, -1f, -halfExtents.z); + break; + default: + assert (false); + } + } + + @Override + public void getEdge(int i, Vector3f pa, Vector3f pb) { + int edgeVert0 = 0; + int edgeVert1 = 0; + + switch (i) { + case 0: + edgeVert0 = 0; + edgeVert1 = 1; + break; + case 1: + edgeVert0 = 0; + edgeVert1 = 2; + break; + case 2: + edgeVert0 = 1; + edgeVert1 = 3; + + break; + case 3: + edgeVert0 = 2; + edgeVert1 = 3; + break; + case 4: + edgeVert0 = 0; + edgeVert1 = 4; + break; + case 5: + edgeVert0 = 1; + edgeVert1 = 5; + + break; + case 6: + edgeVert0 = 2; + edgeVert1 = 6; + break; + case 7: + edgeVert0 = 3; + edgeVert1 = 7; + break; + case 8: + edgeVert0 = 4; + edgeVert1 = 5; + break; + case 9: + edgeVert0 = 4; + edgeVert1 = 6; + break; + case 10: + edgeVert0 = 5; + edgeVert1 = 7; + break; + case 11: + edgeVert0 = 6; + edgeVert1 = 7; + break; + default: + assert (false); + } + + getVertex(edgeVert0, pa); + getVertex(edgeVert1, pb); + } + + @Override + public boolean isInside(Vector3f pt, float tolerance) { + // JAVA NOTE: against stack usage, but safe with code below + Vector3f halfExtents = getHalfExtentsWithoutMargin(); + + //btScalar minDist = 2*tolerance; + + boolean result = + (pt.x <= (halfExtents.x + tolerance)) && + (pt.x >= (-halfExtents.x - tolerance)) && + (pt.y <= (halfExtents.y + tolerance)) && + (pt.y >= (-halfExtents.y - tolerance)) && + (pt.z <= (halfExtents.z + tolerance)) && + (pt.z >= (-halfExtents.z - tolerance)); + + return result; + } + + @Override + public String getName() { + return "Box"; + } + + @Override + public int getNumPreferredPenetrationDirections() { + return 6; + } + + @Override + public void getPreferredPenetrationDirection(int index, Vector3f penetrationVector) { + switch (index) { + case 0: + penetrationVector.set(1f, 0f, 0f); + break; + case 1: + penetrationVector.set(-1f, 0f, 0f); + break; + case 2: + penetrationVector.set(0f, 1f, 0f); + break; + case 3: + penetrationVector.set(0f, -1f, 0f); + break; + case 4: + penetrationVector.set(0f, 0f, 1f); + break; + case 5: + penetrationVector.set(0f, 0f, -1f); + break; + default: + assert (false); + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/BvhSubtreeInfo.java b/src/jbullet/src/javabullet/collision/shapes/BvhSubtreeInfo.java new file mode 100644 index 0000000..fb82dbc --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/BvhSubtreeInfo.java @@ -0,0 +1,48 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +/** + * BvhSubtreeInfo provides info to gather a subtree of limited size. + * + * @author jezek2 + */ +public class BvhSubtreeInfo { + + public final /*unsigned*/ short[] quantizedAabbMin = new short[3]; + public final /*unsigned*/ short[] quantizedAabbMax = new short[3]; + // points to the root of the subtree + public int rootNodeIndex; + public int subtreeSize; + + public void setAabbFromQuantizeNode(QuantizedBvhNodes quantizedNodes, int nodeId) { + quantizedAabbMin[0] = (short)quantizedNodes.getQuantizedAabbMin(nodeId, 0); + quantizedAabbMin[1] = (short)quantizedNodes.getQuantizedAabbMin(nodeId, 1); + quantizedAabbMin[2] = (short)quantizedNodes.getQuantizedAabbMin(nodeId, 2); + quantizedAabbMax[0] = (short)quantizedNodes.getQuantizedAabbMax(nodeId, 0); + quantizedAabbMax[1] = (short)quantizedNodes.getQuantizedAabbMax(nodeId, 1); + quantizedAabbMax[2] = (short)quantizedNodes.getQuantizedAabbMax(nodeId, 2); + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/BvhTriangleMeshShape.java b/src/jbullet/src/javabullet/collision/shapes/BvhTriangleMeshShape.java new file mode 100644 index 0000000..42878bc --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/BvhTriangleMeshShape.java @@ -0,0 +1,276 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import java.nio.ByteBuffer; +import javabullet.BulletGlobals; +import javabullet.BulletPool; +import javabullet.ObjectPool; +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.collision.narrowphase.TriangleConvexcastCallback; +import javabullet.collision.narrowphase.TriangleRaycastCallback; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * Bvh Concave triangle mesh is a static-triangle mesh shape with Bounding Volume Hierarchy optimization. + * Uses an interface to access the triangles to allow for sharing graphics/physics triangles. + * + * @author jezek2 + */ +public class BvhTriangleMeshShape extends TriangleMeshShape { + + private OptimizedBvh bvh; + private boolean useQuantizedAabbCompression; + private boolean ownsBvh; + + private ObjectPool<MyNodeOverlapCallback> myNodeCallbacks = BulletPool.get(MyNodeOverlapCallback.class); + + public BvhTriangleMeshShape() { + super(null); + this.bvh = null; + this.ownsBvh = false; + } + + public BvhTriangleMeshShape(StridingMeshInterface meshInterface, boolean useQuantizedAabbCompression) { + this(meshInterface, useQuantizedAabbCompression, true); + } + + public BvhTriangleMeshShape(StridingMeshInterface meshInterface, boolean useQuantizedAabbCompression, boolean buildBvh) { + super(meshInterface); + this.bvh = null; + this.useQuantizedAabbCompression = useQuantizedAabbCompression; + this.ownsBvh = false; + + // construct bvh from meshInterface + //#ifndef DISABLE_BVH + + Vector3f bvhAabbMin = new Vector3f(), bvhAabbMax = new Vector3f(); + meshInterface.calculateAabbBruteForce(bvhAabbMin, bvhAabbMax); + + if (buildBvh) { + bvh = new OptimizedBvh(); + bvh.build(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax); + ownsBvh = true; + } + + // JAVA NOTE: moved from TriangleMeshShape + recalcLocalAabb(); + //#endif //DISABLE_BVH + } + + /** + * Optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb. + */ + public BvhTriangleMeshShape(StridingMeshInterface meshInterface, boolean useQuantizedAabbCompression, Vector3f bvhAabbMin, Vector3f bvhAabbMax) { + this(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax, true); + } + + /** + * Optionally pass in a larger bvh aabb, used for quantization. This allows for deformations within this aabb. + */ + public BvhTriangleMeshShape(StridingMeshInterface meshInterface, boolean useQuantizedAabbCompression, Vector3f bvhAabbMin, Vector3f bvhAabbMax, boolean buildBvh) { + super(meshInterface); + + this.bvh = null; + this.useQuantizedAabbCompression = useQuantizedAabbCompression; + this.ownsBvh = false; + + // construct bvh from meshInterface + //#ifndef DISABLE_BVH + + if (buildBvh) { + bvh = new OptimizedBvh(); + + bvh.build(meshInterface, useQuantizedAabbCompression, bvhAabbMin, bvhAabbMax); + ownsBvh = true; + } + + // JAVA NOTE: moved from TriangleMeshShape + recalcLocalAabb(); + //#endif //DISABLE_BVH + } + + @Override + public BroadphaseNativeType getShapeType() { + return BroadphaseNativeType.TRIANGLE_MESH_SHAPE_PROXYTYPE; + } + + public void performRaycast(TriangleRaycastCallback callback, Vector3f raySource, Vector3f rayTarget) { + MyNodeOverlapCallback myNodeCallback = myNodeCallbacks.get(); + myNodeCallback.init(callback, meshInterface); + + bvh.reportRayOverlappingNodex(myNodeCallback, raySource, rayTarget); + + myNodeCallbacks.release(myNodeCallback); + } + + public void performConvexcast(TriangleConvexcastCallback callback, Vector3f raySource, Vector3f rayTarget, Vector3f aabbMin, Vector3f aabbMax) { + MyNodeOverlapCallback myNodeCallback = myNodeCallbacks.get(); + myNodeCallback.init(callback, meshInterface); + + bvh.reportBoxCastOverlappingNodex(myNodeCallback, raySource, rayTarget, aabbMin, aabbMax); + + myNodeCallbacks.release(myNodeCallback); + } + + /** + * Perform bvh tree traversal and report overlapping triangles to 'callback'. + */ + @Override + public void processAllTriangles(TriangleCallback callback, Vector3f aabbMin, Vector3f aabbMax) { + //#ifdef DISABLE_BVH + // // brute force traverse all triangles + //btTriangleMeshShape::processAllTriangles(callback,aabbMin,aabbMax); + //#else + + // first get all the nodes + MyNodeOverlapCallback myNodeCallback = myNodeCallbacks.get(); + myNodeCallback.init(callback, meshInterface); + + bvh.reportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax); + + myNodeCallbacks.release(myNodeCallback); + //#endif//DISABLE_BVH + } + + public void refitTree() { + bvh.refit(meshInterface); + + recalcLocalAabb(); + } + + /** + * For a fast incremental refit of parts of the tree. Note: the entire AABB of the tree will become more conservative, it never shrinks. + */ + public void partialRefitTree(Vector3f aabbMin, Vector3f aabbMax) { + bvh.refitPartial(meshInterface,aabbMin,aabbMax ); + + VectorUtil.setMin(localAabbMin, aabbMin); + VectorUtil.setMax(localAabbMax, aabbMax); + } + + @Override + public String getName() { + return "BVHTRIANGLEMESH"; + } + + @Override + public void setLocalScaling(Vector3f scaling) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + tmp.sub(getLocalScaling(), scaling); + + if (tmp.lengthSquared() > BulletGlobals.SIMD_EPSILON) { + super.setLocalScaling(scaling); + /* + if (ownsBvh) + { + m_bvh->~btOptimizedBvh(); + btAlignedFree(m_bvh); + } + */ + ///m_localAabbMin/m_localAabbMax is already re-calculated in btTriangleMeshShape. We could just scale aabb, but this needs some more work + bvh = new OptimizedBvh(); + // rebuild the bvh... + bvh.build(meshInterface, useQuantizedAabbCompression, localAabbMin, localAabbMax); + + } + } + finally { + stack.vectors.pop(); + } + } + + public OptimizedBvh getOptimizedBvh() { + return bvh; + } + + public void setOptimizedBvh(OptimizedBvh bvh) { + assert (this.bvh == null); + assert (!ownsBvh); + + this.bvh = bvh; + ownsBvh = false; + } + + public boolean usesQuantizedAabbCompression() { + return useQuantizedAabbCompression; + } + + //////////////////////////////////////////////////////////////////////////// + + protected static class MyNodeOverlapCallback implements NodeOverlapCallback { + public StridingMeshInterface meshInterface; + public TriangleCallback callback; + + private Vector3f[] triangle/*[3]*/ = new Vector3f[] { new Vector3f(), new Vector3f(), new Vector3f() }; + private VertexData data = new VertexData(); + + public MyNodeOverlapCallback() { + } + + public void init(TriangleCallback callback, StridingMeshInterface meshInterface) { + this.meshInterface = meshInterface; + this.callback = callback; + } + + public void processNode(int nodeSubPart, int nodeTriangleIndex) { + meshInterface.getLockedReadOnlyVertexIndexBase(data, nodeSubPart); + + //int* gfxbase = (int*)(indexbase+nodeTriangleIndex*indexstride); + ByteBuffer gfxbase_ptr = data.indexbase; + int gfxbase_index = (nodeTriangleIndex * data.indexstride); + assert (data.indicestype == ScalarType.PHY_INTEGER || data.indicestype == ScalarType.PHY_SHORT); + + Vector3f meshScaling = meshInterface.getScaling(); + for (int j = 2; j >= 0; j--) { + int graphicsindex; + if (data.indicestype == ScalarType.PHY_SHORT) { + graphicsindex = gfxbase_ptr.getShort(gfxbase_index + j * 2) & 0xFFFF; + } + else { + graphicsindex = gfxbase_ptr.getInt(gfxbase_index + j * 4); + } + + //float* graphicsbase = (float*)(vertexbase+graphicsindex*stride); + ByteBuffer graphicsbase_ptr = data.vertexbase; + int graphicsbase_index = graphicsindex * data.stride; + + triangle[j].set( + graphicsbase_ptr.getFloat(graphicsbase_index + 4 * 0) * meshScaling.x, + graphicsbase_ptr.getFloat(graphicsbase_index + 4 * 1) * meshScaling.y, + graphicsbase_ptr.getFloat(graphicsbase_index + 4 * 2) * meshScaling.z); + } + + /* Perform ray vs. triangle collision here */ + callback.processTriangle(triangle, nodeSubPart, nodeTriangleIndex); + meshInterface.unLockReadOnlyVertexBase(nodeSubPart); + + data.unref(); + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/CapsuleShape.java b/src/jbullet/src/javabullet/collision/shapes/CapsuleShape.java new file mode 100644 index 0000000..674ac5d --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/CapsuleShape.java @@ -0,0 +1,160 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.BulletGlobals; +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * CapsuleShape represents a capsule around the Y axis. + * A more general solution that can represent capsules is the MultiSphereShape. + * + * @author jezek2 + */ +public class CapsuleShape extends ConvexInternalShape { + + public CapsuleShape(float radius, float height) { + implicitShapeDimensions.set(radius, 0.5f * height, radius); + } + + @Override + public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec0) { + stack.vectors.push(); + try { + Vector3f supVec = stack.vectors.get(0f, 0f, 0f); + + float maxDot = -1e30f; + + Vector3f vec = stack.vectors.get(vec0); + float lenSqr = vec.lengthSquared(); + if (lenSqr < 0.0001f) { + vec.set(1f, 0f, 0f); + } + else { + float rlen = 1f / (float) Math.sqrt(lenSqr); + vec.scale(rlen); + } + + Vector3f vtx = stack.vectors.get(); + float newDot; + + float radius = getRadius(); + + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + { + Vector3f pos = stack.vectors.get(0f, getHalfHeight(), 0f); + VectorUtil.mul(tmp1, vec, localScaling); + tmp1.scale(radius); + tmp2.scale(getMargin(), vec); + vtx.add(pos, tmp1); + vtx.sub(tmp2); + newDot = vec.dot(vtx); + if (newDot > maxDot) { + maxDot = newDot; + supVec.set(vtx); + } + } + { + Vector3f pos = stack.vectors.get(0f, -getHalfHeight(), 0f); + VectorUtil.mul(tmp1, vec, localScaling); + tmp1.scale(radius); + tmp2.scale(getMargin(), vec); + vtx.add(pos, tmp1); + vtx.sub(tmp2); + newDot = vec.dot(vtx); + if (newDot > maxDot) { + maxDot = newDot; + supVec.set(vtx); + } + } + + return stack.vectors.returning(supVec); + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) { + // TODO: implement + throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public void calculateLocalInertia(float mass, Vector3f inertia) { + stack.pushCommonMath(); + try { + // as an approximation, take the inertia of the box that bounds the spheres + + Transform ident = stack.transforms.get(); + ident.setIdentity(); + + float radius = getRadius(); + + Vector3f halfExtents = stack.vectors.get(radius, radius + getHalfHeight(), radius); + + float margin = BulletGlobals.CONVEX_DISTANCE_MARGIN; + + float lx = 2f * (halfExtents.x + margin); + float ly = 2f * (halfExtents.y + margin); + float lz = 2f * (halfExtents.z + margin); + float x2 = lx * lx; + float y2 = ly * ly; + float z2 = lz * lz; + float scaledmass = mass * 0.08333333f; + + inertia.x = scaledmass * (y2 + z2); + inertia.y = scaledmass * (x2 + z2); + inertia.z = scaledmass * (x2 + y2); + } + finally { + stack.popCommonMath(); + } + } + + @Override + public BroadphaseNativeType getShapeType() { + return BroadphaseNativeType.CAPSULE_SHAPE_PROXYTYPE; + } + + @Override + public String getName() { + return "CapsuleShape"; + } + + public float getRadius() { + return implicitShapeDimensions.x; + } + + public float getHalfHeight() { + return implicitShapeDimensions.y; + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/CollisionShape.java b/src/jbullet/src/javabullet/collision/shapes/CollisionShape.java new file mode 100644 index 0000000..87145f4 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/CollisionShape.java @@ -0,0 +1,171 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.BulletStack; +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * CollisionShape provides interface for collision shapes that can be shared among CollisionObjects. + * + * @author jezek2 + */ +public abstract class CollisionShape { + + protected final BulletStack stack = BulletStack.get(); + + ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. + public abstract void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax); + + public void getBoundingSphere(Vector3f center, float[] radius) { + stack.pushCommonMath(); + try { + Vector3f tmp = stack.vectors.get(); + + Transform tr = stack.transforms.get(); + tr.setIdentity(); + Vector3f aabbMin = stack.vectors.get(), aabbMax = stack.vectors.get(); + + getAabb(tr, aabbMin, aabbMax); + + tmp.sub(aabbMax, aabbMin); + radius[0] = tmp.length() * 0.5f; + + tmp.add(aabbMin, aabbMax); + center.scale(0.5f, tmp); + } + finally { + stack.popCommonMath(); + } + } + + ///getAngularMotionDisc returns the maximus radius needed for Conservative Advancement to handle time-of-impact with rotations. + public float getAngularMotionDisc() { + stack.vectors.push(); + try { + Vector3f center = stack.vectors.get(); + float[] disc = new float[1]; // TODO: stack + getBoundingSphere(center, disc); + disc[0] += center.length(); + return disc[0]; + } + finally { + stack.vectors.pop(); + } + } + + ///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) + ///result is conservative + public void calculateTemporalAabb(Transform curTrans, Vector3f linvel, Vector3f angvel, float timeStep, Vector3f temporalAabbMin, Vector3f temporalAabbMax) { + stack.vectors.push(); + try { + //start with static aabb + getAabb(curTrans, temporalAabbMin, temporalAabbMax); + + float temporalAabbMaxx = temporalAabbMax.x; + float temporalAabbMaxy = temporalAabbMax.y; + float temporalAabbMaxz = temporalAabbMax.z; + float temporalAabbMinx = temporalAabbMin.x; + float temporalAabbMiny = temporalAabbMin.y; + float temporalAabbMinz = temporalAabbMin.z; + + // add linear motion + Vector3f linMotion = stack.vectors.get(linvel); + linMotion.scale(timeStep); + + //todo: simd would have a vector max/min operation, instead of per-element access + if (linMotion.x > 0f) { + temporalAabbMaxx += linMotion.x; + } + else { + temporalAabbMinx += linMotion.x; + } + if (linMotion.y > 0f) { + temporalAabbMaxy += linMotion.y; + } + else { + temporalAabbMiny += linMotion.y; + } + if (linMotion.z > 0f) { + temporalAabbMaxz += linMotion.z; + } + else { + temporalAabbMinz += linMotion.z; + } + + //add conservative angular motion + float angularMotion = angvel.length() * getAngularMotionDisc() * timeStep; + Vector3f angularMotion3d = stack.vectors.get(angularMotion, angularMotion, angularMotion); + temporalAabbMin.set(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz); + temporalAabbMax.set(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz); + + temporalAabbMin.sub(angularMotion3d); + temporalAabbMax.add(angularMotion3d); + } + finally { + stack.vectors.pop(); + } + } + +//#ifndef __SPU__ + public boolean isPolyhedral() { + return getShapeType().isPolyhedral(); + } + + public boolean isConvex() { + return getShapeType().isConvex(); + } + + public boolean isConcave() { + return getShapeType().isConcave(); + } + + public boolean isCompound() { + return getShapeType().isCompound(); + } + + ///isInfinite is used to catch simulation error (aabb check) + public boolean isInfinite() { + return getShapeType().isInfinite(); + } + + public abstract BroadphaseNativeType getShapeType(); + + public abstract void setLocalScaling(Vector3f scaling); + + // TODO: returns const + public abstract Vector3f getLocalScaling(); + + public abstract void calculateLocalInertia(float mass, Vector3f inertia); + + +//debugging support + public abstract String getName(); +//#endif //__SPU__ + public abstract void setMargin(float margin); + + public abstract float getMargin(); +} diff --git a/src/jbullet/src/javabullet/collision/shapes/CompoundShape.java b/src/jbullet/src/javabullet/collision/shapes/CompoundShape.java new file mode 100644 index 0000000..8531fdf --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/CompoundShape.java @@ -0,0 +1,212 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import java.util.ArrayList; +import java.util.List; +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.linearmath.MatrixUtil; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Matrix3f; +import javax.vecmath.Vector3f; + +/** + * CompoundShape allows to store multiple other CollisionShapes. + * This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape. + * + * @author jezek2 + */ +public class CompoundShape extends CollisionShape { + + private final List<CompoundShapeChild> children = new ArrayList<CompoundShapeChild>(); + private final Vector3f localAabbMin = new Vector3f(1e30f, 1e30f, 1e30f); + private final Vector3f localAabbMax = new Vector3f(-1e30f, -1e30f, -1e30f); + + private OptimizedBvh aabbTree = null; + + private float collisionMargin = 0f; + protected final Vector3f localScaling = new Vector3f(1f, 1f, 1f); + + public void addChildShape(Transform localTransform, CollisionShape shape) { + stack.vectors.push(); + try { + //m_childTransforms.push_back(localTransform); + //m_childShapes.push_back(shape); + CompoundShapeChild child = new CompoundShapeChild(); + child.transform.set(localTransform); + child.childShape = shape; + child.childShapeType = shape.getShapeType(); + child.childMargin = shape.getMargin(); + + children.add(child); + + // extend the local aabbMin/aabbMax + Vector3f _localAabbMin = stack.vectors.get(), _localAabbMax = stack.vectors.get(); + shape.getAabb(localTransform, _localAabbMin, _localAabbMax); + + // JAVA NOTE: rewritten + // for (int i=0;i<3;i++) + // { + // if (this.localAabbMin[i] > _localAabbMin[i]) + // { + // this.localAabbMin[i] = _localAabbMin[i]; + // } + // if (this.localAabbMax[i] < _localAabbMax[i]) + // { + // this.localAabbMax[i] = _localAabbMax[i]; + // } + // } + VectorUtil.setMin(this.localAabbMin, _localAabbMin); + VectorUtil.setMax(this.localAabbMax, _localAabbMax); + } + finally { + stack.vectors.pop(); + } + } + + public int getNumChildShapes() { + return children.size(); + } + + public CollisionShape getChildShape(int index) { + return children.get(index).childShape; + } + + public Transform getChildTransform(int index) { + return children.get(index).transform; + } + + public List<CompoundShapeChild> getChildList() { + return children; + } + + /** + * getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version. + */ + @Override + public void getAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax) { + stack.pushCommonMath(); + try { + Vector3f localHalfExtents = stack.vectors.get(); + localHalfExtents.sub(localAabbMax, localAabbMin); + localHalfExtents.scale(0.5f); + + Vector3f localCenter = stack.vectors.get(); + localCenter.add(localAabbMax, localAabbMin); + localCenter.scale(0.5f); + + Matrix3f abs_b = stack.matrices.get(trans.basis); + MatrixUtil.absolute(abs_b); + + Vector3f center = stack.vectors.get(localCenter); + trans.transform(center); + + Vector3f tmp = stack.vectors.get(); + + Vector3f extent = stack.vectors.get(); + abs_b.getRow(0, tmp); + extent.x = tmp.dot(localHalfExtents); + abs_b.getRow(1, tmp); + extent.y = tmp.dot(localHalfExtents); + abs_b.getRow(2, tmp); + extent.z = tmp.dot(localHalfExtents); + + extent.x += getMargin(); + extent.y += getMargin(); + extent.z += getMargin(); + + aabbMin.sub(center, extent); + aabbMax.add(center, extent); + } + finally { + stack.popCommonMath(); + } + } + + @Override + public void setLocalScaling(Vector3f scaling) { + localScaling.set(scaling); + } + + @Override + public Vector3f getLocalScaling() { + return localScaling; + } + + @Override + public void calculateLocalInertia(float mass, Vector3f inertia) { + stack.pushCommonMath(); + try { + // approximation: take the inertia from the aabb for now + Transform ident = stack.transforms.get(); + ident.setIdentity(); + Vector3f aabbMin = stack.vectors.get(), aabbMax = stack.vectors.get(); + getAabb(ident, aabbMin, aabbMax); + + Vector3f halfExtents = stack.vectors.get(); + halfExtents.sub(aabbMax, aabbMin); + halfExtents.scale(0.5f); + + float lx = 2f * halfExtents.x; + float ly = 2f * halfExtents.y; + float lz = 2f * halfExtents.z; + + inertia.x = (mass / 12f) * (ly * ly + lz * lz); + inertia.y = (mass / 12f) * (lx * lx + lz * lz); + inertia.z = (mass / 12f) * (lx * lx + ly * ly); + } + finally { + stack.popCommonMath(); + } + } + + @Override + public BroadphaseNativeType getShapeType() { + return BroadphaseNativeType.COMPOUND_SHAPE_PROXYTYPE; + } + + @Override + public void setMargin(float margin) { + collisionMargin = margin; + } + + @Override + public float getMargin() { + return collisionMargin; + } + + @Override + public String getName() { + return "Compound"; + } + + // this is optional, but should make collision queries faster, by culling non-overlapping nodes + // void createAabbTreeFromChildren(); + + public OptimizedBvh getAabbTree() { + return aabbTree; + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/CompoundShapeChild.java b/src/jbullet/src/javabullet/collision/shapes/CompoundShapeChild.java new file mode 100644 index 0000000..0f320b0 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/CompoundShapeChild.java @@ -0,0 +1,40 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.linearmath.Transform; + +/** + * + * @author jezek2 + */ +public class CompoundShapeChild { + + public final Transform transform = new Transform(); + public CollisionShape childShape; + public BroadphaseNativeType childShapeType; + public float childMargin; + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/ConcaveShape.java b/src/jbullet/src/javabullet/collision/shapes/ConcaveShape.java new file mode 100644 index 0000000..846b78d --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/ConcaveShape.java @@ -0,0 +1,48 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javax.vecmath.Vector3f; + +/** + * Concave shape proves an interface concave shapes that can produce triangles that overlapping a given AABB. + * Static triangle mesh, infinite plane, height field/landscapes are example that implement this interface. + * + * @author jezek2 + */ +public abstract class ConcaveShape extends CollisionShape { + + protected float collisionMargin = 0f; + + public abstract void processAllTriangles(TriangleCallback callback, Vector3f aabbMin, Vector3f aabbMax); + + public float getMargin() { + return collisionMargin; + } + + public void setMargin(float margin) { + this.collisionMargin = margin; + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/ConvexHullShape.java b/src/jbullet/src/javabullet/collision/shapes/ConvexHullShape.java new file mode 100644 index 0000000..8636802 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/ConvexHullShape.java @@ -0,0 +1,223 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import java.util.ArrayList; +import java.util.List; +import javabullet.BulletGlobals; +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices). + * No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices. + * On modern hardware, due to cache coherency this isn't that bad. Complex algorithms tend to trash the cashe + * (memory is much slower then the cpu). + * + * @author jezek2 + */ +public class ConvexHullShape extends PolyhedralConvexShape { + + private final List<Vector3f> points = new ArrayList<Vector3f>(); + + /** + * TODO: This constructor optionally takes in a pointer to points. Each point is assumed to be 3 consecutive float (x,y,z), the striding defines the number of bytes between each point, in memory. + * It is easier to not pass any points in the constructor, and just add one point at a time, using addPoint. + * ConvexHullShape make an internal copy of the points. + */ + // TODO: make better constuctors (ByteBuffer, etc.) + public ConvexHullShape(List<Vector3f> points) { + // JAVA NOTE: rewritten + + for (int i=0; i<points.size(); i++) { + this.points.add(new Vector3f(points.get(i))); + } + + recalcLocalAabb(); + } + + public void addPoint(Vector3f point) { + points.add(new Vector3f(point)); + recalcLocalAabb(); + } + + public List<Vector3f> getPoints() { + return points; + } + + public int getNumPoints() { + return points.size(); + } + + @Override + public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec0) { + stack.vectors.push(); + try { + Vector3f supVec = stack.vectors.get(0f, 0f, 0f); + float newDot, maxDot = -1e30f; + + Vector3f vec = stack.vectors.get(vec0); + float lenSqr = vec.lengthSquared(); + if (lenSqr < 0.0001f) { + vec.set(1f, 0f, 0f); + } + else { + float rlen = 1f / (float) Math.sqrt(lenSqr); + vec.scale(rlen); + } + + + Vector3f vtx = stack.vectors.get(); + for (int i = 0; i < points.size(); i++) { + VectorUtil.mul(vtx, points.get(i), localScaling); + + newDot = vec.dot(vtx); + if (newDot > maxDot) { + maxDot = newDot; + supVec.set(vtx); + } + } + return stack.vectors.returning(supVec); + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) { + stack.vectors.push(); + try { + float newDot; + + // JAVA NOTE: rewritten as code used W coord for temporary usage in Vector3 + // TODO: optimize it + float[] wcoords = new float[numVectors]; + + // use 'w' component of supportVerticesOut? + { + for (int i = 0; i < numVectors; i++) { + //supportVerticesOut[i][3] = btScalar(-1e30); + wcoords[i] = -1e30f; + } + } + Vector3f vtx = stack.vectors.get(); + for (int i = 0; i < points.size(); i++) { + VectorUtil.mul(vtx, points.get(i), localScaling); + + for (int j = 0; j < numVectors; j++) { + Vector3f vec = vectors[j]; + + newDot = vec.dot(vtx); + //if (newDot > supportVerticesOut[j][3]) + if (newDot > wcoords[j]) { + // WARNING: don't swap next lines, the w component would get overwritten! + supportVerticesOut[j].set(vtx); + //supportVerticesOut[j][3] = newDot; + wcoords[j] = newDot; + } + } + } + } + finally { + stack.vectors.pop(); + } + } + + @Override + public Vector3f localGetSupportingVertex(Vector3f vec) { + stack.vectors.push(); + try { + Vector3f supVertex = stack.vectors.get(localGetSupportingVertexWithoutMargin(vec)); + + if (getMargin() != 0f) { + Vector3f vecnorm = stack.vectors.get(vec); + if (vecnorm.lengthSquared() < (BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) { + vecnorm.set(-1f, -1f, -1f); + } + vecnorm.normalize(); + supVertex.scaleAdd(getMargin(), vecnorm, supVertex); + } + return stack.vectors.returning(supVertex); + } + finally { + stack.vectors.pop(); + } + } + + /** + * Currently just for debugging (drawing), perhaps future support for algebraic continuous collision detection. + * Please note that you can debug-draw ConvexHullShape with the Raytracer Demo. + */ + @Override + public int getNumVertices() { + return points.size(); + } + + @Override + public int getNumEdges() { + return points.size(); + } + + @Override + public void getEdge(int i, Vector3f pa, Vector3f pb) { + int index0 = i % points.size(); + int index1 = (i + 1) % points.size(); + VectorUtil.mul(pa, points.get(index0), localScaling); + VectorUtil.mul(pb, points.get(index1), localScaling); + } + + @Override + public void getVertex(int i, Vector3f vtx) { + VectorUtil.mul(vtx, points.get(i), localScaling); + } + + @Override + public int getNumPlanes() { + return 0; + } + + @Override + public void getPlane(Vector3f planeNormal, Vector3f planeSupport, int i) { + assert false; + } + + @Override + public boolean isInside(Vector3f pt, float tolerance) { + assert false; + return false; + } + + @Override + public BroadphaseNativeType getShapeType() { + return BroadphaseNativeType.CONVEX_HULL_SHAPE_PROXYTYPE; + } + + @Override + public String getName() { + return "Convex"; + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/ConvexInternalShape.java b/src/jbullet/src/javabullet/collision/shapes/ConvexInternalShape.java new file mode 100644 index 0000000..0f7836c --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/ConvexInternalShape.java @@ -0,0 +1,131 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.BulletGlobals; +import javabullet.linearmath.MatrixUtil; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public abstract class ConvexInternalShape extends ConvexShape { + + //local scaling. collisionMargin is not scaled ! + protected final Vector3f localScaling = new Vector3f(1f, 1f, 1f); + protected final Vector3f implicitShapeDimensions = new Vector3f(); + protected float collisionMargin = BulletGlobals.CONVEX_DISTANCE_MARGIN; + + /** + * getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version. + */ + @Override + public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) { + getAabbSlow(t, aabbMin, aabbMax); + } + + @Override + public void getAabbSlow(Transform trans, Vector3f minAabb, Vector3f maxAabb) { + stack.vectors.push(); + try { + float margin = getMargin(); + for (int i=0;i<3;i++) + { + Vector3f vec = stack.vectors.get(0f, 0f, 0f); + VectorUtil.setCoord(vec, i, 1f); + + Vector3f tmp1 = stack.vectors.get(); + MatrixUtil.transposeTransform(tmp1, vec, trans.basis); + Vector3f sv = stack.vectors.get(localGetSupportingVertex(tmp1)); + + Vector3f tmp2 = stack.vectors.get(sv); + trans.transform(tmp2); + + VectorUtil.setCoord(maxAabb, i, VectorUtil.getCoord(tmp2, i) + margin); + + VectorUtil.setCoord(vec, i, -1f); + + MatrixUtil.transposeTransform(tmp1, vec, trans.basis); + tmp2.set(localGetSupportingVertex(tmp1)); + trans.transform(tmp2); + + VectorUtil.setCoord(minAabb, i, VectorUtil.getCoord(tmp2, i) - margin); + } + } + finally { + stack.vectors.pop(); + } + } + + @Override + public Vector3f localGetSupportingVertex(Vector3f vec) { + stack.vectors.push(); + try { + Vector3f supVertex = stack.vectors.get(localGetSupportingVertexWithoutMargin(vec)); + + if (getMargin() != 0f) { + Vector3f vecnorm = stack.vectors.get(vec); + if (vecnorm.lengthSquared() < (BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON)) { + vecnorm.set(-1f, -1f, -1f); + } + vecnorm.normalize(); + supVertex.scaleAdd(getMargin(), vecnorm, supVertex); + } + return stack.vectors.returning(supVertex); + } + finally { + stack.vectors.pop(); + } + } + + public void setLocalScaling(Vector3f scaling) { + this.localScaling.set(scaling); + } + + public Vector3f getLocalScaling() { + return localScaling; + } + + public float getMargin() { + return collisionMargin; + } + + public void setMargin(float margin) { + this.collisionMargin = margin; + } + + @Override + public int getNumPreferredPenetrationDirections() { + return 0; + } + + @Override + public void getPreferredPenetrationDirection(int index, Vector3f penetrationVector) { + throw new InternalError(); + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/ConvexShape.java b/src/jbullet/src/javabullet/collision/shapes/ConvexShape.java new file mode 100644 index 0000000..f1352ea --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/ConvexShape.java @@ -0,0 +1,61 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * ConvexShape is an abstract shape interface. + * It describes general convex shapes using the localGetSupportingVertex interface + * used in combination with GJK or btConvexCast + * + * @author jezek2 + */ +public abstract class ConvexShape extends CollisionShape { + + public abstract Vector3f localGetSupportingVertex(Vector3f vec); + + //#ifndef __SPU__ + public abstract Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec); + + //notice that the vectors should be unit length + public abstract void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors); + //#endif + + public abstract void getAabbSlow(Transform t, Vector3f aabbMin, Vector3f aabbMax); + + public abstract void setLocalScaling(Vector3f scaling); + + public abstract Vector3f getLocalScaling(); + + public abstract void setMargin(float margin); + + public abstract float getMargin(); + + public abstract int getNumPreferredPenetrationDirections(); + + public abstract void getPreferredPenetrationDirection(int index, Vector3f penetrationVector); + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/CylinderShape.java b/src/jbullet/src/javabullet/collision/shapes/CylinderShape.java new file mode 100644 index 0000000..f74dcdd --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/CylinderShape.java @@ -0,0 +1,152 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.BulletGlobals; +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * Implements cylinder shape interface. + * + * @author jezek2 + */ +public class CylinderShape extends BoxShape { + + protected int upAxis; + + public CylinderShape(Vector3f halfExtents) { + super(halfExtents); + upAxis = 1; + recalcLocalAabb(); + } + + protected CylinderShape(Vector3f halfExtents, boolean unused) { + super(halfExtents); + } + + @Override + public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) { + _PolyhedralConvexShape_getAabb(t, aabbMin, aabbMax); + } + + protected Vector3f cylinderLocalSupportX(Vector3f halfExtents, Vector3f v) { + return cylinderLocalSupport(halfExtents, v, 0, 1, 0, 2); + } + + protected Vector3f cylinderLocalSupportY(Vector3f halfExtents, Vector3f v) { + return cylinderLocalSupport(halfExtents, v, 1, 0, 1, 2); + } + + protected Vector3f cylinderLocalSupportZ(Vector3f halfExtents, Vector3f v) { + return cylinderLocalSupport(halfExtents, v, 2, 0, 2, 1); + } + + private Vector3f cylinderLocalSupport(Vector3f halfExtents, Vector3f v, int cylinderUpAxis, int XX, int YY, int ZZ) { + stack.vectors.push(); + try { + //mapping depends on how cylinder local orientation is + // extents of the cylinder is: X,Y is for radius, and Z for height + + float radius = VectorUtil.getCoord(halfExtents, XX); + float halfHeight = VectorUtil.getCoord(halfExtents, cylinderUpAxis); + + Vector3f tmp = stack.vectors.get(); + float d; + + float s = (float) Math.sqrt(VectorUtil.getCoord(v, XX) * VectorUtil.getCoord(v, XX) + VectorUtil.getCoord(v, ZZ) * VectorUtil.getCoord(v, ZZ)); + if (s != 0f) { + d = radius / s; + VectorUtil.setCoord(tmp, XX, VectorUtil.getCoord(v, XX) * d); + VectorUtil.setCoord(tmp, YY, VectorUtil.getCoord(v, YY) < 0f ? -halfHeight : halfHeight); + VectorUtil.setCoord(tmp, ZZ, VectorUtil.getCoord(v, ZZ) * d); + return stack.vectors.returning(tmp); + } + else { + VectorUtil.setCoord(tmp, XX, radius); + VectorUtil.setCoord(tmp, YY, VectorUtil.getCoord(v, YY) < 0f ? -halfHeight : halfHeight); + VectorUtil.setCoord(tmp, ZZ, 0f); + return stack.vectors.returning(tmp); + } + } + finally { + stack.vectors.pop(); + } + } + + @Override + public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) { + return cylinderLocalSupportY(getHalfExtentsWithoutMargin(), vec); + } + + @Override + public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) { + for (int i = 0; i < numVectors; i++) { + supportVerticesOut[i].set(cylinderLocalSupportY(getHalfExtentsWithoutMargin(), vectors[i])); + } + } + + @Override + public Vector3f localGetSupportingVertex(Vector3f vec) { + stack.vectors.push(); + try { + Vector3f supVertex = stack.vectors.get(); + supVertex.set(localGetSupportingVertexWithoutMargin(vec)); + + if (getMargin() != 0f) { + Vector3f vecnorm = stack.vectors.get(vec); + if (vecnorm.lengthSquared() < (BulletGlobals.SIMD_EPSILON * BulletGlobals.SIMD_EPSILON)) { + vecnorm.set(-1f, -1f, -1f); + } + vecnorm.normalize(); + supVertex.scaleAdd(getMargin(), vecnorm, supVertex); + } + return stack.vectors.returning(supVertex); + } + finally { + stack.vectors.pop(); + } + } + + @Override + public BroadphaseNativeType getShapeType() { + return BroadphaseNativeType.CYLINDER_SHAPE_PROXYTYPE; + } + + public int getUpAxis() { + return upAxis; + } + + public float getRadius() { + return getHalfExtentsWithMargin().x; + } + + @Override + public String getName() { + return "CylinderY"; + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/CylinderShapeX.java b/src/jbullet/src/javabullet/collision/shapes/CylinderShapeX.java new file mode 100644 index 0000000..1126cf1 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/CylinderShapeX.java @@ -0,0 +1,62 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class CylinderShapeX extends CylinderShape { + + public CylinderShapeX(Vector3f halfExtents) { + super(halfExtents, false); + upAxis = 0; + recalcLocalAabb(); + } + + @Override + public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) { + return cylinderLocalSupportX(getHalfExtentsWithoutMargin(), vec); + } + + @Override + public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) { + for (int i = 0; i < numVectors; i++) { + supportVerticesOut[i].set(cylinderLocalSupportX(getHalfExtentsWithoutMargin(), vectors[i])); + } + } + + @Override + public float getRadius() { + return getHalfExtentsWithMargin().y; + } + + @Override + public String getName() { + return "CylinderX"; + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/CylinderShapeZ.java b/src/jbullet/src/javabullet/collision/shapes/CylinderShapeZ.java new file mode 100644 index 0000000..a94812c --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/CylinderShapeZ.java @@ -0,0 +1,62 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class CylinderShapeZ extends CylinderShape { + + public CylinderShapeZ(Vector3f halfExtents) { + super(halfExtents, false); + upAxis = 2; + recalcLocalAabb(); + } + + @Override + public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) { + return cylinderLocalSupportZ(getHalfExtentsWithoutMargin(), vec); + } + + @Override + public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) { + for (int i = 0; i < numVectors; i++) { + supportVerticesOut[i].set(cylinderLocalSupportZ(getHalfExtentsWithoutMargin(), vectors[i])); + } + } + + @Override + public float getRadius() { + return getHalfExtentsWithMargin().x; + } + + @Override + public String getName() { + return "CylinderZ"; + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/IndexedMesh.java b/src/jbullet/src/javabullet/collision/shapes/IndexedMesh.java new file mode 100644 index 0000000..7307f66 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/IndexedMesh.java @@ -0,0 +1,44 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import java.nio.ByteBuffer; + +/** + * + * @author jezek2 + */ +public class IndexedMesh { + + public int numTriangles; + public ByteBuffer triangleIndexBase; + public int triangleIndexStride; + public int numVertices; + public ByteBuffer vertexBase; + public int vertexStride; + // The index type is set when adding an indexed mesh to the + // TriangleIndexVertexArray, do not set it manually + public ScalarType indexType; + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/InternalTriangleIndexCallback.java b/src/jbullet/src/javabullet/collision/shapes/InternalTriangleIndexCallback.java new file mode 100644 index 0000000..418f3eb --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/InternalTriangleIndexCallback.java @@ -0,0 +1,36 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public interface InternalTriangleIndexCallback { + + public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex); + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/MinkowskiSumShape.java b/src/jbullet/src/javabullet/collision/shapes/MinkowskiSumShape.java new file mode 100644 index 0000000..cb4fdf2 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/MinkowskiSumShape.java @@ -0,0 +1,128 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.linearmath.MatrixUtil; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes. + * + * @author jezek2 + */ +public class MinkowskiSumShape extends ConvexInternalShape { + + private final Transform transA = new Transform(); + private final Transform transB = new Transform(); + private ConvexShape shapeA; + private ConvexShape shapeB; + + public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB) { + this.shapeA = shapeA; + this.shapeB = shapeB; + this.transA.setIdentity(); + this.transB.setIdentity(); + } + + @Override + public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + Vector3f supVertexA = stack.vectors.get(); + Vector3f supVertexB = stack.vectors.get(); + + // btVector3 supVertexA = m_transA(m_shapeA->localGetSupportingVertexWithoutMargin(vec*m_transA.getBasis())); + MatrixUtil.transposeTransform(tmp, vec, transA.basis); + supVertexA.set(shapeA.localGetSupportingVertexWithoutMargin(tmp)); + transA.transform(supVertexA); + + // btVector3 supVertexB = m_transB(m_shapeB->localGetSupportingVertexWithoutMargin(vec*m_transB.getBasis())); + MatrixUtil.transposeTransform(tmp, vec, transB.basis); + supVertexB.set(shapeB.localGetSupportingVertexWithoutMargin(tmp)); + transB.transform(supVertexB); + + //return supVertexA + supVertexB; + Vector3f result = stack.vectors.get(); + result.add(supVertexA, supVertexB); + return stack.vectors.returning(result); + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) { + //todo: could make recursive use of batching. probably this shape is not used frequently. + for (int i = 0; i < numVectors; i++) { + supportVerticesOut[i].set(localGetSupportingVertexWithoutMargin(vectors[i])); + } + } + + @Override + public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) { + throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public BroadphaseNativeType getShapeType() { + return BroadphaseNativeType.MINKOWSKI_SUM_SHAPE_PROXYTYPE; + } + + @Override + public void calculateLocalInertia(float mass, Vector3f inertia) { + assert (false); + inertia.set(0, 0, 0); + } + + @Override + public String getName() { + return "MinkowskiSum"; + } + + @Override + public float getMargin() { + return shapeA.getMargin() + shapeB.getMargin(); + } + + public void setTransformA(Transform transA) { + this.transA.set(transA); + } + + public void setTransformB(Transform transB) { + this.transB.set(transB); + } + + public void getTransformA(Transform dest) { + dest.set(transA); + } + + public void getTransformB(Transform dest) { + dest.set(transB); + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/NodeOverlapCallback.java b/src/jbullet/src/javabullet/collision/shapes/NodeOverlapCallback.java new file mode 100644 index 0000000..216e0d7 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/NodeOverlapCallback.java @@ -0,0 +1,34 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +/** + * + * @author jezek2 + */ +public interface NodeOverlapCallback { + + public void processNode(int subPart, int triangleIndex); + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/OptimizedBvh.java b/src/jbullet/src/javabullet/collision/shapes/OptimizedBvh.java new file mode 100644 index 0000000..8ae7621 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/OptimizedBvh.java @@ -0,0 +1,943 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import java.nio.ByteBuffer; +import java.util.ArrayList; +import java.util.List; +import javabullet.BulletStack; +import javabullet.linearmath.AabbUtil2; +import javabullet.linearmath.MiscUtil; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future). + * + * @author jezek2 + */ +public class OptimizedBvh { + + protected final BulletStack stack = BulletStack.get(); + + private static final boolean DEBUG_TREE_BUILDING = false; + private static int gStackDepth = 0; + private static int gMaxStackDepth = 0; + + private static int maxIterations = 0; + + // Note: currently we have 16 bytes per quantized node + public static final int MAX_SUBTREE_SIZE_IN_BYTES = 2048; + + // 10 gives the potential for 1024 parts, with at most 2^21 (2097152) (minus one + // actually) triangles each (since the sign bit is reserved + public static final int MAX_NUM_PARTS_IN_BITS = 10; + + //////////////////////////////////////////////////////////////////////////// + + private final List<OptimizedBvhNode> leafNodes = new ArrayList<OptimizedBvhNode>(); + private final List<OptimizedBvhNode> contiguousNodes = new ArrayList<OptimizedBvhNode>(); + + private QuantizedBvhNodes quantizedLeafNodes = new QuantizedBvhNodes(); + private QuantizedBvhNodes quantizedContiguousNodes = new QuantizedBvhNodes(); + + private int curNodeIndex; + + // quantization data + private boolean useQuantization; + private final Vector3f bvhAabbMin = new Vector3f(); + private final Vector3f bvhAabbMax = new Vector3f(); + private final Vector3f bvhQuantization = new Vector3f(); + + protected TraversalMode traversalMode; + protected final List<BvhSubtreeInfo> SubtreeHeaders = new ArrayList<BvhSubtreeInfo>(); + // This is only used for serialization so we don't have to add serialization directly to btAlignedObjectArray + protected int subtreeHeaderCount; + + // two versions, one for quantized and normal nodes. This allows code-reuse while maintaining readability (no template/macro!) + // this might be refactored into a virtual, it is usually not calculated at run-time + public void setInternalNodeAabbMin(int nodeIndex, Vector3f aabbMin) { + if (useQuantization) { + quantizedContiguousNodes.setQuantizedAabbMin(nodeIndex, quantizeWithClamp(aabbMin)); + } + else { + contiguousNodes.get(nodeIndex).aabbMinOrg.set(aabbMin); + } + } + + public void setInternalNodeAabbMax(int nodeIndex, Vector3f aabbMax) { + if (useQuantization) { + quantizedContiguousNodes.setQuantizedAabbMax(nodeIndex, quantizeWithClamp(aabbMax)); + } + else { + contiguousNodes.get(nodeIndex).aabbMaxOrg.set(aabbMax); + } + } + + public Vector3f getAabbMin(int nodeIndex) { + if (useQuantization) { + Vector3f tmp = new Vector3f(); + unQuantize(tmp, quantizedLeafNodes.getQuantizedAabbMin(nodeIndex)); + return tmp; + } + + // non-quantized + return leafNodes.get(nodeIndex).aabbMinOrg; + } + + public Vector3f getAabbMax(int nodeIndex) { + if (useQuantization) { + Vector3f tmp = new Vector3f(); + unQuantize(tmp, quantizedLeafNodes.getQuantizedAabbMax(nodeIndex)); + return tmp; + } + + // non-quantized + return leafNodes.get(nodeIndex).aabbMaxOrg; + } + + public void setQuantizationValues(Vector3f aabbMin, Vector3f aabbMax) { + setQuantizationValues(aabbMin, aabbMax, 1f); + } + + public void setQuantizationValues(Vector3f aabbMin, Vector3f aabbMax, float quantizationMargin) { + stack.vectors.push(); + try { + // enlarge the AABB to avoid division by zero when initializing the quantization values + Vector3f clampValue = stack.vectors.get(quantizationMargin,quantizationMargin,quantizationMargin); + bvhAabbMin.sub(aabbMin, clampValue); + bvhAabbMax.add(aabbMax, clampValue); + Vector3f aabbSize = stack.vectors.get(); + aabbSize.sub(bvhAabbMax, bvhAabbMin); + bvhQuantization.set(65535f, 65535f, 65535f); + VectorUtil.div(bvhQuantization, bvhQuantization, aabbSize); + } + finally { + stack.vectors.pop(); + } + } + + public void setInternalNodeEscapeIndex(int nodeIndex, int escapeIndex) { + if (useQuantization) { + quantizedContiguousNodes.setEscapeIndexOrTriangleIndex(nodeIndex, -escapeIndex); + } + else { + contiguousNodes.get(nodeIndex).escapeIndex = escapeIndex; + } + } + + public void mergeInternalNodeAabb(int nodeIndex, Vector3f newAabbMin, Vector3f newAabbMax) { + if (useQuantization) { + long quantizedAabbMin; + long quantizedAabbMax; + + quantizedAabbMin = quantizeWithClamp(newAabbMin); + quantizedAabbMax = quantizeWithClamp(newAabbMax); + for (int i = 0; i < 3; i++) { + if (quantizedContiguousNodes.getQuantizedAabbMin(nodeIndex, i) > QuantizedBvhNodes.getCoord(quantizedAabbMin, i)) { + quantizedContiguousNodes.setQuantizedAabbMin(nodeIndex, i, QuantizedBvhNodes.getCoord(quantizedAabbMin, i)); + } + + if (quantizedContiguousNodes.getQuantizedAabbMax(nodeIndex, i) < QuantizedBvhNodes.getCoord(quantizedAabbMax, i)) { + quantizedContiguousNodes.setQuantizedAabbMax(nodeIndex, i, QuantizedBvhNodes.getCoord(quantizedAabbMax, i)); + } + } + } + else { + // non-quantized + VectorUtil.setMin(contiguousNodes.get(nodeIndex).aabbMinOrg, newAabbMin); + VectorUtil.setMax(contiguousNodes.get(nodeIndex).aabbMaxOrg, newAabbMax); + } + } + + public void swapLeafNodes(int i, int splitIndex) { + if (useQuantization) { + quantizedLeafNodes.swap(i, splitIndex); + } + else { + // JAVA NOTE: changing reference instead of copy + OptimizedBvhNode tmp = leafNodes.get(i); + leafNodes.set(i, leafNodes.get(splitIndex)); + leafNodes.set(splitIndex, tmp); + } + } + + public void assignInternalNodeFromLeafNode(int internalNode, int leafNodeIndex) { + if (useQuantization) { + quantizedContiguousNodes.set(internalNode, quantizedLeafNodes, leafNodeIndex); + } + else { + contiguousNodes.get(internalNode).set(leafNodes.get(leafNodeIndex)); + } + } + + private static class NodeTriangleCallback implements InternalTriangleIndexCallback { + public List<OptimizedBvhNode> triangleNodes; + + public NodeTriangleCallback(List<OptimizedBvhNode> triangleNodes) { + this.triangleNodes = triangleNodes; + } + + private final Vector3f aabbMin = new Vector3f(), aabbMax = new Vector3f(); + + public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) { + OptimizedBvhNode node = new OptimizedBvhNode(); + aabbMin.set(1e30f, 1e30f, 1e30f); + aabbMax.set(-1e30f, -1e30f, -1e30f); + VectorUtil.setMin(aabbMin, triangle[0]); + VectorUtil.setMax(aabbMax, triangle[0]); + VectorUtil.setMin(aabbMin, triangle[1]); + VectorUtil.setMax(aabbMax, triangle[1]); + VectorUtil.setMin(aabbMin, triangle[2]); + VectorUtil.setMax(aabbMax, triangle[2]); + + // with quantization? + node.aabbMinOrg.set(aabbMin); + node.aabbMaxOrg.set(aabbMax); + + node.escapeIndex = -1; + + // for child nodes + node.subPart = partId; + node.triangleIndex = triangleIndex; + triangleNodes.add(node); + } + } + + private static class QuantizedNodeTriangleCallback implements InternalTriangleIndexCallback { + protected final BulletStack stack = BulletStack.get(); + + public QuantizedBvhNodes triangleNodes; + public OptimizedBvh optimizedTree; // for quantization + + public QuantizedNodeTriangleCallback(QuantizedBvhNodes triangleNodes, OptimizedBvh tree) { + this.triangleNodes = triangleNodes; + this.optimizedTree = tree; + } + + public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) { + // The partId and triangle index must fit in the same (positive) integer + assert (partId < (1 << MAX_NUM_PARTS_IN_BITS)); + assert (triangleIndex < (1 << (31 - MAX_NUM_PARTS_IN_BITS))); + // negative indices are reserved for escapeIndex + assert (triangleIndex >= 0); + + stack.vectors.push(); + try { + int nodeId = triangleNodes.add(); + Vector3f aabbMin = stack.vectors.get(), aabbMax = stack.vectors.get(); + aabbMin.set(1e30f, 1e30f, 1e30f); + aabbMax.set(-1e30f, -1e30f, -1e30f); + VectorUtil.setMin(aabbMin, triangle[0]); + VectorUtil.setMax(aabbMax, triangle[0]); + VectorUtil.setMin(aabbMin, triangle[1]); + VectorUtil.setMax(aabbMax, triangle[1]); + VectorUtil.setMin(aabbMin, triangle[2]); + VectorUtil.setMax(aabbMax, triangle[2]); + + // PCK: add these checks for zero dimensions of aabb + final float MIN_AABB_DIMENSION = 0.002f; + final float MIN_AABB_HALF_DIMENSION = 0.001f; + if (aabbMax.x - aabbMin.x < MIN_AABB_DIMENSION) { + aabbMax.x = (aabbMax.x + MIN_AABB_HALF_DIMENSION); + aabbMin.x = (aabbMin.x - MIN_AABB_HALF_DIMENSION); + } + if (aabbMax.y - aabbMin.y < MIN_AABB_DIMENSION) { + aabbMax.y = (aabbMax.y + MIN_AABB_HALF_DIMENSION); + aabbMin.y = (aabbMin.y - MIN_AABB_HALF_DIMENSION); + } + if (aabbMax.z - aabbMin.z < MIN_AABB_DIMENSION) { + aabbMax.z = (aabbMax.z + MIN_AABB_HALF_DIMENSION); + aabbMin.z = (aabbMin.z - MIN_AABB_HALF_DIMENSION); + } + + triangleNodes.setQuantizedAabbMin(nodeId, optimizedTree.quantizeWithClamp(aabbMin)); + triangleNodes.setQuantizedAabbMax(nodeId, optimizedTree.quantizeWithClamp(aabbMax)); + + triangleNodes.setEscapeIndexOrTriangleIndex(nodeId, (partId << (31 - MAX_NUM_PARTS_IN_BITS)) | triangleIndex); + } + finally { + stack.vectors.pop(); + } + } + } + + public void build(StridingMeshInterface triangles, boolean useQuantizedAabbCompression, Vector3f _aabbMin, Vector3f _aabbMax) { + stack.vectors.push(); + try { + this.useQuantization = useQuantizedAabbCompression; + + // NodeArray triangleNodes; + + int numLeafNodes = 0; + + if (useQuantization) { + // initialize quantization values + setQuantizationValues(_aabbMin, _aabbMax); + + QuantizedNodeTriangleCallback callback = new QuantizedNodeTriangleCallback(quantizedLeafNodes, this); + + triangles.internalProcessAllTriangles(callback, bvhAabbMin, bvhAabbMax); + + // now we have an array of leafnodes in m_leafNodes + numLeafNodes = quantizedLeafNodes.size(); + + quantizedContiguousNodes.resize(2 * numLeafNodes); + } + else { + NodeTriangleCallback callback = new NodeTriangleCallback(leafNodes); + + Vector3f aabbMin = stack.vectors.get(-1e30f, -1e30f, -1e30f); + Vector3f aabbMax = stack.vectors.get(1e30f, 1e30f, 1e30f); + + triangles.internalProcessAllTriangles(callback, aabbMin, aabbMax); + + // now we have an array of leafnodes in m_leafNodes + numLeafNodes = leafNodes.size(); + + // TODO: check + //contiguousNodes.resize(2*numLeafNodes); + MiscUtil.resize(contiguousNodes, 2 * numLeafNodes, OptimizedBvhNode.class); + } + + curNodeIndex = 0; + + buildTree(0, numLeafNodes); + + // if the entire tree is small then subtree size, we need to create a header info for the tree + if (useQuantization && SubtreeHeaders.size() == 0) { + BvhSubtreeInfo subtree = new BvhSubtreeInfo(); + SubtreeHeaders.add(subtree); + + subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, 0); + subtree.rootNodeIndex = 0; + subtree.subtreeSize = quantizedContiguousNodes.isLeafNode(0) ? 1 : quantizedContiguousNodes.getEscapeIndex(0); + } + + // PCK: update the copy of the size + subtreeHeaderCount = SubtreeHeaders.size(); + + // PCK: clear m_quantizedLeafNodes and m_leafNodes, they are temporary + quantizedLeafNodes.clear(); + leafNodes.clear(); + } + finally { + stack.vectors.pop(); + } + } + + public void refit(StridingMeshInterface meshInterface) { + stack.vectors.push(); + try { + if (useQuantization) { + // calculate new aabb + Vector3f aabbMin = stack.vectors.get(), aabbMax = stack.vectors.get(); + meshInterface.calculateAabbBruteForce(aabbMin, aabbMax); + + setQuantizationValues(aabbMin, aabbMax); + + updateBvhNodes(meshInterface, 0, curNodeIndex, 0); + + // now update all subtree headers + + int i; + for (i = 0; i < SubtreeHeaders.size(); i++) { + BvhSubtreeInfo subtree = SubtreeHeaders.get(i); + subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, subtree.rootNodeIndex); + } + + } + else { + // JAVA NOTE: added for testing, it's too slow for practical use + build(meshInterface, false, null, null); + } + } + finally { + stack.vectors.pop(); + } + } + + public void refitPartial(StridingMeshInterface meshInterface, Vector3f aabbMin, Vector3f aabbMax) { + throw new UnsupportedOperationException(); +// // incrementally initialize quantization values +// assert (useQuantization); +// +// btAssert(aabbMin.getX() > m_bvhAabbMin.getX()); +// btAssert(aabbMin.getY() > m_bvhAabbMin.getY()); +// btAssert(aabbMin.getZ() > m_bvhAabbMin.getZ()); +// +// btAssert(aabbMax.getX() < m_bvhAabbMax.getX()); +// btAssert(aabbMax.getY() < m_bvhAabbMax.getY()); +// btAssert(aabbMax.getZ() < m_bvhAabbMax.getZ()); +// +// ///we should update all quantization values, using updateBvhNodes(meshInterface); +// ///but we only update chunks that overlap the given aabb +// +// unsigned short quantizedQueryAabbMin[3]; +// unsigned short quantizedQueryAabbMax[3]; +// +// quantizeWithClamp(&quantizedQueryAabbMin[0],aabbMin); +// quantizeWithClamp(&quantizedQueryAabbMax[0],aabbMax); +// +// int i; +// for (i=0;i<this->m_SubtreeHeaders.size();i++) +// { +// btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i]; +// +// //PCK: unsigned instead of bool +// unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax); +// if (overlap != 0) +// { +// updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i); +// +// subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]); +// } +// } + } + + private VertexData data = new VertexData(); + + public void updateBvhNodes(StridingMeshInterface meshInterface, int firstNode, int endNode, int index) { + assert (useQuantization); + + stack.vectors.push(); + try { + int curNodeSubPart = -1; + + Vector3f[] triangleVerts/*[3]*/ = new Vector3f[] { stack.vectors.get(), stack.vectors.get(), stack.vectors.get() }; + Vector3f aabbMin = stack.vectors.get(), aabbMax = stack.vectors.get(); + Vector3f meshScaling = meshInterface.getScaling(); + + for (int i = endNode - 1; i >= firstNode; i--) { + QuantizedBvhNodes curNodes = quantizedContiguousNodes; + int curNodeId = i; + + if (curNodes.isLeafNode(curNodeId)) { + // recalc aabb from triangle data + int nodeSubPart = curNodes.getPartId(curNodeId); + int nodeTriangleIndex = curNodes.getTriangleIndex(curNodeId); + if (nodeSubPart != curNodeSubPart) { + if (curNodeSubPart >= 0) { + meshInterface.unLockReadOnlyVertexBase(curNodeSubPart); + } + meshInterface.getLockedReadOnlyVertexIndexBase(data, nodeSubPart); + assert (data.indicestype == ScalarType.PHY_INTEGER || data.indicestype == ScalarType.PHY_SHORT); + } + //triangles->getLockedReadOnlyVertexIndexBase(vertexBase,numVerts, + + ByteBuffer gfxbase_ptr = data.indexbase; + int gfxbase_index = nodeTriangleIndex * data.indexstride; + + for (int j = 2; j >= 0; j--) { + int graphicsindex; + if (data.indicestype == ScalarType.PHY_SHORT) { + graphicsindex = gfxbase_ptr.getShort(gfxbase_index + j * 2) & 0xFFFF; + } + else { + graphicsindex = gfxbase_ptr.getInt(gfxbase_index + j * 4); + } + + ByteBuffer graphicsbase_ptr = data.vertexbase; + int graphicsbase_index = graphicsindex * data.stride; + + //#ifdef DEBUG_PATCH_COLORS + //btVector3 mycolor = color[index&3]; + //graphicsbase[8] = mycolor.getX(); + //graphicsbase[9] = mycolor.getY(); + //graphicsbase[10] = mycolor.getZ(); + //#endif //DEBUG_PATCH_COLORS + + triangleVerts[j].set( + graphicsbase_ptr.getFloat(graphicsbase_index + 4 * 0) * meshScaling.x, + graphicsbase_ptr.getFloat(graphicsbase_index + 4 * 1) * meshScaling.y, + graphicsbase_ptr.getFloat(graphicsbase_index + 4 * 2) * meshScaling.z); + } + + aabbMin.set(1e30f, 1e30f, 1e30f); + aabbMax.set(-1e30f, -1e30f, -1e30f); + VectorUtil.setMin(aabbMin, triangleVerts[0]); + VectorUtil.setMax(aabbMax, triangleVerts[0]); + VectorUtil.setMin(aabbMin, triangleVerts[1]); + VectorUtil.setMax(aabbMax, triangleVerts[1]); + VectorUtil.setMin(aabbMin, triangleVerts[2]); + VectorUtil.setMax(aabbMax, triangleVerts[2]); + + curNodes.setQuantizedAabbMin(curNodeId, quantizeWithClamp(aabbMin)); + curNodes.setQuantizedAabbMax(curNodeId, quantizeWithClamp(aabbMax)); + } + else { + // combine aabb from both children + + //quantizedContiguousNodes + int leftChildNodeId = i + 1; + + int rightChildNodeId = quantizedContiguousNodes.isLeafNode(leftChildNodeId) ? i + 2 : i + 1 + quantizedContiguousNodes.getEscapeIndex(leftChildNodeId); + + for (int i2 = 0; i2 < 3; i2++) { + curNodes.setQuantizedAabbMin(curNodeId, i2, quantizedContiguousNodes.getQuantizedAabbMin(leftChildNodeId, i2)); + if (curNodes.getQuantizedAabbMin(curNodeId, i2) > quantizedContiguousNodes.getQuantizedAabbMin(rightChildNodeId, i2)) { + curNodes.setQuantizedAabbMin(curNodeId, i2, quantizedContiguousNodes.getQuantizedAabbMin(rightChildNodeId, i2)); + } + + curNodes.setQuantizedAabbMax(curNodeId, i2, quantizedContiguousNodes.getQuantizedAabbMax(leftChildNodeId, i2)); + if (curNodes.getQuantizedAabbMax(curNodeId, i2) < quantizedContiguousNodes.getQuantizedAabbMax(rightChildNodeId, i2)) { + curNodes.setQuantizedAabbMax(curNodeId, i2, quantizedContiguousNodes.getQuantizedAabbMax(rightChildNodeId, i2)); + } + } + } + } + + if (curNodeSubPart >= 0) { + meshInterface.unLockReadOnlyVertexBase(curNodeSubPart); + } + + data.unref(); + } + finally { + stack.vectors.pop(); + } + } + + protected void buildTree(int startIndex, int endIndex) { + stack.vectors.push(); + try { + //#ifdef DEBUG_TREE_BUILDING + if (DEBUG_TREE_BUILDING) { + gStackDepth++; + if (gStackDepth > gMaxStackDepth) { + gMaxStackDepth = gStackDepth; + } + } + //#endif //DEBUG_TREE_BUILDING + + int splitAxis, splitIndex, i; + int numIndices = endIndex - startIndex; + int curIndex = curNodeIndex; + + assert (numIndices > 0); + + if (numIndices == 1) { + //#ifdef DEBUG_TREE_BUILDING + if (DEBUG_TREE_BUILDING) { + gStackDepth--; + } + //#endif //DEBUG_TREE_BUILDING + + assignInternalNodeFromLeafNode(curNodeIndex, startIndex); + + curNodeIndex++; + return; + } + // calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'. + + splitAxis = calcSplittingAxis(startIndex, endIndex); + + splitIndex = sortAndCalcSplittingIndex(startIndex, endIndex, splitAxis); + + int internalNodeIndex = curNodeIndex; + + setInternalNodeAabbMax(curNodeIndex, stack.vectors.get(-1e30f, -1e30f, -1e30f)); + setInternalNodeAabbMin(curNodeIndex, stack.vectors.get(1e30f, 1e30f, 1e30f)); + + for (i = startIndex; i < endIndex; i++) { + mergeInternalNodeAabb(curNodeIndex, getAabbMin(i), getAabbMax(i)); + } + + curNodeIndex++; + + //internalNode->m_escapeIndex; + + int leftChildNodexIndex = curNodeIndex; + + //build left child tree + buildTree(startIndex, splitIndex); + + int rightChildNodexIndex = curNodeIndex; + // build right child tree + buildTree(splitIndex, endIndex); + + //#ifdef DEBUG_TREE_BUILDING + if (DEBUG_TREE_BUILDING) { + gStackDepth--; + } + //#endif //DEBUG_TREE_BUILDING + + int escapeIndex = curNodeIndex - curIndex; + + if (useQuantization) { + // escapeIndex is the number of nodes of this subtree + int sizeQuantizedNode = QuantizedBvhNodes.getNodeSize(); + int treeSizeInBytes = escapeIndex * sizeQuantizedNode; + if (treeSizeInBytes > MAX_SUBTREE_SIZE_IN_BYTES) { + updateSubtreeHeaders(leftChildNodexIndex, rightChildNodexIndex); + } + } + + setInternalNodeEscapeIndex(internalNodeIndex, escapeIndex); + } + finally { + stack.vectors.pop(); + } + } + + protected boolean testQuantizedAabbAgainstQuantizedAabb(long aabbMin1, long aabbMax1, long aabbMin2, long aabbMax2) { + int aabbMin1_0 = QuantizedBvhNodes.getCoord(aabbMin1, 0); + int aabbMin1_1 = QuantizedBvhNodes.getCoord(aabbMin1, 1); + int aabbMin1_2 = QuantizedBvhNodes.getCoord(aabbMin1, 2); + + int aabbMax1_0 = QuantizedBvhNodes.getCoord(aabbMax1, 0); + int aabbMax1_1 = QuantizedBvhNodes.getCoord(aabbMax1, 1); + int aabbMax1_2 = QuantizedBvhNodes.getCoord(aabbMax1, 2); + + int aabbMin2_0 = QuantizedBvhNodes.getCoord(aabbMin2, 0); + int aabbMin2_1 = QuantizedBvhNodes.getCoord(aabbMin2, 1); + int aabbMin2_2 = QuantizedBvhNodes.getCoord(aabbMin2, 2); + + int aabbMax2_0 = QuantizedBvhNodes.getCoord(aabbMax2, 0); + int aabbMax2_1 = QuantizedBvhNodes.getCoord(aabbMax2, 1); + int aabbMax2_2 = QuantizedBvhNodes.getCoord(aabbMax2, 2); + + boolean overlap = true; + overlap = (aabbMin1_0 > aabbMax2_0 || aabbMax1_0 < aabbMin2_0) ? false : overlap; + overlap = (aabbMin1_2 > aabbMax2_2 || aabbMax1_2 < aabbMin2_2) ? false : overlap; + overlap = (aabbMin1_1 > aabbMax2_1 || aabbMax1_1 < aabbMin2_1) ? false : overlap; + return overlap; + } + + protected void updateSubtreeHeaders(int leftChildNodexIndex, int rightChildNodexIndex) { + assert (useQuantization); + + //btQuantizedBvhNode& leftChildNode = m_quantizedContiguousNodes[leftChildNodexIndex]; + int leftSubTreeSize = quantizedContiguousNodes.isLeafNode(leftChildNodexIndex) ? 1 : quantizedContiguousNodes.getEscapeIndex(leftChildNodexIndex); + int leftSubTreeSizeInBytes = leftSubTreeSize * QuantizedBvhNodes.getNodeSize(); + + //btQuantizedBvhNode& rightChildNode = m_quantizedContiguousNodes[rightChildNodexIndex]; + int rightSubTreeSize = quantizedContiguousNodes.isLeafNode(rightChildNodexIndex) ? 1 : quantizedContiguousNodes.getEscapeIndex(rightChildNodexIndex); + int rightSubTreeSizeInBytes = rightSubTreeSize * QuantizedBvhNodes.getNodeSize(); + + if (leftSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) { + BvhSubtreeInfo subtree = new BvhSubtreeInfo(); + SubtreeHeaders.add(subtree); + + subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, leftChildNodexIndex); + subtree.rootNodeIndex = leftChildNodexIndex; + subtree.subtreeSize = leftSubTreeSize; + } + + if (rightSubTreeSizeInBytes <= MAX_SUBTREE_SIZE_IN_BYTES) { + BvhSubtreeInfo subtree = new BvhSubtreeInfo(); + SubtreeHeaders.add(subtree); + + subtree.setAabbFromQuantizeNode(quantizedContiguousNodes, rightChildNodexIndex); + subtree.rootNodeIndex = rightChildNodexIndex; + subtree.subtreeSize = rightSubTreeSize; + } + + // PCK: update the copy of the size + subtreeHeaderCount = SubtreeHeaders.size(); + } + + protected int sortAndCalcSplittingIndex(int startIndex, int endIndex, int splitAxis) { + stack.vectors.push(); + try { + int i; + int splitIndex = startIndex; + int numIndices = endIndex - startIndex; + float splitValue; + + Vector3f means = stack.vectors.get(0f, 0f, 0f); + Vector3f center = stack.vectors.get(); + for (i = startIndex; i < endIndex; i++) { + center.add(getAabbMax(i), getAabbMin(i)); + center.scale(0.5f); + means.add(center); + } + means.scale(1f / (float) numIndices); + + splitValue = VectorUtil.getCoord(means, splitAxis); + + //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'. + for (i = startIndex; i < endIndex; i++) { + //Vector3f center = new Vector3f(); + center.add(getAabbMax(i), getAabbMin(i)); + center.scale(0.5f); + + if (VectorUtil.getCoord(center, splitAxis) > splitValue) { + // swap + swapLeafNodes(i, splitIndex); + splitIndex++; + } + } + + // if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex + // otherwise the tree-building might fail due to stack-overflows in certain cases. + // unbalanced1 is unsafe: it can cause stack overflows + // bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1))); + + // unbalanced2 should work too: always use center (perfect balanced trees) + // bool unbalanced2 = true; + + // this should be safe too: + int rangeBalancedIndices = numIndices / 3; + boolean unbalanced = ((splitIndex <= (startIndex + rangeBalancedIndices)) || (splitIndex >= (endIndex - 1 - rangeBalancedIndices))); + + if (unbalanced) { + splitIndex = startIndex + (numIndices >> 1); + } + + boolean unbal = (splitIndex == startIndex) || (splitIndex == (endIndex)); + assert (!unbal); + + return splitIndex; + } + finally { + stack.vectors.pop(); + } + } + + protected int calcSplittingAxis(int startIndex, int endIndex) { + stack.vectors.push(); + try { + int i; + + Vector3f means = stack.vectors.get(0f, 0f, 0f); + Vector3f variance = stack.vectors.get(0f, 0f, 0f); + int numIndices = endIndex - startIndex; + + Vector3f center = stack.vectors.get(); + for (i = startIndex; i < endIndex; i++) { + center.add(getAabbMax(i), getAabbMin(i)); + center.scale(0.5f); + means.add(center); + } + means.scale(1f / (float) numIndices); + + Vector3f diff2 = stack.vectors.get(); + for (i = startIndex; i < endIndex; i++) { + center.add(getAabbMax(i), getAabbMin(i)); + center.scale(0.5f); + diff2.sub(center, means); + //diff2 = diff2 * diff2; + VectorUtil.mul(diff2, diff2, diff2); + variance.add(diff2); + } + variance.scale(1f / ((float) numIndices - 1)); + + return VectorUtil.maxAxis(variance); + } + finally { + stack.vectors.pop(); + } + } + + public void reportAabbOverlappingNodex(NodeOverlapCallback nodeCallback, Vector3f aabbMin, Vector3f aabbMax) { + // either choose recursive traversal (walkTree) or stackless (walkStacklessTree) + + if (useQuantization) { + // quantize query AABB + long quantizedQueryAabbMin; + long quantizedQueryAabbMax; + quantizedQueryAabbMin = quantizeWithClamp(aabbMin); + quantizedQueryAabbMax = quantizeWithClamp(aabbMax); + + // TODO + /* + switch (traversalMode) { + case TRAVERSAL_STACKLESS: + walkStacklessQuantizedTree(nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax, 0, curNodeIndex); + break; + + case TRAVERSAL_STACKLESS_CACHE_FRIENDLY: + walkStacklessQuantizedTreeCacheFriendly(nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax); + break; + + case TRAVERSAL_RECURSIVE:*/ + walkRecursiveQuantizedTreeAgainstQueryAabb(quantizedContiguousNodes, 0, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax); + /*break; + + default: + assert (false); // unsupported + }*/ + } + else { + walkStacklessTree(nodeCallback, aabbMin, aabbMax); + } + } + + protected void walkStacklessTree(NodeOverlapCallback nodeCallback, Vector3f aabbMin, Vector3f aabbMax) { + assert (!useQuantization); + + // JAVA NOTE: rewritten + OptimizedBvhNode rootNode = null;//contiguousNodes.get(0); + int rootNode_index = 0; + + int escapeIndex, curIndex = 0; + int walkIterations = 0; + boolean isLeafNode; + //PCK: unsigned instead of bool + //unsigned aabbOverlap; + boolean aabbOverlap; + + while (curIndex < curNodeIndex) { + // catch bugs in tree data + assert (walkIterations < curNodeIndex); + + walkIterations++; + + rootNode = contiguousNodes.get(rootNode_index); + + aabbOverlap = AabbUtil2.testAabbAgainstAabb2(aabbMin, aabbMax, rootNode.aabbMinOrg, rootNode.aabbMaxOrg); + isLeafNode = (rootNode.escapeIndex == -1); + + // PCK: unsigned instead of bool + if (isLeafNode && (aabbOverlap/* != 0*/)) { + nodeCallback.processNode(rootNode.subPart, rootNode.triangleIndex); + } + + rootNode = null; + + //PCK: unsigned instead of bool + if ((aabbOverlap/* != 0*/) || isLeafNode) { + rootNode_index++; + curIndex++; + } + else { + escapeIndex = /*rootNode*/ contiguousNodes.get(rootNode_index).escapeIndex; + rootNode_index += escapeIndex; + curIndex += escapeIndex; + } + } + if (maxIterations < walkIterations) { + maxIterations = walkIterations; + } + } + + protected void walkRecursiveQuantizedTreeAgainstQueryAabb(QuantizedBvhNodes currentNodes, int currentNodeId, NodeOverlapCallback nodeCallback, long quantizedQueryAabbMin, long quantizedQueryAabbMax) { + assert (useQuantization); + + boolean isLeafNode; + boolean aabbOverlap; + + aabbOverlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin, quantizedQueryAabbMax, currentNodes.getQuantizedAabbMin(currentNodeId), currentNodes.getQuantizedAabbMax(currentNodeId)); + isLeafNode = currentNodes.isLeafNode(currentNodeId); + + if (aabbOverlap) { + if (isLeafNode) { + nodeCallback.processNode(currentNodes.getPartId(currentNodeId), currentNodes.getTriangleIndex(currentNodeId)); + } + else { + // process left and right children + int leftChildNodeId = currentNodeId + 1; + walkRecursiveQuantizedTreeAgainstQueryAabb(currentNodes, leftChildNodeId, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax); + + int rightChildNodeId = currentNodes.isLeafNode(leftChildNodeId) ? leftChildNodeId + 1 : leftChildNodeId + currentNodes.getEscapeIndex(leftChildNodeId); + walkRecursiveQuantizedTreeAgainstQueryAabb(currentNodes, rightChildNodeId, nodeCallback, quantizedQueryAabbMin, quantizedQueryAabbMax); + } + } + } + + public void reportRayOverlappingNodex(NodeOverlapCallback nodeCallback, Vector3f raySource, Vector3f rayTarget) { + stack.vectors.push(); + try { + boolean fast_path = useQuantization && traversalMode == TraversalMode.TRAVERSAL_STACKLESS; + if (fast_path) { + throw new UnsupportedOperationException(); + //walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, btVector3(0, 0, 0), btVector3(0, 0, 0), 0, m_curNodeIndex); + } + else { + /* Otherwise fallback to AABB overlap test */ + Vector3f aabbMin = stack.vectors.get(raySource); + Vector3f aabbMax = stack.vectors.get(raySource); + VectorUtil.setMin(aabbMin, rayTarget); + VectorUtil.setMax(aabbMax, rayTarget); + reportAabbOverlappingNodex(nodeCallback, aabbMin, aabbMax); + } + } + finally { + stack.vectors.pop(); + } + } + + public void reportBoxCastOverlappingNodex(NodeOverlapCallback nodeCallback, Vector3f raySource, Vector3f rayTarget, Vector3f aabbMin, Vector3f aabbMax) { + stack.vectors.push(); + try { + boolean fast_path = useQuantization && traversalMode == TraversalMode.TRAVERSAL_STACKLESS; + if (fast_path) { + throw new UnsupportedOperationException(); + //walkStacklessQuantizedTreeAgainstRay(nodeCallback, raySource, rayTarget, aabbMin, aabbMax, 0, m_curNodeIndex); + } + else { + /* Slow path: + Construct the bounding box for the entire box cast and send that down the tree */ + Vector3f qaabbMin = stack.vectors.get(raySource); + Vector3f qaabbMax = stack.vectors.get(raySource); + VectorUtil.setMin(qaabbMin, rayTarget); + VectorUtil.setMax(qaabbMax, rayTarget); + qaabbMin.add(aabbMin); + qaabbMax.add(aabbMax); + reportAabbOverlappingNodex(nodeCallback, qaabbMin, qaabbMax); + } + } + finally { + stack.vectors.pop(); + } + } + + public long quantizeWithClamp(Vector3f point) { + assert (useQuantization); + + stack.vectors.push(); + try { + Vector3f clampedPoint = stack.vectors.get(point); + VectorUtil.setMax(clampedPoint, bvhAabbMin); + VectorUtil.setMin(clampedPoint, bvhAabbMax); + + Vector3f v = stack.vectors.get(); + v.sub(clampedPoint, bvhAabbMin); + VectorUtil.mul(v, v, bvhQuantization); + + int out0 = (int)(v.x + 0.5f) & 0xFFFF; + int out1 = (int)(v.y + 0.5f) & 0xFFFF; + int out2 = (int)(v.z + 0.5f) & 0xFFFF; + + return ((long)out0) | (((long)out1) << 16) | (((long)out2) << 32); + } + finally { + stack.vectors.pop(); + } + } + + public void unQuantize(Vector3f vecOut, long vecIn) { + int vecIn0 = (int)((vecIn & 0x00000000FFFFL)); + int vecIn1 = (int)((vecIn & 0x0000FFFF0000L) >>> 16); + int vecIn2 = (int)((vecIn & 0xFFFF00000000L) >>> 32); + + vecOut.x = (float)vecIn0 / (bvhQuantization.x); + vecOut.y = (float)vecIn1 / (bvhQuantization.y); + vecOut.z = (float)vecIn2 / (bvhQuantization.z); + + vecOut.add(bvhAabbMin); + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/OptimizedBvhNode.java b/src/jbullet/src/javabullet/collision/shapes/OptimizedBvhNode.java new file mode 100644 index 0000000..b493588 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/OptimizedBvhNode.java @@ -0,0 +1,53 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javax.vecmath.Vector3f; + +/** + * OptimizedBvhNode contains both internal and leaf node information. + * Total node size is 44 bytes / node. You can use the compressed version of 16 bytes. + * + * @author jezek2 + */ +public class OptimizedBvhNode { + + public final Vector3f aabbMinOrg = new Vector3f(); + public final Vector3f aabbMaxOrg = new Vector3f(); + + public int escapeIndex; + + // for child nodes + public int subPart; + public int triangleIndex; + + public void set(OptimizedBvhNode n) { + aabbMinOrg.set(n.aabbMinOrg); + aabbMaxOrg.set(n.aabbMaxOrg); + escapeIndex = n.escapeIndex; + subPart = n.subPart; + triangleIndex = n.triangleIndex; + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/PolyhedralConvexShape.java b/src/jbullet/src/javabullet/collision/shapes/PolyhedralConvexShape.java new file mode 100644 index 0000000..4396be1 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/PolyhedralConvexShape.java @@ -0,0 +1,248 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.linearmath.MatrixUtil; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Matrix3f; +import javax.vecmath.Vector3f; + +/** + * PolyhedralConvexShape is an interface class for feature based (vertex/edge/face) convex shapes. + * + * @author jezek2 + */ +public abstract class PolyhedralConvexShape extends ConvexInternalShape { + + protected final Vector3f localAabbMin = new Vector3f(1f, 1f, 1f); + protected final Vector3f localAabbMax = new Vector3f(-1f, -1f, -1f); + protected boolean isLocalAabbValid = false; + +// /** optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp */ +// public Hull optionalHull = null; + + @Override + public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec0) { + stack.vectors.push(); + try { + int i; + Vector3f supVec = stack.vectors.get(0f, 0f, 0f); + + float maxDot = -1e30f; + + Vector3f vec = stack.vectors.get(vec0); + float lenSqr = vec.lengthSquared(); + if (lenSqr < 0.0001f) { + vec.set(1f, 0f, 0f); + } + else { + float rlen = 1f / (float) Math.sqrt(lenSqr); + vec.scale(rlen); + } + + Vector3f vtx = stack.vectors.get(); + float newDot; + + for (i = 0; i < getNumVertices(); i++) { + getVertex(i, vtx); + newDot = vec.dot(vtx); + if (newDot > maxDot) { + maxDot = newDot; + supVec = vtx; + } + } + + return stack.vectors.returning(supVec); + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) { + stack.vectors.push(); + try { + int i; + + Vector3f vtx = stack.vectors.get(); + float newDot; + + // JAVA NOTE: rewritten as code used W coord for temporary usage in Vector3 + // TODO: optimize it + float[] wcoords = new float[numVectors]; + + for (i = 0; i < numVectors; i++) { + // TODO: used w in vector3: + //supportVerticesOut[i].w = -1e30f; + wcoords[i] = -1e30f; + } + + for (int j = 0; j < numVectors; j++) { + Vector3f vec = vectors[j]; + + for (i = 0; i < getNumVertices(); i++) { + getVertex(i, vtx); + newDot = vec.dot(vtx); + //if (newDot > supportVerticesOut[j].w) + if (newDot > wcoords[j]) { + //WARNING: don't swap next lines, the w component would get overwritten! + supportVerticesOut[j].set(vtx); + //supportVerticesOut[j].w = newDot; + wcoords[j] = newDot; + } + } + } + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void calculateLocalInertia(float mass, Vector3f inertia) { + stack.pushCommonMath(); + try { + // not yet, return box inertia + + float margin = getMargin(); + + Transform ident = stack.transforms.get(); + ident.setIdentity(); + Vector3f aabbMin = stack.vectors.get(), aabbMax = stack.vectors.get(); + getAabb(ident, aabbMin, aabbMax); + + Vector3f halfExtents = stack.vectors.get(); + halfExtents.sub(aabbMax, aabbMin); + halfExtents.scale(0.5f); + + float lx = 2f * (halfExtents.x + margin); + float ly = 2f * (halfExtents.y + margin); + float lz = 2f * (halfExtents.z + margin); + float x2 = lx * lx; + float y2 = ly * ly; + float z2 = lz * lz; + float scaledmass = mass * 0.08333333f; + + inertia.set(y2 + z2, x2 + z2, x2 + y2); + inertia.scale(scaledmass); + } + finally { + stack.popCommonMath(); + } + } + + private void getNonvirtualAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax, float margin) { + stack.pushCommonMath(); + try { + // lazy evaluation of local aabb + assert (isLocalAabbValid); + + assert (localAabbMin.x <= localAabbMax.x); + assert (localAabbMin.y <= localAabbMax.y); + assert (localAabbMin.z <= localAabbMax.z); + + Vector3f localHalfExtents = stack.vectors.get(); + localHalfExtents.sub(localAabbMax, localAabbMin); + localHalfExtents.scale(0.5f); + + Vector3f localCenter = stack.vectors.get(); + localCenter.add(localAabbMax, localAabbMin); + localCenter.scale(0.5f); + + Matrix3f abs_b = stack.matrices.get(trans.basis); + MatrixUtil.absolute(abs_b); + + Vector3f center = stack.vectors.get(localCenter); + trans.transform(center); + + Vector3f extent = stack.vectors.get(); + Vector3f tmp = stack.vectors.get(); + + abs_b.getRow(0, tmp); + extent.x = tmp.dot(localHalfExtents); + abs_b.getRow(1, tmp); + extent.y = tmp.dot(localHalfExtents); + abs_b.getRow(2, tmp); + extent.z = tmp.dot(localHalfExtents); + + extent.x += margin; + extent.y += margin; + extent.z += margin; + + aabbMin.sub(center, extent); + aabbMax.add(center, extent); + } + finally { + stack.popCommonMath(); + } + } + + @Override + public void getAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax) { + getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin()); + } + + protected final void _PolyhedralConvexShape_getAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax) { + getNonvirtualAabb(trans, aabbMin, aabbMax, getMargin()); + } + + public void recalcLocalAabb() { + stack.vectors.push(); + try { + isLocalAabbValid = true; + + for (int i = 0; i < 3; i++) { + Vector3f vec = stack.vectors.get(0f, 0f, 0f); + VectorUtil.setCoord(vec, i, 1f); + Vector3f tmp = stack.vectors.get(localGetSupportingVertex(vec)); + VectorUtil.setCoord(localAabbMax, i, VectorUtil.getCoord(tmp, i) + collisionMargin); + VectorUtil.setCoord(vec, i, -1f); + tmp.set(localGetSupportingVertex(vec)); + VectorUtil.setCoord(localAabbMin, i, VectorUtil.getCoord(tmp, i) - collisionMargin); + } + } + finally { + stack.vectors.pop(); + } + } + + public abstract int getNumVertices(); + + public abstract int getNumEdges(); + + public abstract void getEdge(int i, Vector3f pa, Vector3f pb); + + public abstract void getVertex(int i, Vector3f vtx); + + public abstract int getNumPlanes(); + + public abstract void getPlane(Vector3f planeNormal, Vector3f planeSupport, int i); + +// public abstract int getIndex(int i) const = 0 ; + + public abstract boolean isInside(Vector3f pt, float tolerance); + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/QuantizedBvhNodes.java b/src/jbullet/src/javabullet/collision/shapes/QuantizedBvhNodes.java new file mode 100644 index 0000000..38466d3 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/QuantizedBvhNodes.java @@ -0,0 +1,211 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +/** + * QuantizedBvhNodes is array of compressed AABB nodes, each of 16 bytes. + * Node can be used for leafnode or internal node. Leafnodes can point to 32-bit + * triangle index (non-negative range).<p> + * + * <i>Implementation note:</i> the nodes are internally stored in int[] array + * and bit packed. The actual structure is: + * + * <pre> + * unsigned short quantizedAabbMin[3] + * unsigned short quantizedAabbMax[3] + * signed int escapeIndexOrTriangleIndex + * </pre> + * + * @author jezek2 + */ +public class QuantizedBvhNodes { + + private static final int STRIDE = 4; // 16 bytes + + private int[] buf; + private int size = 0; + + public QuantizedBvhNodes() { + resize(16); + } + + public int add() { + while (size+1 >= capacity()) { + resize(capacity()*2); + } + return size++; + } + + public int size() { + return size; + } + + public int capacity() { + return buf.length / STRIDE; + } + + public void clear() { + size = 0; + } + + public void resize(int num) { + int[] oldBuf = buf; + + buf = new int[num*STRIDE]; + if (oldBuf != null) { + System.arraycopy(oldBuf, 0, buf, 0, Math.min(oldBuf.length, buf.length)); + } + } + + public static int getNodeSize() { + return STRIDE*4; + } + + public void set(int destId, QuantizedBvhNodes srcNodes, int srcId) { + assert (STRIDE == 4); + + // save field access: + int[] buf = this.buf; + int[] srcBuf = srcNodes.buf; + + buf[destId*STRIDE+0] = srcBuf[srcId*STRIDE+0]; + buf[destId*STRIDE+1] = srcBuf[srcId*STRIDE+1]; + buf[destId*STRIDE+2] = srcBuf[srcId*STRIDE+2]; + buf[destId*STRIDE+3] = srcBuf[srcId*STRIDE+3]; + } + + public void swap(int id1, int id2) { + assert (STRIDE == 4); + + // save field access: + int[] buf = this.buf; + + int temp0 = buf[id1*STRIDE+0]; + int temp1 = buf[id1*STRIDE+1]; + int temp2 = buf[id1*STRIDE+2]; + int temp3 = buf[id1*STRIDE+3]; + + buf[id1*STRIDE+0] = buf[id2*STRIDE+0]; + buf[id1*STRIDE+1] = buf[id2*STRIDE+1]; + buf[id1*STRIDE+2] = buf[id2*STRIDE+2]; + buf[id1*STRIDE+3] = buf[id2*STRIDE+3]; + + buf[id2*STRIDE+0] = temp0; + buf[id2*STRIDE+1] = temp1; + buf[id2*STRIDE+2] = temp2; + buf[id2*STRIDE+3] = temp3; + } + + public int getQuantizedAabbMin(int nodeId, int index) { + switch (index) { + default: + case 0: return (buf[nodeId*STRIDE+0]) & 0xFFFF; + case 1: return (buf[nodeId*STRIDE+0] >>> 16) & 0xFFFF; + case 2: return (buf[nodeId*STRIDE+1]) & 0xFFFF; + } + } + + public long getQuantizedAabbMin(int nodeId) { + return (buf[nodeId*STRIDE+0] & 0xFFFFFFFFL) | ((buf[nodeId*STRIDE+1] & 0xFFFFL) << 32); + } + + public void setQuantizedAabbMin(int nodeId, long value) { + buf[nodeId*STRIDE+0] = (int)value; + setQuantizedAabbMin(nodeId, 2, (short)((value & 0xFFFF00000000L) >>> 32)); + } + + public void setQuantizedAabbMax(int nodeId, long value) { + setQuantizedAabbMax(nodeId, 0, (short)value); + buf[nodeId*STRIDE+2] = (int)(value >>> 16); + } + + public void setQuantizedAabbMin(int nodeId, int index, int value) { + switch (index) { + case 0: buf[nodeId*STRIDE+0] = (buf[nodeId*STRIDE+0] & 0xFFFF0000) | (value & 0xFFFF); break; + case 1: buf[nodeId*STRIDE+0] = (buf[nodeId*STRIDE+0] & 0x0000FFFF) | ((value & 0xFFFF) << 16); break; + case 2: buf[nodeId*STRIDE+1] = (buf[nodeId*STRIDE+1] & 0xFFFF0000) | (value & 0xFFFF); break; + } + } + + public int getQuantizedAabbMax(int nodeId, int index) { + switch (index) { + default: + case 0: return (buf[nodeId*STRIDE+1] >>> 16) & 0xFFFF; + case 1: return (buf[nodeId*STRIDE+2]) & 0xFFFF; + case 2: return (buf[nodeId*STRIDE+2] >>> 16) & 0xFFFF; + } + } + + public long getQuantizedAabbMax(int nodeId) { + return ((buf[nodeId*STRIDE+1] & 0xFFFF0000L) >>> 16) | ((buf[nodeId*STRIDE+2] & 0xFFFFFFFFL) << 16); + } + + public void setQuantizedAabbMax(int nodeId, int index, int value) { + switch (index) { + case 0: buf[nodeId*STRIDE+1] = (buf[nodeId*STRIDE+1] & 0x0000FFFF) | ((value & 0xFFFF) << 16); break; + case 1: buf[nodeId*STRIDE+2] = (buf[nodeId*STRIDE+2] & 0xFFFF0000) | (value & 0xFFFF); break; + case 2: buf[nodeId*STRIDE+2] = (buf[nodeId*STRIDE+2] & 0x0000FFFF) | ((value & 0xFFFF) << 16); break; + } + } + + public int getEscapeIndexOrTriangleIndex(int nodeId) { + return buf[nodeId*STRIDE+3]; + } + + public void setEscapeIndexOrTriangleIndex(int nodeId, int value) { + buf[nodeId*STRIDE+3] = value; + } + + public boolean isLeafNode(int nodeId) { + // skipindex is negative (internal node), triangleindex >=0 (leafnode) + return (getEscapeIndexOrTriangleIndex(nodeId) >= 0); + } + + public int getEscapeIndex(int nodeId) { + assert (!isLeafNode(nodeId)); + return -getEscapeIndexOrTriangleIndex(nodeId); + } + + public int getTriangleIndex(int nodeId) { + assert (isLeafNode(nodeId)); + // Get only the lower bits where the triangle index is stored + return (getEscapeIndexOrTriangleIndex(nodeId) & ~((~0) << (31 - OptimizedBvh.MAX_NUM_PARTS_IN_BITS))); + } + + public int getPartId(int nodeId) { + assert (isLeafNode(nodeId)); + // Get only the highest bits where the part index is stored + return (getEscapeIndexOrTriangleIndex(nodeId) >>> (31 - OptimizedBvh.MAX_NUM_PARTS_IN_BITS)); + } + + public static int getCoord(long vec, int index) { + switch (index) { + default: + case 0: return (int)((vec & 0x00000000FFFFL)) & 0xFFFF; + case 1: return (int)((vec & 0x0000FFFF0000L) >>> 16) & 0xFFFF; + case 2: return (int)((vec & 0xFFFF00000000L) >>> 32) & 0xFFFF; + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/ScalarType.java b/src/jbullet/src/javabullet/collision/shapes/ScalarType.java new file mode 100644 index 0000000..98bd078 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/ScalarType.java @@ -0,0 +1,36 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +/** + * + * @author jezek2 + */ +public enum ScalarType { + PHY_FLOAT, + PHY_DOUBLE, + PHY_INTEGER, + PHY_SHORT, + PHY_FIXEDPOINT88 +} diff --git a/src/jbullet/src/javabullet/collision/shapes/SphereShape.java b/src/jbullet/src/javabullet/collision/shapes/SphereShape.java new file mode 100644 index 0000000..2a5c780 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/SphereShape.java @@ -0,0 +1,101 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * SphereShape implements an implicit (getSupportingVertex) Sphere. + * + * @author jezek2 + */ +public class SphereShape extends ConvexInternalShape { + + public SphereShape(float radius) { + implicitShapeDimensions.x = radius; + } + + @Override + public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) { + return BulletGlobals.ZERO_VECTOR3; + } + + @Override + public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) { + for (int i = 0; i < numVectors; i++) { + supportVerticesOut[i].set(0f, 0f, 0f); + } + } + + @Override + public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) { + stack.vectors.push(); + try { + Vector3f center = t.origin; + Vector3f extent = stack.vectors.get(getMargin(), getMargin(), getMargin()); + aabbMin.sub(center, extent); + aabbMax.add(center, extent); + } + finally { + stack.vectors.pop(); + } + } + + @Override + public BroadphaseNativeType getShapeType() { + return BroadphaseNativeType.SPHERE_SHAPE_PROXYTYPE; + } + + @Override + public void calculateLocalInertia(float mass, Vector3f inertia) { + float elem = 0.4f * mass * getMargin() * getMargin(); + inertia.set(elem, elem, elem); + } + + @Override + public String getName() { + return "SPHERE"; + } + + public float getRadius() { + return implicitShapeDimensions.x * localScaling.x; + } + + @Override + public void setMargin(float margin) { + super.setMargin(margin); + } + + @Override + public float getMargin() { + // to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case + // this means, non-uniform scaling is not supported anymore + return getRadius(); + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/StaticPlaneShape.java b/src/jbullet/src/javabullet/collision/shapes/StaticPlaneShape.java new file mode 100644 index 0000000..173be00 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/StaticPlaneShape.java @@ -0,0 +1,161 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.linearmath.Transform; +import javabullet.linearmath.TransformUtil; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * StaticPlaneShape simulates an 'infinite' plane by dynamically reporting triangles approximated by intersection of the plane with the AABB. + * Assumed is that the other objects is not also infinite, so a reasonable sized AABB. + * + * @author jezek2 + */ +public class StaticPlaneShape extends ConcaveShape { + + protected final Vector3f localAabbMin = new Vector3f(); + protected final Vector3f localAabbMax = new Vector3f(); + + protected final Vector3f planeNormal = new Vector3f(); + protected float planeConstant; + protected final Vector3f localScaling = new Vector3f(0f, 0f, 0f); + + public StaticPlaneShape(Vector3f planeNormal, float planeConstant) { + this.planeNormal.set(planeNormal); + this.planeConstant = planeConstant; + } + + public Vector3f getPlaneNormal() { + return planeNormal; + } + + public float getPlaneConstant() { + return planeConstant; + } + + @Override + public void processAllTriangles(TriangleCallback callback, Vector3f aabbMin, Vector3f aabbMax) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + Vector3f halfExtents = stack.vectors.get(); + halfExtents.sub(aabbMax, aabbMin); + halfExtents.scale(0.5f); + + float radius = halfExtents.length(); + Vector3f center = stack.vectors.get(); + center.add(aabbMax, aabbMin); + center.scale(0.5f); + + // this is where the triangles are generated, given AABB and plane equation (normal/constant) + + Vector3f tangentDir0 = stack.vectors.get(), tangentDir1 = stack.vectors.get(); + + // tangentDir0/tangentDir1 can be precalculated + TransformUtil.planeSpace1(planeNormal, tangentDir0, tangentDir1); + + Vector3f supVertex0 = stack.vectors.get(), supVertex1 = stack.vectors.get(); + + Vector3f projectedCenter = stack.vectors.get(); + tmp.scale(planeNormal.dot(center) - planeConstant, planeNormal); + projectedCenter.sub(center, tmp); + + Vector3f[] triangle = new Vector3f[] { stack.vectors.get(), stack.vectors.get(), stack.vectors.get() }; + + tmp1.scale(radius, tangentDir0); + tmp2.scale(radius, tangentDir1); + VectorUtil.add(triangle[0], projectedCenter, tmp1, tmp2); + + tmp1.scale(radius, tangentDir0); + tmp2.scale(radius, tangentDir1); + tmp.sub(tmp1, tmp2); + VectorUtil.add(triangle[1], projectedCenter, tmp); + + tmp1.scale(radius, tangentDir0); + tmp2.scale(radius, tangentDir1); + tmp.sub(tmp1, tmp2); + triangle[2].sub(projectedCenter, tmp); + + callback.processTriangle(triangle, 0, 0); + + tmp1.scale(radius, tangentDir0); + tmp2.scale(radius, tangentDir1); + tmp.sub(tmp1, tmp2); + triangle[0].sub(projectedCenter, tmp); + + tmp1.scale(radius, tangentDir0); + tmp2.scale(radius, tangentDir1); + tmp.add(tmp1, tmp2); + triangle[1].sub(projectedCenter, tmp); + + tmp1.scale(radius, tangentDir0); + tmp2.scale(radius, tangentDir1); + VectorUtil.add(triangle[2], projectedCenter, tmp1, tmp2); + + callback.processTriangle(triangle, 0, 1); + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) { + aabbMin.set(-1e30f, -1e30f, -1e30f); + aabbMax.set(1e30f, 1e30f, 1e30f); + } + + @Override + public BroadphaseNativeType getShapeType() { + return BroadphaseNativeType.STATIC_PLANE_PROXYTYPE; + } + + @Override + public void setLocalScaling(Vector3f scaling) { + localScaling.set(scaling); + } + + @Override + public Vector3f getLocalScaling() { + return localScaling; + } + + @Override + public void calculateLocalInertia(float mass, Vector3f inertia) { + //moving concave objects not supported + inertia.set(0f, 0f, 0f); + } + + @Override + public String getName() { + return "STATICPLANE"; + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/StridingMeshInterface.java b/src/jbullet/src/javabullet/collision/shapes/StridingMeshInterface.java new file mode 100644 index 0000000..70ee487 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/StridingMeshInterface.java @@ -0,0 +1,200 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * StridingMeshInterface is the interface class for high performance access to triangle meshes. + * It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory. + * + * @author jezek2 + */ +public abstract class StridingMeshInterface { + + protected final Vector3f scaling = new Vector3f(1f, 1f, 1f); + + private VertexData data = new VertexData(); + + public void internalProcessAllTriangles(InternalTriangleIndexCallback callback, Vector3f aabbMin, Vector3f aabbMax) { + int numtotalphysicsverts = 0; + int part, graphicssubparts = getNumSubParts(); + int gfxindex; + Vector3f[] triangle/*[3]*/ = new Vector3f[]{new Vector3f(), new Vector3f(), new Vector3f()}; + int graphicsbase; + + Vector3f meshScaling = new Vector3f(getScaling()); + + // if the number of parts is big, the performance might drop due to the innerloop switch on indextype + for (part = 0; part < graphicssubparts; part++) { + getLockedReadOnlyVertexIndexBase(data, part); + numtotalphysicsverts += data.numfaces * 3; // upper bound + + switch (data.indicestype) { + case PHY_INTEGER: { + for (gfxindex = 0; gfxindex < data.numfaces; gfxindex++) { + //int* tri_indices= (int*)(indexbase+gfxindex*indexstride); + int tri_indices = gfxindex * data.indexstride; + + //graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride); + graphicsbase = data.indexbase.getInt(tri_indices + 0) * data.stride; + + //triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + triangle[0].set( + data.vertexbase.getFloat(graphicsbase + 0) * meshScaling.x, + data.vertexbase.getFloat(graphicsbase + 4) * meshScaling.y, + data.vertexbase.getFloat(graphicsbase + 8) * meshScaling.z); + + //graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride); + graphicsbase = data.indexbase.getInt(tri_indices + 4) * data.stride; + + //triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + triangle[1].set( + data.vertexbase.getFloat(graphicsbase + 0) * meshScaling.x, + data.vertexbase.getFloat(graphicsbase + 4) * meshScaling.y, + data.vertexbase.getFloat(graphicsbase + 8) * meshScaling.z); + + //graphicsbase = (btScalar*)(vertexbase+tri_indices[2]*stride); + graphicsbase = data.indexbase.getInt(tri_indices + 8) * data.stride; + + //triangle[2].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + triangle[2].set( + data.vertexbase.getFloat(graphicsbase + 0) * meshScaling.x, + data.vertexbase.getFloat(graphicsbase + 4) * meshScaling.y, + data.vertexbase.getFloat(graphicsbase + 8) * meshScaling.z); + + callback.internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + case PHY_SHORT: { + for (gfxindex = 0; gfxindex < data.numfaces; gfxindex++) { + //short int* tri_indices= (short int*)(indexbase+gfxindex*indexstride); + int tri_indices = gfxindex * data.indexstride; + + //graphicsbase = (btScalar*)(vertexbase+tri_indices[0]*stride); + graphicsbase = (data.indexbase.getShort(tri_indices + 0) & 0xFFFF) * data.stride; + + //triangle[0].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + triangle[0].set( + data.vertexbase.getFloat(graphicsbase + 0) * meshScaling.x, + data.vertexbase.getFloat(graphicsbase + 4) * meshScaling.y, + data.vertexbase.getFloat(graphicsbase + 8) * meshScaling.z); + + //graphicsbase = (btScalar*)(vertexbase+tri_indices[1]*stride); + graphicsbase = (data.indexbase.getShort(tri_indices + 2) & 0xFFFF) * data.stride; + + //triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + triangle[1].set( + data.vertexbase.getFloat(graphicsbase + 0) * meshScaling.x, + data.vertexbase.getFloat(graphicsbase + 4) * meshScaling.y, + data.vertexbase.getFloat(graphicsbase + 8) * meshScaling.z); + + //graphicsbase = (btScalar*)(vertexbase+tri_indices[2]*stride); + graphicsbase = (data.indexbase.getShort(tri_indices + 4) & 0xFFFF) * data.stride; + + //triangle[1].setValue(graphicsbase[0]*meshScaling.getX(),graphicsbase[1]*meshScaling.getY(),graphicsbase[2]*meshScaling.getZ()); + triangle[2].set( + data.vertexbase.getFloat(graphicsbase + 0) * meshScaling.x, + data.vertexbase.getFloat(graphicsbase + 4) * meshScaling.y, + data.vertexbase.getFloat(graphicsbase + 8) * meshScaling.z); + + callback.internalProcessTriangleIndex(triangle, part, gfxindex); + } + break; + } + default: + assert ((data.indicestype == ScalarType.PHY_INTEGER) || (data.indicestype == ScalarType.PHY_SHORT)); + } + + unLockReadOnlyVertexBase(part); + } + + data.unref(); + } + + private static class AabbCalculationCallback implements InternalTriangleIndexCallback { + public final Vector3f aabbMin = new Vector3f(1e30f, 1e30f, 1e30f); + public final Vector3f aabbMax = new Vector3f(-1e30f, -1e30f, -1e30f); + + public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) { + VectorUtil.setMin(aabbMin, triangle[0]); + VectorUtil.setMax(aabbMax, triangle[0]); + VectorUtil.setMin(aabbMin, triangle[1]); + VectorUtil.setMax(aabbMax, triangle[1]); + VectorUtil.setMin(aabbMin, triangle[2]); + VectorUtil.setMax(aabbMax, triangle[2]); + } + } + + public void calculateAabbBruteForce(Vector3f aabbMin, Vector3f aabbMax) { + // first calculate the total aabb for all triangles + AabbCalculationCallback aabbCallback = new AabbCalculationCallback(); + aabbMin.set(-1e30f, -1e30f, -1e30f); + aabbMax.set(1e30f, 1e30f, 1e30f); + internalProcessAllTriangles(aabbCallback, aabbMin, aabbMax); + + aabbMin.set(aabbCallback.aabbMin); + aabbMax.set(aabbCallback.aabbMax); + } + + /** + * Get read and write access to a subpart of a triangle mesh. + * This subpart has a continuous array of vertices and indices. + * In this way the mesh can be handled as chunks of memory with striding + * very similar to OpenGL vertexarray support. + * Make a call to unLockVertexBase when the read and write access is finished. + */ + public abstract void getLockedVertexIndexBase(VertexData data, int subpart/*=0*/); + + public abstract void getLockedReadOnlyVertexIndexBase(VertexData data, int subpart/*=0*/); + + /** + * unLockVertexBase finishes the access to a subpart of the triangle mesh. + * Make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished. + */ + public abstract void unLockVertexBase(int subpart); + + public abstract void unLockReadOnlyVertexBase(int subpart); + + /** + * getNumSubParts returns the number of seperate subparts. + * Each subpart has a continuous array of vertices and indices. + */ + public abstract int getNumSubParts(); + + public abstract void preallocateVertices(int numverts); + public abstract void preallocateIndices(int numindices); + + public Vector3f getScaling() { + return scaling; + } + + public void setScaling(Vector3f scaling) + { + this.scaling.set(scaling); + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/TraversalMode.java b/src/jbullet/src/javabullet/collision/shapes/TraversalMode.java new file mode 100644 index 0000000..7558205 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/TraversalMode.java @@ -0,0 +1,34 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +/** + * + * @author jezek2 + */ +public enum TraversalMode { + TRAVERSAL_STACKLESS, + TRAVERSAL_STACKLESS_CACHE_FRIENDLY, + TRAVERSAL_RECURSIVE +} diff --git a/src/jbullet/src/javabullet/collision/shapes/TriangleCallback.java b/src/jbullet/src/javabullet/collision/shapes/TriangleCallback.java new file mode 100644 index 0000000..39ab89f --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/TriangleCallback.java @@ -0,0 +1,36 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public interface TriangleCallback { + + public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex); + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/TriangleIndexVertexArray.java b/src/jbullet/src/javabullet/collision/shapes/TriangleIndexVertexArray.java new file mode 100644 index 0000000..7f4dd16 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/TriangleIndexVertexArray.java @@ -0,0 +1,140 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import java.nio.ByteBuffer; +import java.util.ArrayList; +import java.util.List; + +/** + * + * @author jezek2 + */ +public class TriangleIndexVertexArray extends StridingMeshInterface { + + private List<IndexedMesh> indexedMeshes = new ArrayList<IndexedMesh>(); + + public TriangleIndexVertexArray() { + } + + /** + * Just to be backwards compatible. + */ + public TriangleIndexVertexArray(int numTriangles, ByteBuffer triangleIndexBase, int triangleIndexStride, int numVertices, ByteBuffer vertexBase, int vertexStride) { + IndexedMesh mesh = new IndexedMesh(); + + mesh.numTriangles = numTriangles; + mesh.triangleIndexBase = triangleIndexBase; + mesh.triangleIndexStride = triangleIndexStride; + mesh.numVertices = numVertices; + mesh.vertexBase = vertexBase; + mesh.vertexStride = vertexStride; + + addIndexedMesh(mesh); + } + + public void addIndexedMesh(IndexedMesh mesh) { + addIndexedMesh(mesh, ScalarType.PHY_INTEGER); + } + + public void addIndexedMesh(IndexedMesh mesh, ScalarType indexType) { + indexedMeshes.add(mesh); + indexedMeshes.get(indexedMeshes.size() - 1).indexType = indexType; + } + + @Override + public void getLockedVertexIndexBase(VertexData data, int subpart) { + assert (subpart < getNumSubParts()); + + IndexedMesh mesh = indexedMeshes.get(subpart); + + data.numverts = mesh.numVertices; + data.vertexbase = mesh.vertexBase; + //#ifdef BT_USE_DOUBLE_PRECISION + //type = PHY_DOUBLE; + //#else + data.type = ScalarType.PHY_FLOAT; + //#endif + data.stride = mesh.vertexStride; + + data.numfaces = mesh.numTriangles; + + data.indexbase = mesh.triangleIndexBase; + data.indexstride = mesh.triangleIndexStride; + data.indicestype = mesh.indexType; + } + + @Override + public void getLockedReadOnlyVertexIndexBase(VertexData data, int subpart) { + IndexedMesh mesh = indexedMeshes.get(subpart); + + data.numverts = mesh.numVertices; + data.vertexbase = mesh.vertexBase; + //#ifdef BT_USE_DOUBLE_PRECISION + //type = PHY_DOUBLE; + //#else + data.type = ScalarType.PHY_FLOAT; + //#endif + data.stride = mesh.vertexStride; + + data.numfaces = mesh.numTriangles; + data.indexbase = mesh.triangleIndexBase; + data.indexstride = mesh.triangleIndexStride; + data.indicestype = mesh.indexType; + } + + /** + * unLockVertexBase finishes the access to a subpart of the triangle mesh. + * Make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished. + */ + @Override + public void unLockVertexBase(int subpart) { + } + + @Override + public void unLockReadOnlyVertexBase(int subpart) { + } + + /** + * getNumSubParts returns the number of seperate subparts. + * Each subpart has a continuous array of vertices and indices. + */ + @Override + public int getNumSubParts() { + return indexedMeshes.size(); + } + + public List<IndexedMesh> getIndexedMeshArray() { + return indexedMeshes; + } + + @Override + public void preallocateVertices(int numverts) { + } + + @Override + public void preallocateIndices(int numindices) { + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/TriangleMeshShape.java b/src/jbullet/src/javabullet/collision/shapes/TriangleMeshShape.java new file mode 100644 index 0000000..3dddf2a --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/TriangleMeshShape.java @@ -0,0 +1,235 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.linearmath.AabbUtil2; +import javabullet.linearmath.MatrixUtil; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Matrix3f; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public abstract class TriangleMeshShape extends ConcaveShape { + + protected final Vector3f localAabbMin = new Vector3f(); + protected final Vector3f localAabbMax = new Vector3f(); + protected StridingMeshInterface meshInterface; + + /** + * TriangleMeshShape constructor has been disabled/protected, so that users will not mistakenly use this class. + * Don't use btTriangleMeshShape but use btBvhTriangleMeshShape instead! + */ + protected TriangleMeshShape(StridingMeshInterface meshInterface) { + this.meshInterface = meshInterface; + + // JAVA NOTE: moved to BvhTriangleMeshShape + //recalcLocalAabb(); + } + + public Vector3f localGetSupportingVertex(Vector3f vec) { + stack.pushCommonMath(); + try { + Vector3f tmp = stack.vectors.get(); + + Vector3f supportVertex = stack.vectors.get(); + + Transform ident = stack.transforms.get(); + ident.setIdentity(); + + SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident); + + Vector3f aabbMax = stack.vectors.get(1e30f, 1e30f, 1e30f); + tmp.negate(aabbMax); + + processAllTriangles(supportCallback, tmp, aabbMax); + + supportVertex.set(supportCallback.getSupportVertexLocal()); + + return stack.vectors.returning(supportVertex); + } + finally { + stack.popCommonMath(); + } + } + + public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec) { + assert (false); + return localGetSupportingVertex(vec); + } + + public void recalcLocalAabb() { + stack.vectors.push(); + try { + for (int i = 0; i < 3; i++) { + Vector3f vec = stack.vectors.get(0f, 0f, 0f); + VectorUtil.setCoord(vec, i, 1f); + Vector3f tmp = stack.vectors.get(localGetSupportingVertex(vec)); + VectorUtil.setCoord(localAabbMax, i, VectorUtil.getCoord(tmp, i) + collisionMargin); + VectorUtil.setCoord(vec, i, -1f); + tmp.set(localGetSupportingVertex(vec)); + VectorUtil.setCoord(localAabbMin, i, VectorUtil.getCoord(tmp, i) - collisionMargin); + } + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void getAabb(Transform trans, Vector3f aabbMin, Vector3f aabbMax) { + stack.pushCommonMath(); + try { + Vector3f tmp = stack.vectors.get(); + + Vector3f localHalfExtents = stack.vectors.get(); + localHalfExtents.sub(localAabbMax, localAabbMin); + localHalfExtents.scale(0.5f); + + Vector3f localCenter = stack.vectors.get(); + localCenter.add(localAabbMax, localAabbMin); + localCenter.scale(0.5f); + + Matrix3f abs_b = stack.matrices.get(trans.basis); + MatrixUtil.absolute(abs_b); + + Vector3f center = stack.vectors.get(localCenter); + trans.transform(center); + + Vector3f extent = stack.vectors.get(); + abs_b.getRow(0, tmp); + extent.x = tmp.dot(localHalfExtents); + abs_b.getRow(1, tmp); + extent.y = tmp.dot(localHalfExtents); + abs_b.getRow(2, tmp); + extent.z = tmp.dot(localHalfExtents); + + extent.add(stack.vectors.get(getMargin(), getMargin(), getMargin())); + + aabbMin.sub(center, extent); + aabbMax.add(center, extent); + } + finally { + stack.popCommonMath(); + } + } + + @Override + public void processAllTriangles(TriangleCallback callback, Vector3f aabbMin, Vector3f aabbMax) { + FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax); + + meshInterface.internalProcessAllTriangles(filterCallback, aabbMin, aabbMax); + } + + @Override + public void calculateLocalInertia(float mass, Vector3f inertia) { + // moving concave objects not supported + assert (false); + inertia.set(0f, 0f, 0f); + } + + + @Override + public void setLocalScaling(Vector3f scaling) { + meshInterface.setScaling(scaling); + recalcLocalAabb(); + } + + @Override + public Vector3f getLocalScaling() { + return meshInterface.getScaling(); + } + + public StridingMeshInterface getMeshInterface() { + return meshInterface; + } + + @Override + public String getName() { + return "TRIANGLEMESH"; + } + + //////////////////////////////////////////////////////////////////////////// + + private class SupportVertexCallback implements TriangleCallback { + private final Vector3f supportVertexLocal = new Vector3f(0f, 0f, 0f); + public final Transform worldTrans = new Transform(); + public float maxDot = -1e30f; + public final Vector3f supportVecLocal = new Vector3f(); + + public SupportVertexCallback(Vector3f supportVecWorld,Transform trans) { + this.worldTrans.set(trans); + MatrixUtil.transposeTransform(supportVecLocal, supportVecWorld, worldTrans.basis); + } + + public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) { + for (int i = 0; i < 3; i++) { + float dot = supportVecLocal.dot(triangle[i]); + if (dot > maxDot) { + maxDot = dot; + supportVertexLocal.set(triangle[i]); + } + } + } + + public Vector3f getSupportVertexWorldSpace() { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(supportVertexLocal); + worldTrans.transform(tmp); + return stack.vectors.returning(tmp); + } + finally { + stack.vectors.pop(); + } + } + + public Vector3f getSupportVertexLocal() { + return supportVertexLocal; + } + } + + private static class FilteredCallback implements InternalTriangleIndexCallback { + public TriangleCallback callback; + public final Vector3f aabbMin = new Vector3f(); + public final Vector3f aabbMax = new Vector3f(); + + public FilteredCallback(TriangleCallback callback, Vector3f aabbMin, Vector3f aabbMax) { + this.callback = callback; + this.aabbMin.set(aabbMin); + this.aabbMax.set(aabbMax); + } + + public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) { + if (AabbUtil2.testTriangleAgainstAabb2(triangle, aabbMin, aabbMax)) { + // check aabb in triangle-space, before doing this + callback.processTriangle(triangle, partId, triangleIndex); + } + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/TriangleShape.java b/src/jbullet/src/javabullet/collision/shapes/TriangleShape.java new file mode 100644 index 0000000..d5030ad --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/TriangleShape.java @@ -0,0 +1,216 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class TriangleShape extends PolyhedralConvexShape { + + public final Vector3f[] vertices1/*[3]*/ = new Vector3f[] { new Vector3f(), new Vector3f(), new Vector3f() }; + + // JAVA NOTE: added + public TriangleShape() { + } + + public TriangleShape(Vector3f p0, Vector3f p1, Vector3f p2) { + vertices1[0].set(p0); + vertices1[1].set(p1); + vertices1[2].set(p2); + } + + // JAVA NOTE: added + public void init(Vector3f p0, Vector3f p1, Vector3f p2) { + vertices1[0].set(p0); + vertices1[1].set(p1); + vertices1[2].set(p2); + } + + @Override + public int getNumVertices() { + return 3; + } + + public Vector3f getVertexPtr(int index) { + return vertices1[index]; + } + + @Override + public void getVertex(int index, Vector3f vert) { + vert.set(vertices1[index]); + } + + @Override + public BroadphaseNativeType getShapeType() { + return BroadphaseNativeType.TRIANGLE_SHAPE_PROXYTYPE; + } + + @Override + public int getNumEdges() { + return 3; + } + + @Override + public void getEdge(int i, Vector3f pa, Vector3f pb) { + getVertex(i, pa); + getVertex((i + 1) % 3, pb); + } + + @Override + public void getAabb(Transform t, Vector3f aabbMin, Vector3f aabbMax) { +// btAssert(0); + getAabbSlow(t, aabbMin, aabbMax); + } + + @Override + public Vector3f localGetSupportingVertexWithoutMargin(Vector3f dir) { + stack.vectors.push(); + try { + Vector3f dots = stack.vectors.get(dir.dot(vertices1[0]), dir.dot(vertices1[1]), dir.dot(vertices1[2])); + return vertices1[VectorUtil.maxAxis(dots)]; + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void batchedUnitVectorGetSupportingVertexWithoutMargin(Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) { + stack.vectors.push(); + try { + Vector3f dots = stack.vectors.get(); + + for (int i = 0; i < numVectors; i++) { + Vector3f dir = vectors[i]; + dots.set(dir.dot(vertices1[0]), dir.dot(vertices1[1]), dir.dot(vertices1[2])); + supportVerticesOut[i].set(vertices1[VectorUtil.maxAxis(dots)]); + } + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void getPlane(Vector3f planeNormal, Vector3f planeSupport, int i) { + getPlaneEquation(i,planeNormal,planeSupport); + } + + @Override + public int getNumPlanes() { + return 1; + } + + public void calcNormal(Vector3f normal) { + stack.vectors.push(); + try { + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + tmp1.sub(vertices1[1], vertices1[0]); + tmp2.sub(vertices1[2], vertices1[0]); + + normal.cross(tmp1, tmp2); + normal.normalize(); + } + finally { + stack.vectors.pop(); + } + } + + public void getPlaneEquation(int i, Vector3f planeNormal, Vector3f planeSupport) { + calcNormal(planeNormal); + planeSupport.set(vertices1[0]); + } + + @Override + public void calculateLocalInertia(float mass, Vector3f inertia) { + assert (false); + inertia.set(0f, 0f, 0f); + } + + @Override + public boolean isInside(Vector3f pt, float tolerance) { + stack.vectors.push(); + try { + Vector3f normal = stack.vectors.get(); + calcNormal(normal); + // distance to plane + float dist = pt.dot(normal); + float planeconst = vertices1[0].dot(normal); + dist -= planeconst; + if (dist >= -tolerance && dist <= tolerance) { + // inside check on edge-planes + int i; + for (i = 0; i < 3; i++) { + Vector3f pa = stack.vectors.get(), pb = stack.vectors.get(); + getEdge(i, pa, pb); + Vector3f edge = stack.vectors.get(); + edge.sub(pb, pa); + Vector3f edgeNormal = stack.vectors.get(); + edgeNormal.cross(edge, normal); + edgeNormal.normalize(); + /*float*/ dist = pt.dot(edgeNormal); + float edgeConst = pa.dot(edgeNormal); + dist -= edgeConst; + if (dist < -tolerance) { + return false; + } + } + + return true; + } + + return false; + } + finally { + stack.vectors.pop(); + } + } + + @Override + public String getName() { + return "Triangle"; + } + + @Override + public int getNumPreferredPenetrationDirections() { + return 2; + } + + @Override + public void getPreferredPenetrationDirection(int index, Vector3f penetrationVector) { + calcNormal(penetrationVector); + if (index != 0) { + penetrationVector.scale(-1f); + } + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/VertexData.java b/src/jbullet/src/javabullet/collision/shapes/VertexData.java new file mode 100644 index 0000000..b88350e --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/VertexData.java @@ -0,0 +1,52 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.collision.shapes; + +import java.nio.ByteBuffer; + +/** + * Represents information for accessing vertex data. + * + * @author jezek2 + */ +public class VertexData { + + public ByteBuffer vertexbase; + public int numverts; + public ScalarType type; + public int stride; + public ByteBuffer indexbase; + public int indexstride; + public int numfaces; + public ScalarType indicestype; + + /** + * Unreferences data buffers to avoid memory leaks. + */ + public void unref() { + vertexbase = null; + indexbase = null; + } + +} diff --git a/src/jbullet/src/javabullet/collision/shapes/package-info.java b/src/jbullet/src/javabullet/collision/shapes/package-info.java new file mode 100644 index 0000000..9705359 --- /dev/null +++ b/src/jbullet/src/javabullet/collision/shapes/package-info.java @@ -0,0 +1,28 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +/** + * Collision shapes. + */ +package javabullet.collision.shapes; + diff --git a/src/jbullet/src/javabullet/demos/genericjoint/GenericJointDemo.java b/src/jbullet/src/javabullet/demos/genericjoint/GenericJointDemo.java new file mode 100644 index 0000000..bf786c1 --- /dev/null +++ b/src/jbullet/src/javabullet/demos/genericjoint/GenericJointDemo.java @@ -0,0 +1,123 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Ragdoll Demo + * Copyright (c) 2007 Starbreeze Studios + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + * + * Originally Written by: Marten Svanfeldt + * ReWritten by: Francisco Le�n + */ + +package javabullet.demos.genericjoint; + +import java.util.ArrayList; +import java.util.List; +import javabullet.collision.broadphase.BroadphaseInterface; +import javabullet.collision.broadphase.SimpleBroadphase; +import javabullet.collision.dispatch.CollisionDispatcher; +import javabullet.collision.dispatch.DefaultCollisionConfiguration; +import javabullet.collision.shapes.BoxShape; +import javabullet.collision.shapes.CollisionShape; +import javabullet.demos.opengl.DemoApplication; +import javabullet.demos.opengl.JOGL; +import javabullet.dynamics.DiscreteDynamicsWorld; +import javabullet.dynamics.constraintsolver.ConstraintSolver; +import javabullet.dynamics.constraintsolver.SequentialImpulseConstraintSolver; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; +import javax.media.opengl.*; + +/** + * + * @author jezek2 + */ +public class GenericJointDemo extends DemoApplication { + + private List<RagDoll> ragdolls = new ArrayList<RagDoll>(); + + public GenericJointDemo(String[] args) { + super(args); + } + + public void initPhysics() { + // Setup the basic world + DefaultCollisionConfiguration collision_config = new DefaultCollisionConfiguration(); + + CollisionDispatcher dispatcher = new CollisionDispatcher(collision_config); + + //btPoint3 worldAabbMin(-10000,-10000,-10000); + //btPoint3 worldAabbMax(10000,10000,10000); + //btBroadphaseInterface* overlappingPairCache = new btAxisSweep3 (worldAabbMin, worldAabbMax); + BroadphaseInterface overlappingPairCache = new SimpleBroadphase(); + + //#ifdef USE_ODE_QUICKSTEP + //btConstraintSolver* constraintSolver = new OdeConstraintSolver(); + //#else + ConstraintSolver constraintSolver = new SequentialImpulseConstraintSolver(); + //#endif + + dynamicsWorld = new DiscreteDynamicsWorld(dispatcher, overlappingPairCache, constraintSolver, collision_config); + + dynamicsWorld.setGravity(new Vector3f(0f, -30f, 0f)); + + // Setup a big ground box + { + CollisionShape groundShape = new BoxShape(new Vector3f(200f, 10f, 200f)); + Transform groundTransform = new Transform(); + groundTransform.setIdentity(); + groundTransform.origin.set(0f, -15f, 0f); + localCreateRigidBody(0f, groundTransform, groundShape); + } + + // Spawn one ragdoll + spawnRagdoll(); + + clientResetScene(); + } + + public void spawnRagdoll() { + spawnRagdoll(false); + } + + public void spawnRagdoll(boolean random) { + RagDoll ragDoll = new RagDoll(dynamicsWorld, new Vector3f(0f, 0f, 10f), 5f); + ragdolls.add(ragDoll); + } + + @Override + public void keyboardCallback(char key) { + switch (key) { + case 'e': + spawnRagdoll(true); + break; + default: + super.keyboardCallback(key); + } + } + + public static void main(String[] args) { + GenericJointDemo demoApp = new GenericJointDemo(args); + demoApp.initPhysics(); + demoApp.setCameraDistance(10f); + + JOGL.main("Joint 6DOF - Sequencial Impulse Solver", demoApp, args); + } + +} diff --git a/src/jbullet/src/javabullet/demos/genericjoint/RagDoll.java b/src/jbullet/src/javabullet/demos/genericjoint/RagDoll.java new file mode 100644 index 0000000..20ec000 --- /dev/null +++ b/src/jbullet/src/javabullet/demos/genericjoint/RagDoll.java @@ -0,0 +1,474 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Ragdoll Demo + * Copyright (c) 2007 Starbreeze Studios + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + * + * Written by: Marten Svanfeldt + */ + +package javabullet.demos.genericjoint; + +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javabullet.collision.shapes.CapsuleShape; +import javabullet.collision.shapes.CollisionShape; +import javabullet.dynamics.DynamicsWorld; +import javabullet.dynamics.RigidBody; +import javabullet.dynamics.RigidBodyConstructionInfo; +import javabullet.dynamics.constraintsolver.Generic6DofConstraint; +import javabullet.dynamics.constraintsolver.TypedConstraint; +import javabullet.linearmath.DefaultMotionState; +import javabullet.linearmath.MatrixUtil; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class RagDoll { + + protected final BulletStack stack = BulletStack.get(); + + public enum BodyPart { + BODYPART_PELVIS, + BODYPART_SPINE, + BODYPART_HEAD, + + BODYPART_LEFT_UPPER_LEG, + BODYPART_LEFT_LOWER_LEG, + + BODYPART_RIGHT_UPPER_LEG, + BODYPART_RIGHT_LOWER_LEG, + + BODYPART_LEFT_UPPER_ARM, + BODYPART_LEFT_LOWER_ARM, + + BODYPART_RIGHT_UPPER_ARM, + BODYPART_RIGHT_LOWER_ARM, + + BODYPART_COUNT; + } + + public enum JointType { + JOINT_PELVIS_SPINE, + JOINT_SPINE_HEAD, + + JOINT_LEFT_HIP, + JOINT_LEFT_KNEE, + + JOINT_RIGHT_HIP, + JOINT_RIGHT_KNEE, + + JOINT_LEFT_SHOULDER, + JOINT_LEFT_ELBOW, + + JOINT_RIGHT_SHOULDER, + JOINT_RIGHT_ELBOW, + + JOINT_COUNT + } + + private DynamicsWorld ownerWorld; + private CollisionShape[] shapes = new CollisionShape[BodyPart.BODYPART_COUNT.ordinal()]; + private RigidBody[] bodies = new RigidBody[BodyPart.BODYPART_COUNT.ordinal()]; + private TypedConstraint[] joints = new TypedConstraint[JointType.JOINT_COUNT.ordinal()]; + + public RagDoll(DynamicsWorld ownerWorld, Vector3f positionOffset) { + this(ownerWorld, positionOffset, 1.0f); + } + + public RagDoll(DynamicsWorld ownerWorld, Vector3f positionOffset, float scale_ragdoll) { + this.ownerWorld = ownerWorld; + + stack.pushCommonMath(); + try { + Transform tmpTrans = stack.transforms.get(); + + // Setup the geometry + shapes[BodyPart.BODYPART_PELVIS.ordinal()] = new CapsuleShape(scale_ragdoll * 0.15f, scale_ragdoll * 0.20f); + shapes[BodyPart.BODYPART_SPINE.ordinal()] = new CapsuleShape(scale_ragdoll * 0.15f, scale_ragdoll * 0.28f); + shapes[BodyPart.BODYPART_HEAD.ordinal()] = new CapsuleShape(scale_ragdoll * 0.10f, scale_ragdoll * 0.05f); + shapes[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()] = new CapsuleShape(scale_ragdoll * 0.07f, scale_ragdoll * 0.45f); + shapes[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()] = new CapsuleShape(scale_ragdoll * 0.05f, scale_ragdoll * 0.37f); + shapes[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()] = new CapsuleShape(scale_ragdoll * 0.07f, scale_ragdoll * 0.45f); + shapes[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()] = new CapsuleShape(scale_ragdoll * 0.05f, scale_ragdoll * 0.37f); + shapes[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()] = new CapsuleShape(scale_ragdoll * 0.05f, scale_ragdoll * 0.33f); + shapes[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()] = new CapsuleShape(scale_ragdoll * 0.04f, scale_ragdoll * 0.25f); + shapes[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()] = new CapsuleShape(scale_ragdoll * 0.05f, scale_ragdoll * 0.33f); + shapes[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()] = new CapsuleShape(scale_ragdoll * 0.04f, scale_ragdoll * 0.25f); + + // Setup all the rigid bodies + Transform offset = stack.transforms.get(); + offset.setIdentity(); + offset.origin.set(positionOffset); + + Transform transform = stack.transforms.get(); + transform.setIdentity(); + transform.origin.set(0f, scale_ragdoll * 1f, 0f); + tmpTrans.mul(offset, transform); + bodies[BodyPart.BODYPART_PELVIS.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_PELVIS.ordinal()]); + + transform.setIdentity(); + transform.origin.set(0f, scale_ragdoll * 1.2f, 0f); + tmpTrans.mul(offset, transform); + bodies[BodyPart.BODYPART_SPINE.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_SPINE.ordinal()]); + + transform.setIdentity(); + transform.origin.set(0f, scale_ragdoll * 1.6f, 0f); + tmpTrans.mul(offset, transform); + bodies[BodyPart.BODYPART_HEAD.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_HEAD.ordinal()]); + + transform.setIdentity(); + transform.origin.set(-0.18f * scale_ragdoll, 0.65f * scale_ragdoll, 0f); + tmpTrans.mul(offset, transform); + bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()]); + + transform.setIdentity(); + transform.origin.set(-0.18f * scale_ragdoll, 0.2f * scale_ragdoll, 0f); + tmpTrans.mul(offset, transform); + bodies[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()]); + + transform.setIdentity(); + transform.origin.set(0.18f * scale_ragdoll, 0.65f * scale_ragdoll, 0f); + tmpTrans.mul(offset, transform); + bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()]); + + transform.setIdentity(); + transform.origin.set(0.18f * scale_ragdoll, 0.2f * scale_ragdoll, 0f); + tmpTrans.mul(offset, transform); + bodies[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()]); + + transform.setIdentity(); + transform.origin.set(-0.35f * scale_ragdoll, 1.45f * scale_ragdoll, 0f); + MatrixUtil.setEulerZYX(transform.basis, 0, 0, BulletGlobals.SIMD_HALF_PI); + tmpTrans.mul(offset, transform); + bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()]); + + transform.setIdentity(); + transform.origin.set(-0.7f * scale_ragdoll, 1.45f * scale_ragdoll, 0f); + MatrixUtil.setEulerZYX(transform.basis, 0, 0, BulletGlobals.SIMD_HALF_PI); + tmpTrans.mul(offset, transform); + bodies[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()]); + + transform.setIdentity(); + transform.origin.set(0.35f * scale_ragdoll, 1.45f * scale_ragdoll, 0f); + MatrixUtil.setEulerZYX(transform.basis, 0, 0, -BulletGlobals.SIMD_HALF_PI); + tmpTrans.mul(offset, transform); + bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()]); + + transform.setIdentity(); + transform.origin.set(0.7f * scale_ragdoll, 1.45f * scale_ragdoll, 0f); + MatrixUtil.setEulerZYX(transform.basis, 0, 0, -BulletGlobals.SIMD_HALF_PI); + tmpTrans.mul(offset, transform); + bodies[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()] = localCreateRigidBody(1f, tmpTrans, shapes[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()]); + + // Setup some damping on the m_bodies + for (int i = 0; i < BodyPart.BODYPART_COUNT.ordinal(); ++i) { + bodies[i].setDamping(0.05f, 0.85f); + bodies[i].setDeactivationTime(0.8f); + bodies[i].setSleepingThresholds(1.6f, 2.5f); + } + + ///////////////////////////// SETTING THE CONSTRAINTS /////////////////////////////////////////////7777 + // Now setup the constraints + Generic6DofConstraint joint6DOF; + Transform localA = stack.transforms.get(), localB = stack.transforms.get(); + boolean useLinearReferenceFrameA = true; + /// ******* SPINE HEAD ******** /// + { + localA.setIdentity(); + localB.setIdentity(); + + localA.origin.set(0f, 0.30f * scale_ragdoll, 0f); + + localB.origin.set(0f, -0.14f * scale_ragdoll, 0f); + + joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_SPINE.ordinal()], bodies[BodyPart.BODYPART_HEAD.ordinal()], localA, localB, useLinearReferenceFrameA); + + //#ifdef RIGID + //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON)); + //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON)); + //#else + joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_PI * 0.3f, -BulletGlobals.FLT_EPSILON, -BulletGlobals.SIMD_PI * 0.3f)); + joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.5f, BulletGlobals.FLT_EPSILON, BulletGlobals.SIMD_PI * 0.3f)); + //#endif + joints[JointType.JOINT_SPINE_HEAD.ordinal()] = joint6DOF; + ownerWorld.addConstraint(joints[JointType.JOINT_SPINE_HEAD.ordinal()], true); + } + /// *************************** /// + + /// ******* LEFT SHOULDER ******** /// + { + localA.setIdentity(); + localB.setIdentity(); + + localA.origin.set(-0.2f * scale_ragdoll, 0.15f * scale_ragdoll, 0f); + + MatrixUtil.setEulerZYX(localB.basis, BulletGlobals.SIMD_HALF_PI, 0, -BulletGlobals.SIMD_HALF_PI); + localB.origin.set(0f, -0.18f * scale_ragdoll, 0f); + + joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_SPINE.ordinal()], bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA); + + //#ifdef RIGID + //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON)); + //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON)); + //#else + joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_PI * 0.8f, -BulletGlobals.FLT_EPSILON, -BulletGlobals.SIMD_PI * 0.5f)); + joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.8f, BulletGlobals.FLT_EPSILON, BulletGlobals.SIMD_PI * 0.5f)); + //#endif + joints[JointType.JOINT_LEFT_SHOULDER.ordinal()] = joint6DOF; + ownerWorld.addConstraint(joints[JointType.JOINT_LEFT_SHOULDER.ordinal()], true); + } + /// *************************** /// + + /// ******* RIGHT SHOULDER ******** /// + { + localA.setIdentity(); + localB.setIdentity(); + + localA.origin.set(0.2f * scale_ragdoll, 0.15f * scale_ragdoll, 0f); + MatrixUtil.setEulerZYX(localB.basis, 0, 0, BulletGlobals.SIMD_HALF_PI); + localB.origin.set(0f, -0.18f * scale_ragdoll, 0f); + joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_SPINE.ordinal()], bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA); + + //#ifdef RIGID + //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON)); + //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON)); + //#else + joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_PI * 0.8f, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_PI * 0.5f)); + joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.8f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_PI * 0.5f)); + //#endif + joints[JointType.JOINT_RIGHT_SHOULDER.ordinal()] = joint6DOF; + ownerWorld.addConstraint(joints[JointType.JOINT_RIGHT_SHOULDER.ordinal()], true); + } + /// *************************** /// + + /// ******* LEFT ELBOW ******** /// + { + localA.setIdentity(); + localB.setIdentity(); + + localA.origin.set(0f, 0.18f * scale_ragdoll, 0f); + localB.origin.set(0f, -0.14f * scale_ragdoll, 0f); + joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_LEFT_UPPER_ARM.ordinal()], bodies[BodyPart.BODYPART_LEFT_LOWER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA); + + //#ifdef RIGID + //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON)); + //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON)); + //#else + joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON)); + joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.7f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON)); + //#endif + joints[JointType.JOINT_LEFT_ELBOW.ordinal()] = joint6DOF; + ownerWorld.addConstraint(joints[JointType.JOINT_LEFT_ELBOW.ordinal()], true); + } + /// *************************** /// + + /// ******* RIGHT ELBOW ******** /// + { + localA.setIdentity(); + localB.setIdentity(); + + localA.origin.set(0f, 0.18f * scale_ragdoll, 0f); + localB.origin.set(0f, -0.14f * scale_ragdoll, 0f); + joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_RIGHT_UPPER_ARM.ordinal()], bodies[BodyPart.BODYPART_RIGHT_LOWER_ARM.ordinal()], localA, localB, useLinearReferenceFrameA); + + //#ifdef RIGID + //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON)); + //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON)); + //#else + joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON)); + joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.7f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON)); + //#endif + + joints[JointType.JOINT_RIGHT_ELBOW.ordinal()] = joint6DOF; + ownerWorld.addConstraint(joints[JointType.JOINT_RIGHT_ELBOW.ordinal()], true); + } + /// *************************** /// + + + /// ******* PELVIS ******** /// + { + localA.setIdentity(); + localB.setIdentity(); + + MatrixUtil.setEulerZYX(localA.basis, 0, BulletGlobals.SIMD_HALF_PI, 0); + localA.origin.set(0f, 0.15f * scale_ragdoll, 0f); + MatrixUtil.setEulerZYX(localB.basis, 0, BulletGlobals.SIMD_HALF_PI, 0); + localB.origin.set(0f, -0.15f * scale_ragdoll, 0f); + joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_PELVIS.ordinal()], bodies[BodyPart.BODYPART_SPINE.ordinal()], localA, localB, useLinearReferenceFrameA); + + //#ifdef RIGID + //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON)); + //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON)); + //#else + joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_PI * 0.2f, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_PI * 0.3f)); + joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.2f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_PI * 0.6f)); + //#endif + joints[JointType.JOINT_PELVIS_SPINE.ordinal()] = joint6DOF; + ownerWorld.addConstraint(joints[JointType.JOINT_PELVIS_SPINE.ordinal()], true); + } + /// *************************** /// + + /// ******* LEFT HIP ******** /// + { + localA.setIdentity(); + localB.setIdentity(); + + localA.origin.set(-0.18f * scale_ragdoll, -0.10f * scale_ragdoll, 0f); + + localB.origin.set(0f, 0.225f * scale_ragdoll, 0f); + + joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_PELVIS.ordinal()], bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA); + + //#ifdef RIGID + //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON)); + //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON)); + //#else + joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_HALF_PI * 0.5f, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON)); + joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_HALF_PI * 0.8f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_HALF_PI * 0.6f)); + //#endif + joints[JointType.JOINT_LEFT_HIP.ordinal()] = joint6DOF; + ownerWorld.addConstraint(joints[JointType.JOINT_LEFT_HIP.ordinal()], true); + } + /// *************************** /// + + + /// ******* RIGHT HIP ******** /// + { + localA.setIdentity(); + localB.setIdentity(); + + localA.origin.set(0.18f * scale_ragdoll, -0.10f * scale_ragdoll, 0f); + localB.origin.set(0f, 0.225f * scale_ragdoll, 0f); + + joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_PELVIS.ordinal()], bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA); + + //#ifdef RIGID + //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON)); + //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON)); + //#else + joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_HALF_PI * 0.5f, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_HALF_PI * 0.6f)); + joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_HALF_PI * 0.8f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON)); + //#endif + joints[JointType.JOINT_RIGHT_HIP.ordinal()] = joint6DOF; + ownerWorld.addConstraint(joints[JointType.JOINT_RIGHT_HIP.ordinal()], true); + } + /// *************************** /// + + + /// ******* LEFT KNEE ******** /// + { + localA.setIdentity(); + localB.setIdentity(); + + localA.origin.set(0f, -0.225f * scale_ragdoll, 0f); + localB.origin.set(0f, 0.185f * scale_ragdoll, 0f); + joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_LEFT_UPPER_LEG.ordinal()], bodies[BodyPart.BODYPART_LEFT_LOWER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA); + // + //#ifdef RIGID + //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON)); + //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON)); + //#else + joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON)); + joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.7f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON)); + //#endif + joints[JointType.JOINT_LEFT_KNEE.ordinal()] = joint6DOF; + ownerWorld.addConstraint(joints[JointType.JOINT_LEFT_KNEE.ordinal()], true); + } + /// *************************** /// + + /// ******* RIGHT KNEE ******** /// + { + localA.setIdentity(); + localB.setIdentity(); + + localA.origin.set(0f, -0.225f * scale_ragdoll, 0f); + localB.origin.set(0f, 0.185f * scale_ragdoll, 0f); + joint6DOF = new Generic6DofConstraint(bodies[BodyPart.BODYPART_RIGHT_UPPER_LEG.ordinal()], bodies[BodyPart.BODYPART_RIGHT_LOWER_LEG.ordinal()], localA, localB, useLinearReferenceFrameA); + + //#ifdef RIGID + //joint6DOF->setAngularLowerLimit(btVector3(-SIMD_EPSILON,-SIMD_EPSILON,-SIMD_EPSILON)); + //joint6DOF->setAngularUpperLimit(btVector3(SIMD_EPSILON,SIMD_EPSILON,SIMD_EPSILON)); + //#else + joint6DOF.setAngularLowerLimit(stack.vectors.get(-BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON, -BulletGlobals.SIMD_EPSILON)); + joint6DOF.setAngularUpperLimit(stack.vectors.get(BulletGlobals.SIMD_PI * 0.7f, BulletGlobals.SIMD_EPSILON, BulletGlobals.SIMD_EPSILON)); + //#endif + joints[JointType.JOINT_RIGHT_KNEE.ordinal()] = joint6DOF; + ownerWorld.addConstraint(joints[JointType.JOINT_RIGHT_KNEE.ordinal()], true); + } + /// *************************** /// + } + finally { + stack.popCommonMath(); + } + } + + public void destroy() { + int i; + + // Remove all constraints + for (i = 0; i < JointType.JOINT_COUNT.ordinal(); ++i) { + ownerWorld.removeConstraint(joints[i]); + //joints[i].destroy(); + joints[i] = null; + } + + // Remove all bodies and shapes + for (i = 0; i < BodyPart.BODYPART_COUNT.ordinal(); ++i) { + ownerWorld.removeRigidBody(bodies[i]); + + //bodies[i].getMotionState().destroy(); + + bodies[i].destroy(); + bodies[i] = null; + + //shapes[i].destroy(); + shapes[i] = null; + } + } + + private RigidBody localCreateRigidBody(float mass, Transform startTransform, CollisionShape shape) { + stack.vectors.push(); + try { + boolean isDynamic = (mass != 0f); + + Vector3f localInertia = stack.vectors.get(0f, 0f, 0f); + if (isDynamic) { + shape.calculateLocalInertia(mass, localInertia); + } + + DefaultMotionState myMotionState = new DefaultMotionState(startTransform); + RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia); + rbInfo.additionalDamping = true; + RigidBody body = new RigidBody(rbInfo); + + ownerWorld.addRigidBody(body); + + return body; + } + finally { + stack.vectors.pop(); + } + } + +} diff --git a/src/jbullet/src/javabullet/demos/opengl/DejaVu_Sans_11.fnt b/src/jbullet/src/javabullet/demos/opengl/DejaVu_Sans_11.fnt Binary files differnew file mode 100644 index 0000000..0abba78 --- /dev/null +++ b/src/jbullet/src/javabullet/demos/opengl/DejaVu_Sans_11.fnt diff --git a/src/jbullet/src/javabullet/demos/opengl/DemoApplication.java b/src/jbullet/src/javabullet/demos/opengl/DemoApplication.java new file mode 100644 index 0000000..18c0bae --- /dev/null +++ b/src/jbullet/src/javabullet/demos/opengl/DemoApplication.java @@ -0,0 +1,1203 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.demos.opengl; + +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javabullet.collision.dispatch.CollisionObject; +import javabullet.collision.dispatch.CollisionWorld; +import javabullet.collision.shapes.BoxShape; +import javabullet.collision.shapes.CollisionShape; +import javabullet.dynamics.DynamicsWorld; +import javabullet.dynamics.RigidBody; +import javabullet.dynamics.RigidBodyConstructionInfo; +import javabullet.dynamics.constraintsolver.Point2PointConstraint; +import javabullet.dynamics.constraintsolver.TypedConstraint; +import javabullet.linearmath.Clock; +import javabullet.linearmath.DebugDrawModes; +import javabullet.linearmath.DefaultMotionState; +import javabullet.linearmath.QuaternionUtil; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Color3f; +import javax.vecmath.Matrix3f; +import javax.vecmath.Quat4f; +import javax.vecmath.Vector3f; +import javax.media.opengl.*; +import javax.media.opengl.glu.*; +import java.nio.*; + +import com.sun.javafx.newt.*; + +/** + * + * @author jezek2 + */ +public abstract class DemoApplication + implements GLEventListener, MouseListener, KeyListener +{ + + protected final BulletStack stack = BulletStack.get(); + + private static final float STEPSIZE = 5; + + public static int numObjects = 0; + public static final int maxNumObjects = 16384; + public static Transform[] startTransforms = new Transform[maxNumObjects]; + public static CollisionShape[] gShapePtr = new CollisionShape[maxNumObjects]; //1 rigidbody has 1 shape (no re-use of shapes) + + public static RigidBody pickedBody = null; // for deactivation state + + static { + for (int i=0; i<startTransforms.length; i++) { + startTransforms[i] = new Transform(); + } + } + // TODO: class CProfileIterator* m_profileIterator; + + protected Clock clock = new Clock(); + + // this is the most important class + protected DynamicsWorld dynamicsWorld = null; + + // constraint for mouse picking + protected TypedConstraint pickConstraint = null; + + protected CollisionShape shootBoxShape = null; + + protected float cameraDistance = 15f; + protected int debugMode = 0; + + protected float ele = 20f; + protected float azi = 0f; + protected final Vector3f cameraPosition = new Vector3f(0f, 0f, 0f); + protected final Vector3f cameraTargetPosition = new Vector3f(0f, 0f, 0f); // look at + + protected float scaleBottom = 0.5f; + protected float scaleFactor = 2f; + protected final Vector3f cameraUp = new Vector3f(0f, 1f, 0f); + protected int forwardAxis = 2; + + protected int glutScreenWidth = 0; + protected int glutScreenHeight = 0; + + protected float ShootBoxInitialSpeed = 40f; + + protected boolean stepping = true; + protected boolean singleStep = false; + protected boolean idle = false; + protected int lastKey; + + protected GLU glu=null; + protected GLSRT glsrt=null; + + protected boolean useLight0 = true; + protected boolean useLight1 = true; + + public DemoApplication(String[] args) { + // debugMode |= DebugDrawModes.DRAW_WIREFRAME; + debugMode |= DebugDrawModes.NO_HELP_TEXT; + //debugMode |= DebugDrawModes.DISABLE_BULLET_LCP; + + for(int i=args.length-1; i>=0; i--) { + if(args[i].equals("-nolight0")) { + useLight0=false; + } else if(args[i].equals("-nolight1")) { + useLight1=false; + } + } + } + + public abstract void initPhysics() throws Exception; + + public void destroy() { + // TODO: CProfileManager::Release_Iterator(m_profileIterator); + //if (m_shootBoxShape) + // delete m_shootBoxShape; + } + + // + // GLEventListener + // + + public void init(GLAutoDrawable drawable) { + float[] light_ambient = new float[] { 0.2f, 0.2f, 0.2f, 1.0f }; + float[] light_diffuse = new float[] { 1.0f, 1.0f, 1.0f, 1.0f }; + float[] light_specular = new float[] { 1.0f, 1.0f, 1.0f, 1.0f }; + /* light_position is NOT default value */ + float[] light_position0 = new float[] { 1.0f, 10.0f, 1.0f, 0.0f }; + float[] light_position1 = new float[] { -1.0f, -10.0f, -1.0f, 0.0f }; + gl = drawable.getGL(); + glu = GLU.createGLU(); + + if(gl.isGLES2()) { + gl.getGLES2().enableFixedFunctionEmulationMode(GLES2.FIXED_EMULATION_VERTEXCOLORTEXTURE); + } + + System.err.println("Entering initialization"); + System.err.print("GL Profile: "); + System.err.println(GLProfile.getProfile()); + System.err.print("GL:"); + System.err.println(gl); + System.err.print("GL_VERSION="); + System.err.println(gl.glGetString(gl.GL_VERSION)); + System.err.print("GL_EXTENSIONS: "); + System.err.println(gl.glGetString(gl.GL_EXTENSIONS)); + + glsrt = new GLSRT(glu, gl); + if(useLight0) { + gl.glLightfv(gl.GL_LIGHT0, gl.GL_AMBIENT, light_ambient, 0); + gl.glLightfv(gl.GL_LIGHT0, gl.GL_DIFFUSE, light_diffuse, 0); + gl.glLightfv(gl.GL_LIGHT0, gl.GL_SPECULAR, light_specular, 0); + gl.glLightfv(gl.GL_LIGHT0, gl.GL_POSITION, light_position0, 0); + } + + if(useLight1) { + gl.glLightfv(gl.GL_LIGHT1, gl.GL_AMBIENT, light_ambient, 0); + gl.glLightfv(gl.GL_LIGHT1, gl.GL_DIFFUSE, light_diffuse, 0); + gl.glLightfv(gl.GL_LIGHT1, gl.GL_SPECULAR, light_specular, 0); + gl.glLightfv(gl.GL_LIGHT1, gl.GL_POSITION, light_position1, 0); + } + + if(useLight0 || useLight1) { + gl.glEnable(gl.GL_LIGHTING); + } + if(useLight0) { + gl.glEnable(gl.GL_LIGHT0); + } + if(useLight1) { + gl.glEnable(gl.GL_LIGHT1); + } + + gl.glShadeModel(gl.GL_SMOOTH); + + gl.glEnable(gl.GL_DEPTH_TEST); + gl.glDepthFunc(gl.GL_LESS); + + gl.glClearColor(0.7f, 0.7f, 0.7f, 0f); + + // JAU + gl.glEnable(gl.GL_CULL_FACE); + gl.glCullFace(gl.GL_BACK); + } + + public void dispose(GLAutoDrawable drawable) { + gl=null; + } + + public void reshape(GLAutoDrawable drawable, int x, int y, int width, int height) { + gl = drawable.getGL(); + glutScreenWidth = width; + glutScreenHeight = height; + + //gl.glViewport(x, y, width, height); + updateCamera(); + + System.out.println("DemoApplication RESHAPE"); + } + + public void display(GLAutoDrawable drawable) { + gl = drawable.getGL(); + + gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT); + + if(!isIdle()) { + // simple dynamics world doesn't handle fixed-time-stepping + float ms = clock.getTimeMicroseconds(); + clock.reset(); + float minFPS = 1000000f / 60f; + if (ms > minFPS) { + ms = minFPS; + } + if (dynamicsWorld != null) { + dynamicsWorld.stepSimulation(ms / 1000000.f); + } + } + + if (dynamicsWorld != null) { + // optional but useful: debug drawing + dynamicsWorld.debugDrawWorld(); + } + + renderme(); + + //glFlush(); + //glutSwapBuffers(); + } + + public void displayChanged(GLAutoDrawable drawable, boolean modeChanged, boolean deviceChanged) { + } + + protected GL gl; + + // + // MouseListener + // + public void mouseClicked(MouseEvent e) { + mouseClick(e.getButton(), e.getX(), e.getY()); + } + public void mouseEntered(MouseEvent e) { + } + public void mouseExited(MouseEvent e) { + } + public void mousePressed(MouseEvent e) { + pickConstrain(e.getButton(), 1, e.getX(), e.getY()); + } + public void mouseReleased(MouseEvent e) { + pickConstrain(e.getButton(), 0, e.getX(), e.getY()); + } + + // + // MouseMotionListener + // + public void mouseDragged(MouseEvent e) { + mouseMotionFunc(e.getX(), e.getY()); + } + + public void mouseMoved(MouseEvent e) { + } + + // + // KeyListener + // + public void keyPressed(KeyEvent e) { + } + public void keyReleased(KeyEvent e) { + char c = e.getKeyChar(); + if(e.isActionKey()) { + specialKeyboard(e.getKeyCode()); + } else { + keyboardCallback(e.getKeyChar()); + } + } + public void keyTyped(KeyEvent e) { + } + + // + // + // + + public void setCameraDistance(float dist) { + cameraDistance = dist; + } + + public float getCameraDistance() { + return cameraDistance; + } + + public void toggleIdle() { + if (idle) { + idle = false; + } + else { + idle = true; + } + } + + public void updateCamera() { + stack.vectors.push(); + stack.matrices.push(); + stack.quats.push(); + try { + gl.glMatrixMode(gl.GL_PROJECTION); + gl.glLoadIdentity(); + float rele = ele * 0.01745329251994329547f; // rads per deg + float razi = azi * 0.01745329251994329547f; // rads per deg + + Quat4f rot = stack.quats.get(); + QuaternionUtil.setRotation(rot, cameraUp, razi); + + Vector3f eyePos = stack.vectors.get(0f, 0f, 0f); + VectorUtil.setCoord(eyePos, forwardAxis, -cameraDistance); + + Vector3f forward = stack.vectors.get(eyePos.x, eyePos.y, eyePos.z); + if (forward.lengthSquared() < BulletGlobals.FLT_EPSILON) { + forward.set(1f, 0f, 0f); + } + Vector3f right = stack.vectors.get(); + right.cross(cameraUp, forward); + Quat4f roll = stack.quats.get(); + QuaternionUtil.setRotation(roll, right, -rele); + + Matrix3f tmpMat1 = stack.matrices.get(); + Matrix3f tmpMat2 = stack.matrices.get(); + tmpMat1.set(rot); + tmpMat2.set(roll); + tmpMat1.mul(tmpMat2); + tmpMat1.transform(eyePos); + + cameraPosition.set(eyePos); + + gl.glFrustumf(-1.0f, 1.0f, -1.0f, 1.0f, 1.0f, 10000.0f); + glu.gluLookAt(cameraPosition.x, cameraPosition.y, cameraPosition.z, + cameraTargetPosition.x, cameraTargetPosition.y, cameraTargetPosition.z, + cameraUp.x, cameraUp.y, cameraUp.z); + gl.glMatrixMode(gl.GL_MODELVIEW); + gl.glLoadIdentity(); + } + finally { + stack.vectors.pop(); + stack.matrices.pop(); + stack.quats.pop(); + } + } + + public void stepLeft() { + azi -= STEPSIZE; + if (azi < 0) { + azi += 360; + } + } + + public void stepRight() { + azi += STEPSIZE; + if (azi >= 360) { + azi -= 360; + } + } + + public void stepFront() { + ele += STEPSIZE; + if (ele >= 360) { + ele -= 360; + } + } + + public void stepBack() { + ele -= STEPSIZE; + if (ele < 0) { + ele += 360; + } + } + + public void zoomIn() { + cameraDistance -= 0.4f; + if (cameraDistance < 0.1f) { + cameraDistance = 0.1f; + } + } + + public void zoomOut() { + cameraDistance += 0.4f; + } + + public void keyboardCallback(char key) { + lastKey = 0; + + if (key >= 0x31 && key < 0x37) { + int child = key - 0x31; + // TODO: m_profileIterator->Enter_Child(child); + } + if (key == 0x30) { + // TODO: m_profileIterator->Enter_Parent(); + } + + switch (key) { + case 'l': + stepLeft(); + break; + case 'r': + stepRight(); + break; + case 'f': + stepFront(); + break; + case 'b': + stepBack(); + break; + case 'z': + zoomIn(); + break; + case 'x': + zoomOut(); + break; + case 'i': + toggleIdle(); + break; + case 'h': + if ((debugMode & DebugDrawModes.NO_HELP_TEXT) != 0) { + debugMode = debugMode & (~DebugDrawModes.NO_HELP_TEXT); + } + else { + debugMode |= DebugDrawModes.NO_HELP_TEXT; + } + break; + + case 'w': + if ((debugMode & DebugDrawModes.DRAW_WIREFRAME) != 0) { + debugMode = debugMode & (~DebugDrawModes.DRAW_WIREFRAME); + } + else { + debugMode |= DebugDrawModes.DRAW_WIREFRAME; + } + break; + + case 'p': + if ((debugMode & DebugDrawModes.PROFILE_TIMINGS) != 0) { + debugMode = debugMode & (~DebugDrawModes.PROFILE_TIMINGS); + } + else { + debugMode |= DebugDrawModes.PROFILE_TIMINGS; + } + break; + + case 'm': + if ((debugMode & DebugDrawModes.ENABLE_SAT_COMPARISON) != 0) { + debugMode = debugMode & (~DebugDrawModes.ENABLE_SAT_COMPARISON); + } + else { + debugMode |= DebugDrawModes.ENABLE_SAT_COMPARISON; + } + break; + + case 'n': + if ((debugMode & DebugDrawModes.DISABLE_BULLET_LCP) != 0) { + debugMode = debugMode & (~DebugDrawModes.DISABLE_BULLET_LCP); + } + else { + debugMode |= DebugDrawModes.DISABLE_BULLET_LCP; + } + break; + + case 't': + if ((debugMode & DebugDrawModes.DRAW_TEXT) != 0) { + debugMode = debugMode & (~DebugDrawModes.DRAW_TEXT); + } + else { + debugMode |= DebugDrawModes.DRAW_TEXT; + } + break; + case 'y': + if ((debugMode & DebugDrawModes.DRAW_FEATURES_TEXT) != 0) { + debugMode = debugMode & (~DebugDrawModes.DRAW_FEATURES_TEXT); + } + else { + debugMode |= DebugDrawModes.DRAW_FEATURES_TEXT; + } + break; + case 'a': + if ((debugMode & DebugDrawModes.DRAW_AABB) != 0) { + debugMode = debugMode & (~DebugDrawModes.DRAW_AABB); + } + else { + debugMode |= DebugDrawModes.DRAW_AABB; + } + break; + case 'c': + if ((debugMode & DebugDrawModes.DRAW_CONTACT_POINTS) != 0) { + debugMode = debugMode & (~DebugDrawModes.DRAW_CONTACT_POINTS); + } + else { + debugMode |= DebugDrawModes.DRAW_CONTACT_POINTS; + } + break; + + case 'd': + if ((debugMode & DebugDrawModes.NO_DEACTIVATION) != 0) { + debugMode = debugMode & (~DebugDrawModes.NO_DEACTIVATION); + } + else { + debugMode |= DebugDrawModes.NO_DEACTIVATION; + } + if ((debugMode & DebugDrawModes.NO_DEACTIVATION) != 0) { + BulletGlobals.gDisableDeactivation = true; + } + else { + BulletGlobals.gDisableDeactivation = false; + } + break; + + case 'o': { + stepping = !stepping; + break; + } + case 's': + break; + // case ' ' : newRandom(); break; + case ' ': + clientResetScene(); + break; + case '1': { + if ((debugMode & DebugDrawModes.ENABLE_CCD) != 0) { + debugMode = debugMode & (~DebugDrawModes.ENABLE_CCD); + } + else { + debugMode |= DebugDrawModes.ENABLE_CCD; + } + break; + } + + case '.': { + shootBox(getCameraTargetPosition()); + break; + } + + case '+': { + ShootBoxInitialSpeed += 10f; + break; + } + case '-': { + ShootBoxInitialSpeed -= 10f; + break; + } + + default: + // std::cout << "unused key : " << key << std::endl; + break; + } + + if (getDynamicsWorld() != null && getDynamicsWorld().getDebugDrawer() != null) { + getDynamicsWorld().getDebugDrawer().setDebugMode(debugMode); + } + + //LWJGL.postRedisplay(); + } + + public int getDebugMode() { + return debugMode; + } + + public void setDebugMode(int mode) { + debugMode = mode; + if (getDynamicsWorld() != null && getDynamicsWorld().getDebugDrawer() != null) { + getDynamicsWorld().getDebugDrawer().setDebugMode(mode); + } + } + + public void specialKeyboard(int keycode) { + switch (keycode) { + case KeyEvent.VK_F1: { + break; + } + case KeyEvent.VK_F2: { + break; + } + case KeyEvent.VK_END: { + int numObj = getDynamicsWorld().getNumCollisionObjects(); + if (numObj != 0) { + CollisionObject obj = getDynamicsWorld().getCollisionObjectArray().get(numObj - 1); + + getDynamicsWorld().removeCollisionObject(obj); + RigidBody body = RigidBody.upcast(obj); + if (body != null && body.getMotionState() != null) { + //delete body->getMotionState(); + } + //delete obj; + } + break; + } + case KeyEvent.VK_LEFT: + stepLeft(); + break; + case KeyEvent.VK_RIGHT: + stepRight(); + break; + case KeyEvent.VK_UP: + stepFront(); + break; + case KeyEvent.VK_DOWN: + stepBack(); + break; + /* + case KeyEvent.VK_PRIOR: + zoomIn(); + break; + case KeyEvent.VK_NEXT: + zoomOut(); + break; + */ + case KeyEvent.VK_HOME: + toggleIdle(); + break; + default: + // std::cout << "unused (special) key : " << key << std::endl; + break; + } + } + + public void shootBox(Vector3f destination) { + if (dynamicsWorld != null) { + float mass = 10f; + Transform startTransform = new Transform(); + startTransform.setIdentity(); + Vector3f camPos = new Vector3f(getCameraPosition()); + startTransform.origin.set(camPos); + + if (shootBoxShape == null) { + //#define TEST_UNIFORM_SCALING_SHAPE 1 + //#ifdef TEST_UNIFORM_SCALING_SHAPE + //btConvexShape* childShape = new btBoxShape(btVector3(1.f,1.f,1.f)); + //m_shootBoxShape = new btUniformScalingShape(childShape,0.5f); + //#else + shootBoxShape = new BoxShape(new Vector3f(1f, 1f, 1f)); + //#endif// + } + + RigidBody body = this.localCreateRigidBody(mass, startTransform, shootBoxShape); + + Vector3f linVel = new Vector3f(destination.x - camPos.x, destination.y - camPos.y, destination.z - camPos.z); + linVel.normalize(); + linVel.scale(ShootBoxInitialSpeed); + + body.getWorldTransform().origin.set(camPos); + body.getWorldTransform().setRotation(new Quat4f(0f, 0f, 0f, 1f)); + body.setLinearVelocity(linVel); + body.setAngularVelocity(new Vector3f(0f, 0f, 0f)); + } + } + + public Vector3f getRayTo(int x, int y) { + float top = 1f; + float bottom = -1f; + float nearPlane = 1f; + float tanFov = (top - bottom) * 0.5f / nearPlane; + float fov = 2f * (float) Math.atan(tanFov); + + Vector3f rayFrom = new Vector3f(getCameraPosition()); + Vector3f rayForward = new Vector3f(); + rayForward.sub(getCameraTargetPosition(), getCameraPosition()); + rayForward.normalize(); + float farPlane = 600f; + rayForward.scale(farPlane); + + Vector3f rightOffset = new Vector3f(); + Vector3f vertical = new Vector3f(cameraUp); + + Vector3f hor = new Vector3f(); + // TODO: check: hor = rayForward.cross(vertical); + hor.cross(rayForward, vertical); + hor.normalize(); + // TODO: check: vertical = hor.cross(rayForward); + vertical.cross(hor, rayForward); + vertical.normalize(); + + float tanfov = (float) Math.tan(0.5f * fov); + hor.scale(2f * farPlane * tanfov); + vertical.scale(2f * farPlane * tanfov); + Vector3f rayToCenter = new Vector3f(); + rayToCenter.add(rayFrom, rayForward); + Vector3f dHor = new Vector3f(hor); + dHor.scale(1f / (float) glutScreenWidth); + Vector3f dVert = new Vector3f(vertical); + dVert.scale(1.f / (float) glutScreenHeight); + + Vector3f tmp1 = new Vector3f(); + Vector3f tmp2 = new Vector3f(); + tmp1.scale(0.5f, hor); + tmp2.scale(0.5f, vertical); + + Vector3f rayTo = new Vector3f(); + rayTo.sub(rayToCenter, tmp1); + rayTo.add(tmp2); + + tmp1.scale(x, dHor); + tmp2.scale(y, dVert); + + rayTo.add(tmp1); + rayTo.sub(tmp2); + return rayTo; + } + + private void mouseClick(int button, int x, int y) { + Vector3f rayTo = new Vector3f(getRayTo(x, y)); + + switch (button) { + case MouseEvent.BUTTON3: { + shootBox(rayTo); + break; + } + case MouseEvent.BUTTON2 : { + // apply an impulse + if (dynamicsWorld != null) { + CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(cameraPosition, rayTo); + dynamicsWorld.rayTest(cameraPosition, rayTo, rayCallback); + if (rayCallback.hasHit()) { + RigidBody body = RigidBody.upcast(rayCallback.collisionObject); + if (body != null) { + body.setActivationState(CollisionObject.ACTIVE_TAG); + Vector3f impulse = new Vector3f(rayTo); + impulse.normalize(); + float impulseStrength = 10f; + impulse.scale(impulseStrength); + Vector3f relPos = new Vector3f(); + relPos.sub(rayCallback.hitPointWorld, body.getCenterOfMassPosition()); + body.applyImpulse(impulse, relPos); + } + } + } + break; + } + } + } + + private void pickConstrain(int button, int state, int x, int y) { + Vector3f rayTo = new Vector3f(getRayTo(x, y)); + + switch (button) { + case MouseEvent.BUTTON1 : { + if (state == 1) { + // add a point to point constraint for picking + if (dynamicsWorld != null) { + CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(cameraPosition, rayTo); + dynamicsWorld.rayTest(cameraPosition, rayTo, rayCallback); + if (rayCallback.hasHit()) { + RigidBody body = RigidBody.upcast(rayCallback.collisionObject); + if (body != null) { + // other exclusions? + if (!(body.isStaticObject() || body.isKinematicObject())) { + pickedBody = body; + pickedBody.setActivationState(CollisionObject.DISABLE_DEACTIVATION); + + Vector3f pickPos = new Vector3f(rayCallback.hitPointWorld); + + Transform tmpTrans = new Transform(body.getCenterOfMassTransform()); + tmpTrans.inverse(); + Vector3f localPivot = new Vector3f(pickPos); + tmpTrans.transform(localPivot); + + Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot); + dynamicsWorld.addConstraint(p2p); + pickConstraint = p2p; + // save mouse position for dragging + BulletGlobals.gOldPickingPos.set(rayTo); + Vector3f eyePos = new Vector3f(cameraPosition); + Vector3f tmp = new Vector3f(); + tmp.sub(pickPos, eyePos); + BulletGlobals.gOldPickingDist = tmp.length(); + // very weak constraint for picking + p2p.setting.tau = 0.1f; + } + } + } + } + + } + else { + + if (pickConstraint != null && dynamicsWorld != null) { + dynamicsWorld.removeConstraint(pickConstraint); + // delete m_pickConstraint; + pickConstraint = null; + pickedBody.forceActivationState(CollisionObject.ACTIVE_TAG); + pickedBody.setDeactivationTime(0f); + pickedBody = null; + } + } + break; + } + default: { + } + } + } + + private void mouseMotionFunc(int x, int y) { + if (pickConstraint != null) { + // move the constraint pivot + Point2PointConstraint p2p = (Point2PointConstraint) pickConstraint; + if (p2p != null) { + // keep it at the same picking distance + + Vector3f newRayTo = new Vector3f(getRayTo(x, y)); + Vector3f eyePos = new Vector3f(cameraPosition); + Vector3f dir = new Vector3f(); + dir.sub(newRayTo, eyePos); + dir.normalize(); + dir.scale(BulletGlobals.gOldPickingDist); + + Vector3f newPos = new Vector3f(); + newPos.add(eyePos, dir); + p2p.setPivotB(newPos); + } + } + } + + public RigidBody localCreateRigidBody(float mass, Transform startTransform, CollisionShape shape) { + // rigidbody is dynamic if and only if mass is non zero, otherwise static + boolean isDynamic = (mass != 0f); + + Vector3f localInertia = new Vector3f(0f, 0f, 0f); + if (isDynamic) { + shape.calculateLocalInertia(mass, localInertia); + } + + // using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects + + //#define USE_MOTIONSTATE 1 + //#ifdef USE_MOTIONSTATE + DefaultMotionState myMotionState = new DefaultMotionState(startTransform); + RigidBody body = new RigidBody(new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia)); + //#else + //btRigidBody* body = new btRigidBody(mass,0,shape,localInertia); + //body->setWorldTransform(startTransform); + //#endif// + dynamicsWorld.addRigidBody(body); + + return body; + } + + // See http://www.lighthouse3d.com/opengl/glut/index.php?bmpfontortho + public void setOrthographicProjection() { + // switch to projection mode + gl.glMatrixMode(gl.GL_PROJECTION); + // save previous matrix which contains the + //settings for the perspective projection + // gl.glPushMatrix(); + // reset matrix + gl.glLoadIdentity(); + // set a 2D orthographic projection + glu.gluOrtho2D(0f, glutScreenWidth, 0f, glutScreenHeight); + // invert the y axis, down is positive + gl.glScalef(1f, -1f, 1f); + // mover the origin from the bottom left corner + // to the upper left corner + gl.glTranslatef(0f, -glutScreenHeight, 0f); + gl.glMatrixMode(gl.GL_MODELVIEW); + } + + /** + public void resetPerspectiveProjection() { + gl.glMatrixMode(gl.GL_PROJECTION); + gl.glPopMatrix(); + gl.glMatrixMode(gl.GL_MODELVIEW); + } + + private void displayProfileString(int xOffset, int yStart, String message) { + //glRasterPos3f(xOffset, yStart, 0); + // TODO: BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),message); + } + + // TODO: protected void showProfileInfo(float& xOffset,float& yStart, float yIncr); + */ + + private final Transform m = new Transform(); + private Vector3f wireColor = new Vector3f(); + private Color3f TEXT_COLOR = new Color3f(0f, 0f, 0f); + // private StringBuilder buf = new StringBuilder(); + + public void renderme() { + // JAU updateCamera(); + + if (dynamicsWorld != null) { + int numObjects = dynamicsWorld.getNumCollisionObjects(); + wireColor.set(1f, 0f, 0f); + for (int i = 0; i < numObjects; i++) { + CollisionObject colObj = dynamicsWorld.getCollisionObjectArray().get(i); + RigidBody body = RigidBody.upcast(colObj); + + if (body != null && body.getMotionState() != null) { + DefaultMotionState myMotionState = (DefaultMotionState) body.getMotionState(); + m.set(myMotionState.graphicsWorldTrans); + } else { + m.set(colObj.getWorldTransform()); + } + + if(0==i) { + wireColor.set(0.5f, 1f, 0.5f); // wants deactivation + } else { + wireColor.set(1f, 1f, 0.5f); // wants deactivation + } + if ((i & 1) != 0) { + wireColor.set(0f, 0f, 1f); + } + + // color differently for active, sleeping, wantsdeactivation states + if (colObj.getActivationState() == 1) // active + { + if ((i & 1) != 0) { + //wireColor.add(new Vector3f(1f, 0f, 0f)); + wireColor.x += 1f; + } + else { + //wireColor.add(new Vector3f(0.5f, 0f, 0f)); + wireColor.x += 0.5f; + } + } + if (colObj.getActivationState() == 2) // ISLAND_SLEEPING + { + if ((i & 1) != 0) { + //wireColor.add(new Vector3f(0f, 1f, 0f)); + wireColor.y += 1f; + } + else { + //wireColor.add(new Vector3f(0f, 0.5f, 0f)); + wireColor.y += 0.5f; + } + } + + // draw (saves the matrix already ..) + GLShapeDrawer.drawOpenGL(glsrt, gl, m, colObj.getCollisionShape(), wireColor, getDebugMode()); + } + GLShapeDrawer.drawCoordSystem(gl); + if(false) { + System.err.println("++++++++++++++++++++++++++++++++"); + System.err.println("++++++++++++++++++++++++++++++++"); + try { + Thread.sleep(2000); + } catch (Exception e) {} + } + + float xOffset = 10f; + float yStart = 20f; + float yIncr = 20f; + + // gl.glDisable(gl.GL_LIGHTING); + // JAU gl.glColor4f(0f, 0f, 0f, 0f); + +/* + if ((debugMode & DebugDrawModes.NO_HELP_TEXT) == 0) { + setOrthographicProjection(); + + // TODO: showProfileInfo(xOffset,yStart,yIncr); + +// #ifdef USE_QUICKPROF +// if ( getDebugMode() & btIDebugDraw::DBG_ProfileTimings) +// { +// static int counter = 0; +// counter++; +// std::map<std::string, hidden::ProfileBlock*>::iterator iter; +// for (iter = btProfiler::mProfileBlocks.begin(); iter != btProfiler::mProfileBlocks.end(); ++iter) +// { +// char blockTime[128]; +// sprintf(blockTime, "%s: %lf",&((*iter).first[0]),btProfiler::getBlockTime((*iter).first, btProfiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT)); +// glRasterPos3f(xOffset,yStart,0); +// BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),blockTime); +// yStart += yIncr; +// +// } +// } +// #endif //USE_QUICKPROF + + + String s = "mouse to interact"; + drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + // JAVA NOTE: added + s = "LMB=shoot, RMB=drag, MIDDLE=apply impulse"; + drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + s = "space to reset"; + drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + s = "cursor keys and z,x to navigate"; + drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + s = "i to toggle simulation, s single step"; + drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + s = "q to quit"; + drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + s = ". to shoot box"; + drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + // not yet hooked up again after refactoring... + + s = "d to toggle deactivation"; + drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + s = "g to toggle mesh animation (ConcaveDemo)"; + drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + // JAVA NOTE: added + s = "e to spawn new body (GenericJointDemo)"; + drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + s = "h to toggle help text"; + drawString(s, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + //buf = "p to toggle profiling (+results to file)"; + //drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + //bool useBulletLCP = !(getDebugMode() & btIDebugDraw::DBG_DisableBulletLCP); + //bool useCCD = (getDebugMode() & btIDebugDraw::DBG_EnableCCD); + //glRasterPos3f(xOffset,yStart,0); + //sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD); + //BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + //yStart += yIncr; + + //glRasterPos3f(xOffset, yStart, 0); + //buf = String.format(%10.2f", ShootBoxInitialSpeed); + buf.setLength(0); + buf.append("+- shooting speed = "); + FastFormat.append(buf, ShootBoxInitialSpeed); + drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + //#ifdef SHOW_NUM_DEEP_PENETRATIONS + buf.setLength(0); + buf.append("gNumDeepPenetrationChecks = "); + FastFormat.append(buf, BulletGlobals.gNumDeepPenetrationChecks); + drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + buf.setLength(0); + buf.append("gNumGjkChecks = "); + FastFormat.append(buf, BulletGlobals.gNumGjkChecks); + drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + //buf = String.format("gNumAlignedAllocs = %d", BulletGlobals.gNumAlignedAllocs); + // TODO: BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + //yStart += yIncr; + + //buf = String.format("gNumAlignedFree= %d", BulletGlobals.gNumAlignedFree); + // TODO: BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + //yStart += yIncr; + + //buf = String.format("# alloc-free = %d", BulletGlobals.gNumAlignedAllocs - BulletGlobals.gNumAlignedFree); + // TODO: BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + //yStart += yIncr; + + //enable BT_DEBUG_MEMORY_ALLOCATIONS define in Bullet/src/LinearMath/btAlignedAllocator.h for memory leak detection + //#ifdef BT_DEBUG_MEMORY_ALLOCATIONS + //glRasterPos3f(xOffset,yStart,0); + //sprintf(buf,"gTotalBytesAlignedAllocs = %d",gTotalBytesAlignedAllocs); + //BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + //yStart += yIncr; + //#endif //BT_DEBUG_MEMORY_ALLOCATIONS + + if (getDynamicsWorld() != null) { + buf.setLength(0); + buf.append("# objects = "); + FastFormat.append(buf, getDynamicsWorld().getNumCollisionObjects()); + drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + buf.setLength(0); + buf.append("# pairs = "); + FastFormat.append(buf, getDynamicsWorld().getBroadphase().getOverlappingPairCache().getNumOverlappingPairs()); + drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + } + //#endif //SHOW_NUM_DEEP_PENETRATIONS + + // JAVA NOTE: added + int free = (int)Runtime.getRuntime().freeMemory(); + int total = (int)Runtime.getRuntime().totalMemory(); + buf.setLength(0); + buf.append("heap = "); + FastFormat.append(buf, (float)(total - free) / (1024*1024)); + buf.append(" / "); + FastFormat.append(buf, (float)(total) / (1024*1024)); + buf.append(" MB"); + drawString(buf, Math.round(xOffset), Math.round(yStart), TEXT_COLOR); + yStart += yIncr; + + resetPerspectiveProjection(); + } */ + + // gl.glEnable(gl.GL_LIGHTING); + } + } + + public void clientResetScene() { + //#ifdef SHOW_NUM_DEEP_PENETRATIONS + BulletGlobals.gNumDeepPenetrationChecks = 0; + BulletGlobals.gNumGjkChecks = 0; + //#endif //SHOW_NUM_DEEP_PENETRATIONS + + int numObjects = 0; + if (dynamicsWorld != null) { + dynamicsWorld.stepSimulation(1f / 60f, 0); + numObjects = dynamicsWorld.getNumCollisionObjects(); + } + + for (int i = 0; i < numObjects; i++) { + CollisionObject colObj = dynamicsWorld.getCollisionObjectArray().get(i); + RigidBody body = RigidBody.upcast(colObj); + if (body != null) { + if (body.getMotionState() != null) { + DefaultMotionState myMotionState = (DefaultMotionState) body.getMotionState(); + myMotionState.graphicsWorldTrans.set(myMotionState.startWorldTrans); + colObj.setWorldTransform(myMotionState.graphicsWorldTrans); + colObj.setInterpolationWorldTransform(myMotionState.startWorldTrans); + colObj.activate(); + } + // removed cached contact points + dynamicsWorld.getBroadphase().getOverlappingPairCache().cleanProxyFromPairs(colObj.getBroadphaseHandle(), getDynamicsWorld().getDispatcher()); + + body = RigidBody.upcast(colObj); + if (body != null && !body.isStaticObject()) { + RigidBody.upcast(colObj).setLinearVelocity(new Vector3f(0f, 0f, 0f)); + RigidBody.upcast(colObj).setAngularVelocity(new Vector3f(0f, 0f, 0f)); + } + } + + /* + //quickly search some issue at a certain simulation frame, pressing space to reset + int fixed=18; + for (int i=0;i<fixed;i++) + { + getDynamicsWorld()->stepSimulation(1./60.f,1); + } + */ + } + } + + public DynamicsWorld getDynamicsWorld() { + return dynamicsWorld; + } + + public void setCameraUp(Vector3f camUp) { + cameraUp.set(camUp); + } + + public void setCameraForwardAxis(int axis) { + forwardAxis = axis; + } + + public Vector3f getCameraPosition() { + return cameraPosition; + } + + public Vector3f getCameraTargetPosition() { + return cameraTargetPosition; + } + + public boolean isIdle() { + return idle; + } + + public void setIdle(boolean idle) { + this.idle = idle; + } + + public void drawString(CharSequence s, int x, int y, Color3f color) { + glsrt.drawString(gl, s, x, y, color.x, color.y, color.z); + } + +} diff --git a/src/jbullet/src/javabullet/demos/opengl/FastFormat.java-nope b/src/jbullet/src/javabullet/demos/opengl/FastFormat.java-nope new file mode 100644 index 0000000..d9311a2 --- /dev/null +++ b/src/jbullet/src/javabullet/demos/opengl/FastFormat.java-nope @@ -0,0 +1,62 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.demos.opengl; + +/** + * + * @author jezek2 + */ +public class FastFormat { + + private static final char[] DIGITS = "0123456789".toCharArray(); + + public static void append(StringBuilder b, int i) { + if (i < 0) { + b.append('-'); + i = Math.abs(i); + } + + int digit = 1000000000; + boolean first = true; + while (digit >= 1) { + int v = (i/digit); + if (v != 0 || !first) { + b.append(DIGITS[v]); + first = false; + } + i -= v*digit; + digit /= 10; + } + + if (first) b.append('0'); + } + + public static void append(StringBuilder b, float f) { + int val = Math.round(f*100f); + append(b, val / 100); + b.append('.'); + append(b, Math.abs(val % 100)); + } + +} diff --git a/src/jbullet/src/javabullet/demos/opengl/FontRender.java-nope b/src/jbullet/src/javabullet/demos/opengl/FontRender.java-nope new file mode 100644 index 0000000..33468c6 --- /dev/null +++ b/src/jbullet/src/javabullet/demos/opengl/FontRender.java-nope @@ -0,0 +1,331 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.demos.opengl; + +import java.awt.*; +import java.awt.color.ColorSpace; +import java.awt.font.*; +import java.awt.geom.Rectangle2D; +import java.awt.image.*; +import java.io.*; +import java.nio.*; +import java.util.Hashtable; + +import javax.media.opengl.*; +import javax.media.opengl.glu.*; +import com.sun.opengl.util.*; + +/** + * + * @author jezek2 + */ +public class FontRender { + + //private static final File cacheDir = new File("/path/to/font/cache/dir/"); + + private FontRender() { + } + + protected static class Glyph { + int x,y,w,h; + int list = -1; + } + + public static class GLFont { + protected int texture; + protected int width, height; + protected Glyph[] glyphs = new Glyph[128-32]; + protected GL gl; + + public GLFont(GL gl) { + this.gl=gl; + for (int i=0; i<glyphs.length; i++) glyphs[i] = new Glyph(); + } + + public GLFont(GL gl, InputStream in) throws IOException { + this(gl); + load(in); + } + + public void destroy() { + gl.glDeleteTextures(1, new int[] { texture }, 0); + } + + protected void save(File f) throws IOException { +/* + DataOutputStream out = new DataOutputStream(new FileOutputStream(f)); + out.writeInt(width); + out.writeInt(height); + + gl.glPixelStorei(gl.GL_PACK_ROW_LENGTH, 0); + gl.glPixelStorei(gl.GL_PACK_ALIGNMENT, 1); + gl.glPixelStorei(gl.GL_PACK_SKIP_ROWS, 0); + gl.glPixelStorei(gl.GL_PACK_SKIP_PIXELS, 0); + + int size = width*height*4; + ByteBuffer buf = BufferUtils.createByteBuffer(size); + byte[] data = new byte[size]; + glBindTexture(gl.GL_TEXTURE_2D, texture); + glGetTexImage(gl.GL_TEXTURE_2D, 0, gl.GL_RGBA, gl.GL_UNSIGNED_BYTE, (ByteBuffer)buf.position(0)); + buf.get(data); + out.write(data); + + for (int i=0; i<glyphs.length; i++) { + out.writeShort(glyphs[i].x); + out.writeShort(glyphs[i].y); + out.writeShort(glyphs[i].w); + out.writeShort(glyphs[i].h); + } + + out.close(); +*/ + } + + protected void load(File f) throws IOException { + load(new FileInputStream(f)); + } + + protected void load(InputStream _in) throws IOException { + DataInputStream in = new DataInputStream(_in); + int w = in.readInt(); + int h = in.readInt(); + int size = w*h*4; + + gl.glPixelStorei(gl.GL_UNPACK_ROW_LENGTH, 0); + gl.glPixelStorei(gl.GL_UNPACK_ALIGNMENT, 1); + gl.glPixelStorei(gl.GL_UNPACK_SKIP_ROWS, 0); + gl.glPixelStorei(gl.GL_UNPACK_SKIP_PIXELS, 0); + + ByteBuffer buf = BufferUtil.newByteBuffer(size); + byte[] data = new byte[size]; + in.read(data); + buf.put(data); + + int[] id = new int[1]; + gl.glGenTextures(1, id, 0); + texture = id[0]; + width = w; + height = h; + + gl.glBindTexture(gl.GL_TEXTURE_2D, texture); + gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MIN_FILTER, gl.GL_LINEAR); + gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MAG_FILTER, gl.GL_LINEAR); + gl.glTexImage2D(gl.GL_TEXTURE_2D, 0, gl.GL_RGBA, w, h, 0, gl.GL_RGBA, gl.GL_UNSIGNED_BYTE, (ByteBuffer)buf.position(0)); + + for (int i=0; i<glyphs.length; i++) { + glyphs[i].x = in.readShort(); + glyphs[i].y = in.readShort(); + glyphs[i].w = in.readShort(); + glyphs[i].h = in.readShort(); + } + + in.close(); + } + } + + private static String getFontFileName(String family, int size, boolean bold) { + return family.replace(' ','_')+"_"+size+(bold? "_bold":"")+".fnt"; + } + + public static GLFont createFont(GLU glu, GL gl, String family, int size, boolean bold, boolean antialiasing) throws IOException { + GLFont gf = new GLFont(gl); + /*File f = new File(cacheDir, getFontFileName(family, size, bold)); + if (f.exists()) { + gf.load(f); + return gf; + }*/ + + BufferedImage img = renderFont(new Font(family, bold? Font.BOLD : Font.PLAIN, size), antialiasing, gf.glyphs); + gf.texture = createTexture(glu, gl, img, false); + gf.width = img.getWidth(); + gf.height = img.getHeight(); + //gf.save(f); + return gf; + } + + public static BufferedImage renderFont(Font font, boolean antialiasing, Glyph[] glyphs) { + FontRenderContext frc = new FontRenderContext(null, antialiasing, false); + + int imgw = 256; + if (font.getSize() >= 36) imgw <<= 1; + if (font.getSize() >= 72) imgw <<= 1; + + //BufferedImage img = new BufferedImage(imgw, 1024, BufferedImage.TYPE_INT_ARGB); + BufferedImage img = createImage(imgw, 1024, true); + Graphics2D g = (Graphics2D)img.getGraphics(); + + if (antialiasing) { + g.setRenderingHint(RenderingHints.KEY_TEXT_ANTIALIASING, RenderingHints.VALUE_TEXT_ANTIALIAS_ON); + } + + g.setColor(Color.WHITE); + g.setFont(font); + + int x=0, y=0,rowsize=0; + for (int c=32; c<128; c++) { + String s = ""+(char)c; + Rectangle2D rect = font.getStringBounds(s, frc); + LineMetrics lm = font.getLineMetrics(s, frc); + int w = (int)rect.getWidth()+1; + int h = (int)rect.getHeight()+2; + + if (x+w+2 > img.getWidth()) { + x = 0; + y += rowsize; + rowsize = 0; + } + + g.drawString(s, x+1, y+(int)lm.getAscent()+1); + + if (glyphs != null) { + glyphs[c-32].x = x+1; + glyphs[c-32].y = y+1; + glyphs[c-32].w = w; + glyphs[c-32].h = h; + } + + w += 2; + h += 2; + + x += w; + rowsize = Math.max(rowsize, h); + } + + y += rowsize; + g.dispose(); + + if (y < 128) img = img.getSubimage(0, 0, img.getWidth(), 128); + else if (y < 256) img = img.getSubimage(0, 0, img.getWidth(), 256); + else if (y < 512) img = img.getSubimage(0, 0, img.getWidth(), 512); + + return img; + } + + private static void renderGlyph(GL gl, GLFont font, Glyph g) { + if (g.list != -1) { + gl.glCallList(g.list); + return; + } + + g.list = gl.glGenLists(1); + gl.glNewList(g.list, gl.GL_COMPILE); + + float tw = font.width; + float th = font.height; + + int x=0, y=0; + + gl.glBegin(gl.GL_QUADS); + gl.glTexCoord2f((float)(g.x)/tw, (float)(g.y)/th); + gl.glVertex3f(x, y, 1); + + gl.glTexCoord2f((float)(g.x+g.w-1)/tw, (float)(g.y)/th); + gl.glVertex3f(x+g.w-1, y, 1); + + gl.glTexCoord2f((float)(g.x+g.w-1)/tw, (float)(g.y+g.h-1)/th); + gl.glVertex3f(x+g.w-1, y+g.h-1, 1); + + gl.glTexCoord2f((float)(g.x)/tw, (float)(g.y+g.h-1)/th); + gl.glVertex3f(x, y+g.h-1, 1); + gl.glEnd(); + + gl.glEndList(); + gl.glCallList(g.list); + } + + public static void drawString(GL gl, GLFont font, CharSequence s, int x, int y, float red, float green, float blue) { + drawString(gl, font, s, x, y, red, green, blue, 1); + } + + public static void drawString(GL gl, GLFont font, CharSequence s, int x, int y, float red, float green, float blue, float alpha) { + gl.glEnable(gl.GL_BLEND); + gl.glBlendFunc(gl.GL_SRC_ALPHA, gl.GL_ONE_MINUS_SRC_ALPHA); + + gl.glPushMatrix(); + gl.glTranslatef(x, y, 0); + + gl.glBindTexture(gl.GL_TEXTURE_2D, font.texture); + gl.glEnable(gl.GL_TEXTURE_2D); + gl.glColor4f(red, green, blue, alpha); + //gl.glColor4f(1, 1, 1, 1); + for (int i=0, n=s.length(); i<n; i++) { + char c = s.charAt(i); + if (c < 32 || c > 128) c = '?'; + Glyph g = font.glyphs[c-32]; + renderGlyph(gl, font, g); + //x += g.w; + //glTranslatef(g.w, 0, 0); + gl.glTranslatef(g.w-2, 0, 0); + } + gl.glDisable(gl.GL_TEXTURE_2D); + + gl.glPopMatrix(); + + gl.glDisable(gl.GL_BLEND); + } + + private static ColorModel glColorModel = new ComponentColorModel(ColorSpace.getInstance(ColorSpace.CS_sRGB), new int[] {8,8,8,0}, false, false, ComponentColorModel.OPAQUE, DataBuffer.TYPE_BYTE); + private static ColorModel glColorModelAlpha = new ComponentColorModel(ColorSpace.getInstance(ColorSpace.CS_sRGB), new int[] {8,8,8,8}, true, false, ComponentColorModel.OPAQUE, DataBuffer.TYPE_BYTE); + + private static int createTexture(GLU glu, GL gl, BufferedImage img, boolean mipMap) { + boolean USE_COMPRESSION = false; + + int[] id = new int[1]; + gl.glGenTextures(1, id, 0); + int tex = id[0]; + + gl.glBindTexture(gl.GL_TEXTURE_2D, tex); + gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MIN_FILTER, mipMap? gl.GL_LINEAR_MIPMAP_LINEAR : gl.GL_LINEAR); + gl.glTexParameteri(gl.GL_TEXTURE_2D, gl.GL_TEXTURE_MAG_FILTER, gl.GL_LINEAR); + + byte[] data = ((DataBufferByte)img.getRaster().getDataBuffer()).getData(); + + ByteBuffer buf = ByteBuffer.allocateDirect(data.length); + buf.order(ByteOrder.nativeOrder()); + buf.put(data, 0, data.length); + buf.flip(); + + boolean alpha = img.getColorModel().hasAlpha(); + + //gl.glTexImage2D(GL_TEXTURE_2D, 0, alpha? GL_RGBA:GL_RGB, img.getWidth(), img.getHeight(), 0, alpha? GL_RGBA:GL_RGB, GL_UNSIGNED_BYTE, buf); + gl.glTexImage2D(gl.GL_TEXTURE_2D, 0, USE_COMPRESSION? (alpha? gl.GL_COMPRESSED_RGBA:gl.GL_COMPRESSED_RGB) : (alpha? gl.GL_RGBA:gl.GL_RGB), img.getWidth(), img.getHeight(), 0, alpha? gl.GL_RGBA:gl.GL_RGB, gl.GL_UNSIGNED_BYTE, buf); + if (mipMap) { + glu.gluBuild2DMipmaps(gl.GL_TEXTURE_2D, USE_COMPRESSION? (alpha? gl.GL_COMPRESSED_RGBA:gl.GL_COMPRESSED_RGB) : (alpha? gl.GL_RGBA:gl.GL_RGB), img.getWidth(), img.getHeight(), alpha? gl.GL_RGBA:gl.GL_RGB, gl.GL_UNSIGNED_BYTE, buf); + //glu.gluBuild2DMipmaps(GL_TEXTURE_2D, GL_COMPRESSED_RGB, img.getWidth(), img.getHeight(), GL_RGB, GL_UNSIGNED_BYTE, buf); + } + + return tex; + } + + private static BufferedImage createImage(int width, int height, boolean alpha) { + if (alpha) { + WritableRaster raster = Raster.createInterleavedRaster(DataBuffer.TYPE_BYTE, width, height, 4, null); + return new BufferedImage(glColorModelAlpha, raster, false, new Hashtable()); + } + + WritableRaster raster = Raster.createInterleavedRaster(DataBuffer.TYPE_BYTE, width, height, 3, null); + return new BufferedImage(glColorModel, raster, false, new Hashtable()); + } + +} diff --git a/src/jbullet/src/javabullet/demos/opengl/GLSRT.java b/src/jbullet/src/javabullet/demos/opengl/GLSRT.java new file mode 100644 index 0000000..cdd4aef --- /dev/null +++ b/src/jbullet/src/javabullet/demos/opengl/GLSRT.java @@ -0,0 +1,293 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.demos.opengl; + +import java.net.URL; +import java.io.IOException; +import java.nio.FloatBuffer; +import java.nio.ByteBuffer; +import java.util.HashMap; +import java.util.Map; +// import javabullet.demos.opengl.FontRender.GLFont; +import javax.media.opengl.*; +import javax.media.opengl.glu.*; +import javax.media.opengl.util.ImmModeSink; +import javax.media.opengl.util.BufferUtil; + +/** + * + * @author jezek2 + */ +public class GLSRT { + + public static final boolean VBO_CACHE = true; // JAU + // public static final boolean VBO_CACHE = false; + + /*static { + ImmModeSink.setVBOUsage(false); + }*/ + + private GLU glu; + // private GLFont font; + + public GLSRT(GLU glu, GL gl) { + System.out.println("VBO_CACHE: "+VBO_CACHE); + this.glu = glu; + /* + try { + font = new GLFont(gl, DemoApplication.class.getResourceAsStream("DejaVu_Sans_11.fnt")); + URL fontURL = DemoApplication.class.getResource("DejaVu_Sans_11.fnt"); + if(fontURL!=null) { + font = new GLFont(gl, fontURL.openStream()); + } + } + catch (IOException e) { + e.printStackTrace(); + } */ + } + + ImmModeSink vboCube = null; + + public void drawCube(GL gl, float extent) { + extent = extent * 0.5f; + + if(vboCube==null) { + vboCube = ImmModeSink.createFixed(GL.GL_STATIC_DRAW, 24, + 3, GL.GL_FLOAT, // vertex + 0, GL.GL_FLOAT, // color + 3, GL.GL_FLOAT, // normal + 0, GL.GL_FLOAT); // texture + + vboCube.glBegin(GL.GL_TRIANGLES); + vboCube.glNormal3f( 0f, 0f, 1f); + vboCube.glNormal3f( 0f, 0f, 1f); + vboCube.glNormal3f( 0f, 0f, 1f); + vboCube.glNormal3f( 0f, 0f, 1f); + vboCube.glVertex3f(-extent,+extent,+extent); + vboCube.glVertex3f(+extent,-extent,+extent); + vboCube.glVertex3f(+extent,+extent,+extent); + vboCube.glVertex3f(-extent,-extent,+extent); + vboCube.glNormal3f( 0f, 0f,-1f); + vboCube.glNormal3f( 0f, 0f,-1f); + vboCube.glNormal3f( 0f, 0f,-1f); + vboCube.glNormal3f( 0f, 0f,-1f); + vboCube.glVertex3f(-extent,+extent,-extent); + vboCube.glVertex3f(+extent,-extent,-extent); + vboCube.glVertex3f(+extent,+extent,-extent); + vboCube.glVertex3f(+extent,+extent,+extent); + vboCube.glNormal3f( 0f, -1f, 0f); + vboCube.glNormal3f( 0f, -1f, 0f); + vboCube.glNormal3f( 0f, -1f, 0f); + vboCube.glNormal3f( 0f, -1f, 0f); + vboCube.glVertex3f(-extent,-extent,+extent); + vboCube.glVertex3f(+extent,-extent,-extent); + vboCube.glVertex3f(+extent,-extent,+extent); + vboCube.glVertex3f(-extent,-extent,-extent); + vboCube.glNormal3f( 0f,1f, 0f); + vboCube.glNormal3f( 0f,1f, 0f); + vboCube.glNormal3f( 0f,1f, 0f); + vboCube.glNormal3f( 0f,1f, 0f); + vboCube.glVertex3f(-extent,+extent,+extent); + vboCube.glVertex3f(+extent,+extent,-extent); + vboCube.glVertex3f(+extent,+extent,+extent); + vboCube.glVertex3f(-extent,+extent,-extent); + vboCube.glNormal3f( 1f,0f, 0f); + vboCube.glNormal3f( 1f,0f, 0f); + vboCube.glNormal3f( 1f,0f, 0f); + vboCube.glNormal3f( 1f,0f, 0f); + vboCube.glVertex3f(+extent,-extent,+extent); + vboCube.glVertex3f(+extent,+extent,-extent); + vboCube.glVertex3f(+extent,+extent,+extent); + vboCube.glVertex3f(+extent,-extent,-extent); + vboCube.glNormal3f( -1f, 0f, 0f); + vboCube.glNormal3f( -1f, 0f, 0f); + vboCube.glNormal3f( -1f, 0f, 0f); + vboCube.glNormal3f( -1f, 0f, 0f); + vboCube.glVertex3f(-extent,-extent,+extent); + vboCube.glVertex3f(-extent,+extent,-extent); + vboCube.glVertex3f(-extent,+extent,+extent); + vboCube.glVertex3f(-extent,-extent,-extent); + vboCube.glEnd(gl, false); + } + vboCube.draw(gl, cubeIndices, true); + } + private static final byte[] s_cubeIndices = + { + 0, 3, 1, 2, 0, 1, /* front */ + 6, 5, 4, 5, 7, 4, /* back */ + 8, 11, 9, 10, 8, 9, /* top */ + 15, 12, 13, 12, 14, 13, /* bottom */ + 16, 19, 17, 18, 16, 17, /* right */ + 23, 20, 21, 20, 22, 21 /* left */ + }; + private ByteBuffer cubeIndices=BufferUtil.newByteBuffer(s_cubeIndices); + + //////////////////////////////////////////////////////////////////////////// + + private static GLUquadric cylinder=null; + private static GLUquadric sphere=null; + + private static class SphereKey { + public float radius; + + public SphereKey() { + } + + public SphereKey(SphereKey key) { + radius = key.radius; + } + + @Override + public boolean equals(Object obj) { + if (obj == null || !(obj instanceof SphereKey)) return false; + SphereKey other = (SphereKey)obj; + return radius == other.radius; + } + + @Override + public int hashCode() { + return Float.floatToIntBits(radius); + } + } + + private static Map<SphereKey,ImmModeSink> sphereDisplayLists = new HashMap<SphereKey,ImmModeSink>(); + private static SphereKey sphereKey = new SphereKey(); + + public void drawSphere(GL gl, float radius, int slices, int stacks) { + if(sphere==null) { + sphere = glu.gluNewQuadric(); + sphere.setImmMode((VBO_CACHE)?false:true); + } + sphereKey.radius = radius; + ImmModeSink vbo = sphereDisplayLists.get(sphereKey); + if (vbo == null) { + glu.gluSphere(sphere, radius, 8, 8); + if(VBO_CACHE) { + vbo = sphere.replaceImmModeSink(); + sphereDisplayLists.put(new SphereKey(sphereKey), vbo); + } + } + + if(VBO_CACHE && null!=vbo) { + vbo.draw(gl, true); + } + } + + //////////////////////////////////////////////////////////////////////////// + + + private static class CylinderKey { + public float radius; + public float halfHeight; + + public CylinderKey() { + } + + public CylinderKey(CylinderKey key) { + radius = key.radius; + halfHeight = key.halfHeight; + } + + public void set(float radius, float halfHeight) { + this.radius = radius; + this.halfHeight = halfHeight; + } + + @Override + public boolean equals(Object obj) { + if (obj == null || !(obj instanceof CylinderKey)) return false; + CylinderKey other = (CylinderKey) obj; + if (radius != other.radius) return false; + if (halfHeight != other.halfHeight) return false; + return true; + } + + @Override + public int hashCode() { + int hash = 7; + hash = 23 * hash + Float.floatToIntBits(radius); + hash = 23 * hash + Float.floatToIntBits(halfHeight); + return hash; + } + } + + private static Map<CylinderKey,ImmModeSink> cylinderDisplayLists = new HashMap<CylinderKey,ImmModeSink>(); + private static CylinderKey cylinderKey = new CylinderKey(); + + public void drawCylinder(GL gl, float radius, float halfHeight, int upAxis) { + if(cylinder==null) { + cylinder = glu.gluNewQuadric(); + cylinder.setImmMode((VBO_CACHE)?false:true); + } + gl.glPushMatrix(); + switch (upAxis) { + case 0: + gl.glRotatef(-90f, 0.0f, 1.0f, 0.0f); + gl.glTranslatef(0.0f, 0.0f, -halfHeight); + break; + case 1: + gl.glRotatef(-90.0f, 1.0f, 0.0f, 0.0f); + gl.glTranslatef(0.0f, 0.0f, -halfHeight); + break; + case 2: + gl.glTranslatef(0.0f, 0.0f, -halfHeight); + break; + default: { + assert (false); + } + } + + // The gluCylinder subroutine draws a cylinder that is oriented along the z axis. + // The base of the cylinder is placed at z = 0; the top of the cylinder is placed at z=height. + // Like a sphere, the cylinder is subdivided around the z axis into slices and along the z axis into stacks. + + cylinderKey.set(radius, halfHeight); + ImmModeSink vbo = cylinderDisplayLists.get(cylinderKey); + if (vbo == null) { + glu.gluQuadricDrawStyle(cylinder, glu.GLU_FILL); + glu.gluQuadricNormals(cylinder, glu.GLU_SMOOTH); + glu.gluCylinder(cylinder, radius, radius, 2f * halfHeight, 15, 10); + if(VBO_CACHE) { + vbo = cylinder.replaceImmModeSink(); + cylinderDisplayLists.put(new CylinderKey(cylinderKey), vbo); + } + } + + if(VBO_CACHE && null!=vbo) { + vbo.draw(gl, true); + } + + gl.glPopMatrix(); + } + + //////////////////////////////////////////////////////////////////////////// + + public void drawString(GL gl, CharSequence s, int x, int y, float red, float green, float blue) { + /* + if (font != null) { + FontRender.drawString(gl, font, s, x, y, red, green, blue); + } */ + } + +} diff --git a/src/jbullet/src/javabullet/demos/opengl/GLShapeDrawer.java b/src/jbullet/src/javabullet/demos/opengl/GLShapeDrawer.java new file mode 100644 index 0000000..76fb9b1 --- /dev/null +++ b/src/jbullet/src/javabullet/demos/opengl/GLShapeDrawer.java @@ -0,0 +1,504 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.demos.opengl; + +import javabullet.BulletStack; +import javabullet.collision.broadphase.BroadphaseNativeType; +import javabullet.collision.shapes.BoxShape; +import javabullet.collision.shapes.CapsuleShape; +import javabullet.collision.shapes.CollisionShape; +import javabullet.collision.shapes.CompoundShape; +import javabullet.collision.shapes.ConcaveShape; +import javabullet.collision.shapes.CylinderShape; +import javabullet.collision.shapes.InternalTriangleIndexCallback; +import javabullet.collision.shapes.PolyhedralConvexShape; +import javabullet.collision.shapes.SphereShape; +import javabullet.collision.shapes.TriangleCallback; +import javabullet.linearmath.DebugDrawModes; +import javabullet.linearmath.Transform; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; +import javax.media.opengl.*; +import javax.media.opengl.util.ImmModeSink; + +/** + * + * @author jezek2 + */ +public class GLShapeDrawer { + + /* + private static Map<CollisionShape,TriMeshKey> g_display_lists = new HashMap<CollisionShape,TriMeshKey>(); + + private static int OGL_get_displaylist_for_shape(CollisionShape shape) { + // JAVA NOTE: rewritten + TriMeshKey trimesh = g_display_lists.get(shape); + if (trimesh != null) { + return trimesh.dlist; + } + + return 0; + } + + private static void OGL_displaylist_clean() { + // JAVA NOTE: rewritten + for (TriMeshKey trimesh : g_display_lists.values()) { + glDeleteLists(trimesh.dlist, 1); + } + + g_display_lists.clear(); + } + */ + + public static void drawCoordSystem(GL gl) { + ImmModeSink vbo = ImmModeSink.createFixed(GL.GL_STATIC_DRAW, 10, + 3, GL.GL_FLOAT, // vertex + 4, GL.GL_FLOAT, // color + 0, GL.GL_FLOAT, // normal + 0, GL.GL_FLOAT); // texture + vbo.glBegin(gl.GL_LINES); + vbo.glColor4f ( 1f, 1f, 1f, 1f); + vbo.glVertex3f( 0f, 0f, 0f); + vbo.glColor4f ( 1f, 1f, 1f, 1f); + vbo.glVertex3f( 1f, 0f, 0f); + vbo.glColor4f ( 1f, 1f, 1f, 1f); + vbo.glVertex3f( 0f, 0f, 0f); + vbo.glColor4f ( 1f, 1f, 1f, 1f); + vbo.glVertex3f( 0f, 1f, 0f); + vbo.glColor4f ( 1f, 1f, 1f, 1f); + vbo.glVertex3f( 0f, 0f, 0f); + vbo.glColor4f ( 1f, 1f, 1f, 1f); + vbo.glVertex3f( 0f, 0f, 1f); + vbo.glEnd(gl); + } + + private static float[] glMat = new float[16]; + + public static void drawOpenGL(GLSRT glsrt, GL gl, Transform trans, CollisionShape shape, Vector3f color, int debugMode) { + BulletStack stack = BulletStack.get(); + + stack.pushCommonMath(); + try { + //System.out.println("shape="+shape+" type="+BroadphaseNativeTypes.forValue(shape.getShapeType())); + + gl.glPushMatrix(); + trans.getOpenGLMatrix(glMat); + gl.glMultMatrixf(glMat, 0); + // if (shape.getShapeType() == BroadphaseNativeTypes.UNIFORM_SCALING_SHAPE_PROXYTYPE.getValue()) + // { + // const btUniformScalingShape* scalingShape = static_cast<const btUniformScalingShape*>(shape); + // const btConvexShape* convexShape = scalingShape->getChildShape(); + // float scalingFactor = (float)scalingShape->getUniformScalingFactor(); + // { + // btScalar tmpScaling[4][4]={{scalingFactor,0,0,0}, + // {0,scalingFactor,0,0}, + // {0,0,scalingFactor,0}, + // {0,0,0,1}}; + // + // drawOpenGL( (btScalar*)tmpScaling,convexShape,color,debugMode); + // } + // return; + // } + + if (shape.getShapeType() == BroadphaseNativeType.COMPOUND_SHAPE_PROXYTYPE) { + CompoundShape compoundShape = (CompoundShape) shape; + for (int i = compoundShape.getNumChildShapes() - 1; i >= 0; i--) { + Transform childTrans = stack.transforms.get(compoundShape.getChildTransform(i)); + CollisionShape colShape = compoundShape.getChildShape(i); + drawOpenGL(glsrt, gl, childTrans, colShape, color, debugMode); + } + } + else { + gl.glEnable(gl.GL_COLOR_MATERIAL); + gl.glColor4f(color.x, color.y, color.z, 1f); + + boolean useWireframeFallback = true; + + if ( (debugMode & DebugDrawModes.DRAW_WIREFRAME) == 0) { + switch (shape.getShapeType()) { + case BOX_SHAPE_PROXYTYPE: { + BoxShape boxShape = (BoxShape) shape; + Vector3f halfExtent = stack.vectors.get(boxShape.getHalfExtentsWithMargin()); + gl.glScalef(2f * halfExtent.x, 2f * halfExtent.y, 2f * halfExtent.z); + glsrt.drawCube(gl, 1f); + useWireframeFallback = false; + break; + } + case TRIANGLE_SHAPE_PROXYTYPE: + case TETRAHEDRAL_SHAPE_PROXYTYPE: { + //todo: + // useWireframeFallback = false; + break; + } + case CONVEX_HULL_SHAPE_PROXYTYPE: + break; + case SPHERE_SHAPE_PROXYTYPE: { + SphereShape sphereShape = (SphereShape) shape; + float radius = sphereShape.getMargin(); // radius doesn't include the margin, so draw with margin + // TODO: glutSolidSphere(radius,10,10); + //sphere.draw(radius, 8, 8); + glsrt.drawSphere(gl, radius, 10, 10); + /* + glPointSize(10f); + glBegin(gl.GL_POINTS); + glVertex3f(0f, 0f, 0f); + glEnd(); + glPointSize(1f); + */ + useWireframeFallback = false; + break; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + CapsuleShape capsuleShape = (CapsuleShape)shape; + float radius = capsuleShape.getRadius(); + float halfHeight = capsuleShape.getHalfHeight(); + int upAxis = 1; + + glsrt.drawCylinder(gl, radius,halfHeight,upAxis); + + gl.glTranslatef(0f, -halfHeight, 0f); + //glutSolidSphere(radius,10,10); + //sphere.draw(radius, 10, 10); + glsrt.drawSphere(gl, radius, 10, 10); + gl.glTranslatef(0f, 2f*halfHeight,0f); + //glutSolidSphere(radius,10,10); + //sphere.draw(radius, 10, 10); + glsrt.drawSphere(gl, radius, 10, 10); + useWireframeFallback = false; + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: { + break; + } + // case CONE_SHAPE_PROXYTYPE: + // { + // const btConeShape* coneShape = static_cast<const btConeShape*>(shape); + // int upIndex = coneShape->getConeUpIndex(); + // float radius = coneShape->getRadius();//+coneShape->getMargin(); + // float height = coneShape->getHeight();//+coneShape->getMargin(); + // switch (upIndex) + // { + // case 0: + // glRotatef(90.0, 0.0, 1.0, 0.0); + // break; + // case 1: + // glRotatef(-90.0, 1.0, 0.0, 0.0); + // break; + // case 2: + // break; + // default: + // { + // } + // }; + // + // glTranslatef(0.0, 0.0, -0.5*height); + // glutSolidCone(radius,height,10,10); + // useWireframeFallback = false; + // break; + // + // } + case CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE: { + useWireframeFallback = false; + break; + } + + case CONVEX_SHAPE_PROXYTYPE: + case CYLINDER_SHAPE_PROXYTYPE: + { + CylinderShape cylinder = (CylinderShape) shape; + int upAxis = cylinder.getUpAxis(); + + float radius = cylinder.getRadius(); + float halfHeight = VectorUtil.getCoord(cylinder.getHalfExtentsWithMargin(), upAxis); + + glsrt.drawCylinder(gl, radius, halfHeight, upAxis); + + break; + } + default: { + } + + } + + } + + if (useWireframeFallback) { + // for polyhedral shapes + if (shape.isPolyhedral()) { + PolyhedralConvexShape polyshape = (PolyhedralConvexShape) shape; + + ImmModeSink vbo = ImmModeSink.createFixed(GL.GL_STATIC_DRAW, polyshape.getNumEdges()+3, + 3, GL.GL_FLOAT, // vertex + 0, GL.GL_FLOAT, // color + 0, GL.GL_FLOAT, // normal + 0, GL.GL_FLOAT); // texture + + vbo.glBegin(gl.GL_LINES); + + Vector3f a = stack.vectors.get(), b = stack.vectors.get(); + int i; + for (i = 0; i < polyshape.getNumEdges(); i++) { + polyshape.getEdge(i, a, b); + + vbo.glVertex3f(a.x, a.y, a.z); + vbo.glVertex3f(b.x, b.y, b.z); + } + vbo.glEnd(gl); + + // if (debugMode==btIDebugDraw::DBG_DrawFeaturesText) + // { + // glRasterPos3f(0.0, 0.0, 0.0); + // //BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape->getExtraDebugInfo()); + // + // glColor3f(1.f, 1.f, 1.f); + // for (i=0;i<polyshape->getNumVertices();i++) + // { + // btPoint3 vtx; + // polyshape->getVertex(i,vtx); + // glRasterPos3f(vtx.x(), vtx.y(), vtx.z()); + // char buf[12]; + // sprintf(buf," %d",i); + // BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + // } + // + // for (i=0;i<polyshape->getNumPlanes();i++) + // { + // btVector3 normal; + // btPoint3 vtx; + // polyshape->getPlane(normal,vtx,i); + // btScalar d = vtx.dot(normal); + // + // glRasterPos3f(normal.x()*d, normal.y()*d, normal.z()*d); + // char buf[12]; + // sprintf(buf," plane %d",i); + // BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); + // + // } + // } + + + } + } + + // #ifdef USE_DISPLAY_LISTS + // + // if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE||shape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) + // { + // GLuint dlist = OGL_get_displaylist_for_shape((btCollisionShape * )shape); + // if (dlist) + // { + // glCallList(dlist); + // } + // else + // { + // #else + if (shape.isConcave())//>getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE||shape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) + // if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) + { + ConcaveShape concaveMesh = (ConcaveShape) shape; + //btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + //btVector3 aabbMax(100,100,100);//btScalar(1e30),btScalar(1e30),btScalar(1e30)); + + //todo pass camera, for some culling + Vector3f aabbMax = stack.vectors.get(1e30f, 1e30f, 1e30f); + Vector3f aabbMin = stack.vectors.get(-1e30f, -1e30f, -1e30f); + + GlDrawcallback drawCallback = new GlDrawcallback(gl); + drawCallback.wireframe = (debugMode & DebugDrawModes.DRAW_WIREFRAME) != 0; + + concaveMesh.processAllTriangles(drawCallback, aabbMin, aabbMax); + } + //#endif + + //#ifdef USE_DISPLAY_LISTS + // } + // } + //#endif + + // if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + // { + // btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; + // + // //todo: pass camera for some culling + // btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + // btVector3 aabbMin(-btScalar(1e30),-btScalar(1e30),-btScalar(1e30)); + // TriangleGlDrawcallback drawCallback; + // convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + // + // } + + // TODO: error in original sources GL_DEPTH_BUFFER_BIT instead of GL_DEPTH_TEST + //gl.glDisable(GL_DEPTH_TEST); + //glRasterPos3f(0, 0, 0);//mvtx.x(), vtx.y(), vtx.z()); + if ((debugMode & DebugDrawModes.DRAW_TEXT) != 0) { + // TODO: BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->getName()); + } + + if ((debugMode & DebugDrawModes.DRAW_FEATURES_TEXT) != 0) { + //BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape->getExtraDebugInfo()); + } + //gl.glEnable(GL_DEPTH_TEST); + + } + } + finally { + gl.glPopMatrix(); + stack.popCommonMath(); + } + } + + //////////////////////////////////////////////////////////////////////////// + + private static class TriMeshKey { + public CollisionShape shape; + public int dlist; // OpenGL display list + } + + private static class GlDisplaylistDrawcallback implements TriangleCallback { + private GL gl; + + private final Vector3f diff1 = new Vector3f(); + private final Vector3f diff2 = new Vector3f(); + private final Vector3f normal = new Vector3f(); + + public GlDisplaylistDrawcallback(GL gl) { + this.gl = gl; + } + + public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) { + diff1.sub(triangle[1], triangle[0]); + diff2.sub(triangle[2], triangle[0]); + normal.cross(diff1, diff2); + + normal.normalize(); + + ImmModeSink vbo = ImmModeSink.createFixed(GL.GL_STATIC_DRAW, 3, + 3, GL.GL_FLOAT, // vertex + 4, GL.GL_FLOAT, // color + 3, GL.GL_FLOAT, // normal + 0, GL.GL_FLOAT); // texture + + vbo.glBegin(gl.GL_TRIANGLES); + vbo.glColor4f(0, 1f, 0, 1f); + vbo.glNormal3f(normal.x, normal.y, normal.z); + vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z); + + vbo.glColor4f(0, 1f, 0, 1f); + vbo.glNormal3f(normal.x, normal.y, normal.z); + vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z); + + vbo.glColor4f(0, 1f, 0, 1f); + vbo.glNormal3f(normal.x, normal.y, normal.z); + vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z); + vbo.glEnd(gl); + + /*glBegin(gl.GL_LINES); + glColor3f(1, 1, 0); + glNormal3d(normal.getX(),normal.getY(),normal.getZ()); + glVertex3d(triangle[0].getX(), triangle[0].getY(), triangle[0].getZ()); + glNormal3d(normal.getX(),normal.getY(),normal.getZ()); + glVertex3d(triangle[1].getX(), triangle[1].getY(), triangle[1].getZ()); + glColor3f(1, 1, 0); + glNormal3d(normal.getX(),normal.getY(),normal.getZ()); + glVertex3d(triangle[2].getX(), triangle[2].getY(), triangle[2].getZ()); + glNormal3d(normal.getX(),normal.getY(),normal.getZ()); + glVertex3d(triangle[1].getX(), triangle[1].getY(), triangle[1].getZ()); + glColor3f(1, 1, 0); + glNormal3d(normal.getX(),normal.getY(),normal.getZ()); + glVertex3d(triangle[2].getX(), triangle[2].getY(), triangle[2].getZ()); + glNormal3d(normal.getX(),normal.getY(),normal.getZ()); + glVertex3d(triangle[0].getX(), triangle[0].getY(), triangle[0].getZ()); + glEnd();*/ + } + } + + private static class GlDrawcallback implements TriangleCallback { + private GL gl; + public boolean wireframe = false; + + public GlDrawcallback(GL gl) { + this.gl = gl; + } + + public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) { + ImmModeSink vbo = ImmModeSink.createFixed(GL.GL_STATIC_DRAW, 10, + 3, GL.GL_FLOAT, // vertex + 4, GL.GL_FLOAT, // color + 0, GL.GL_FLOAT, // normal + 0, GL.GL_FLOAT); // texture + if (wireframe) { + vbo.glBegin(gl.GL_LINES); + vbo.glColor4f(1, 0, 0, 1); + vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z); + vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z); + vbo.glColor4f(0, 1, 0, 1); + vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z); + vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z); + vbo.glColor4f(0, 0, 1, 1); + vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z); + vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z); + vbo.glEnd(gl); + } + else { + vbo.glBegin(gl.GL_TRIANGLES); + vbo.glColor4f(1, 0, 0, 1); + vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z); + vbo.glColor4f(0, 1, 0, 1); + vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z); + vbo.glColor4f(0, 0, 1, 1); + vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z); + vbo.glEnd(gl); + } + } + } + + private static class TriangleGlDrawcallback implements InternalTriangleIndexCallback { + private GL gl; + + public TriangleGlDrawcallback(GL gl) { + this.gl = gl; + } + + public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) { + ImmModeSink vbo = ImmModeSink.createFixed(GL.GL_STATIC_DRAW, 10, + 3, GL.GL_FLOAT, // vertex + 4, GL.GL_FLOAT, // color + 0, GL.GL_FLOAT, // normal + 0, GL.GL_FLOAT); // texture + vbo.glBegin(gl.GL_TRIANGLES);//LINES); + vbo.glColor4f(1, 0, 0, 1); + vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z); + vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z); + vbo.glColor4f(0, 1, 0, 1); + vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z); + vbo.glVertex3f(triangle[1].x, triangle[1].y, triangle[1].z); + vbo.glColor4f(0, 0, 1, 1); + vbo.glVertex3f(triangle[2].x, triangle[2].y, triangle[2].z); + vbo.glVertex3f(triangle[0].x, triangle[0].y, triangle[0].z); + vbo.glEnd(gl); + } + } + +} diff --git a/src/jbullet/src/javabullet/demos/opengl/JOGL.java b/src/jbullet/src/javabullet/demos/opengl/JOGL.java new file mode 100644 index 0000000..1e3408e --- /dev/null +++ b/src/jbullet/src/javabullet/demos/opengl/JOGL.java @@ -0,0 +1,149 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.demos.opengl; + +import com.sun.javafx.newt.*; +import javax.media.nativewindow.*; +import javax.media.opengl.*; +import javax.media.opengl.glu.*; +import javax.media.opengl.util.*; +import java.nio.*; + +/** + * + * @author jezek2 + */ + +public class JOGL implements WindowListener, MouseListener { + + private GLWindow window; + public boolean quit = false; + + public void windowResized(WindowEvent e) { } + public void windowMoved(WindowEvent e) { } + public void windowDestroyNotify(WindowEvent e) { + quit = true; + } + + public void mouseClicked(MouseEvent e) { + //if(e.getClickCount()>1) { + quit=true; + //} + } + public void mouseEntered(MouseEvent e) { + } + public void mouseExited(MouseEvent e) { + } + public void mousePressed(MouseEvent e) { + } + public void mouseReleased(MouseEvent e) { + } + public void mouseMoved(MouseEvent e) { + } + public void mouseDragged(MouseEvent e) { + } + + private static boolean redisplay = false; + + public static void postRedisplay() { + redisplay = true; + } + + private void run(String title, DemoApplication demoApp, int type, int width, int height) { + System.err.print(title); + System.err.println(" run()"); + GLProfile.setProfileGLAny(); + try { + NWCapabilities caps = new NWCapabilities(); + // For emulation library, use 16 bpp + caps.setRedBits(5); + caps.setGreenBits(6); + caps.setBlueBits(5); + caps.setDepthBits(16); + + Window nWindow = null; + if(0!=(type&USE_AWT)) { + Display nDisplay = NewtFactory.createDisplay(NativeWindowFactory.TYPE_AWT, null); // local display + Screen nScreen = NewtFactory.createScreen(NativeWindowFactory.TYPE_AWT, nDisplay, 0); // screen 0 + nWindow = NewtFactory.createWindow(NativeWindowFactory.TYPE_AWT, nScreen, caps); + } + window = GLWindow.create(nWindow, caps); + + window.addWindowListener(this); + window.addMouseListener(this); + window.addMouseListener(demoApp); + window.addKeyListener(demoApp); + window.addGLEventListener(demoApp); + // window.setEventHandlerMode(GLWindow.EVENT_HANDLER_GL_CURRENT); // default + // window.setEventHandlerMode(GLWindow.EVENT_HANDLER_GL_NONE); // no current .. + + window.enablePerfLog(true); + // Size OpenGL to Video Surface + window.setSize(width, height); + window.setFullscreen(true); + window.setVisible(true); + width = window.getWidth(); + height = window.getHeight(); + + while (!quit && window.getDuration() < 200000) { + window.display(); + } + + // Shut things down cooperatively + window.destroy(); + window.getFactory().shutdown(); + System.out.print(title); + System.out.println(" shut down cleanly."); + } catch (Throwable t) { + t.printStackTrace(); + } + } + + public static int USE_NEWT = 0; + public static int USE_AWT = 1 << 0; + + public static void main(String title, DemoApplication demo, String[] args) { + int width = 480; + int height = 800; + int type = USE_NEWT ; + for(int i=args.length-1; i>=0; i--) { + if(args[i].equals("-awt")) { + type |= USE_AWT; + } + if(args[i].equals("-width") && i+1<args.length) { + try { + width = Integer.parseInt(args[i+1]); + } catch (NumberFormatException nfe) {} + } + if(args[i].equals("-height") && i+1<args.length) { + try { + height = Integer.parseInt(args[i+1]); + } catch (NumberFormatException nfe) {} + } + } + new JOGL().run(title, demo, type, width, height); + System.exit(0); + } + +} diff --git a/src/jbullet/src/javabullet/demos/vehicle/VehicleDemo.java-nope b/src/jbullet/src/javabullet/demos/vehicle/VehicleDemo.java-nope new file mode 100644 index 0000000..7c9efbd --- /dev/null +++ b/src/jbullet/src/javabullet/demos/vehicle/VehicleDemo.java-nope @@ -0,0 +1,576 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.demos.vehicle; + +import java.nio.ByteBuffer; +import java.nio.ByteOrder; +import java.util.ArrayList; +import java.util.List; +import javabullet.collision.broadphase.BroadphaseInterface; +import javabullet.collision.broadphase.SimpleBroadphase; +import javabullet.collision.dispatch.CollisionDispatcher; +import javabullet.collision.dispatch.CollisionObject; +import javabullet.collision.dispatch.DefaultCollisionConfiguration; +import javabullet.collision.shapes.BoxShape; +import javabullet.collision.shapes.BvhTriangleMeshShape; +import javabullet.collision.shapes.CollisionShape; +import javabullet.collision.shapes.CompoundShape; +import javabullet.collision.shapes.CylinderShapeX; +import javabullet.collision.shapes.TriangleIndexVertexArray; +import javabullet.demos.opengl.JOGL; +import javabullet.demos.opengl.DemoApplication; +import javabullet.demos.opengl.GLShapeDrawer; +import javabullet.dynamics.DiscreteDynamicsWorld; +import javabullet.dynamics.RigidBody; +import javabullet.dynamics.constraintsolver.ConstraintSolver; +import javabullet.dynamics.constraintsolver.SequentialImpulseConstraintSolver; +import javabullet.dynamics.vehicle.DefaultVehicleRaycaster; +import javabullet.dynamics.vehicle.RaycastVehicle; +import javabullet.dynamics.vehicle.VehicleRaycaster; +import javabullet.dynamics.vehicle.VehicleTuning; +import javabullet.dynamics.vehicle.WheelInfo; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; +import javax.media.opengl.*; +import java.awt.event.*; + +/** + * VehicleDemo shows how to setup and use the built-in raycast vehicle. + * + * @author jezek2 + */ +public class VehicleDemo extends DemoApplication { + + // By default, Bullet Vehicle uses Y as up axis. + // You can override the up axis, for example Z-axis up. Enable this define to see how to: + // //#define FORCE_ZAXIS_UP 1 + + //#ifdef FORCE_ZAXIS_UP + //int rightIndex = 0; + //int upIndex = 2; + //int forwardIndex = 1; + //btVector3 wheelDirectionCS0(0,0,-1); + //btVector3 wheelAxleCS(1,0,0); + //#else + private static final int rightIndex = 0; + private static final int upIndex = 1; + private static final int forwardIndex = 2; + private static final Vector3f wheelDirectionCS0 = new Vector3f(0,-1,0); + private static final Vector3f wheelAxleCS = new Vector3f(-1,0,0); + //#endif + + private static final int maxProxies = 32766; + private static final int maxOverlap = 65535; + + // RaycastVehicle is the interface for the constraint that implements the raycast vehicle + // notice that for higher-quality slow-moving vehicles, another approach might be better + // implementing explicit hinged-wheel constraints with cylinder collision, rather then raycasts + private static float gEngineForce = 0.f; + private static float gBreakingForce = 0.f; + + private static float maxEngineForce = 1000.f;//this should be engine/velocity dependent + private static float maxBreakingForce = 100.f; + + private static float gVehicleSteering = 0.f; + private static float steeringIncrement = 0.04f; + private static float steeringClamp = 0.3f; + private static float wheelRadius = 0.5f; + private static float wheelWidth = 0.4f; + private static float wheelFriction = 1000;//1e30f; + private static float suspensionStiffness = 20.f; + private static float suspensionDamping = 2.3f; + private static float suspensionCompression = 4.4f; + private static float rollInfluence = 0.1f;//1.0f; + + private static final float suspensionRestLength = 0.6f; + + private static final int CUBE_HALF_EXTENTS = 1; + + //////////////////////////////////////////////////////////////////////////// + + public RigidBody carChassis; + public List<CollisionShape> collisionShapes = new ArrayList<CollisionShape>(); + public BroadphaseInterface overlappingPairCache; + public CollisionDispatcher dispatcher; + public ConstraintSolver constraintSolver; + public DefaultCollisionConfiguration collisionConfiguration; + public TriangleIndexVertexArray indexVertexArrays; + + public ByteBuffer vertices; + + public VehicleTuning tuning = new VehicleTuning(); + public VehicleRaycaster vehicleRayCaster; + public RaycastVehicle vehicle; + + public float cameraHeight; + + public float minCameraDistance; + public float maxCameraDistance; + + public VehicleDemo() { + super(); + carChassis = null; + cameraHeight = 4f; + minCameraDistance = 3f; + maxCameraDistance = 10f; + indexVertexArrays = null; + vertices = null; + vehicle = null; + cameraPosition.set(30, 30, 30); + } + + public void initPhysics() { + //#ifdef FORCE_ZAXIS_UP + //m_cameraUp = btVector3(0,0,1); + //m_forwardAxis = 1; + //#endif + + CollisionShape groundShape = new BoxShape(new Vector3f(50, 3, 50)); + collisionShapes.add(groundShape); + collisionConfiguration = new DefaultCollisionConfiguration(); + dispatcher = new CollisionDispatcher(collisionConfiguration); + Vector3f worldMin = new Vector3f(-1000, -1000, -1000); + Vector3f worldMax = new Vector3f(1000, 1000, 1000); + //overlappingPairCache = new AxisSweep3(worldMin,worldMax); + overlappingPairCache = new SimpleBroadphase(); + constraintSolver = new SequentialImpulseConstraintSolver(); + dynamicsWorld = new DiscreteDynamicsWorld(dispatcher, overlappingPairCache, constraintSolver, collisionConfiguration); + //#ifdef FORCE_ZAXIS_UP + //dynamicsWorld.setGravity(new Vector3f(0, 0, -10)); + //#endif + + //m_dynamicsWorld->setGravity(btVector3(0,0,0)); + Transform tr = new Transform(); + tr.setIdentity(); + + // either use heightfield or triangle mesh + //#define USE_TRIMESH_GROUND 1 + //#ifdef USE_TRIMESH_GROUND + + final float TRIANGLE_SIZE = 20f; + + // create a triangle-mesh ground + int vertStride = 4 * 3 /* sizeof(btVector3) */; + int indexStride = 3 * 4 /* 3*sizeof(int) */; + + final int NUM_VERTS_X = 20; + final int NUM_VERTS_Y = 20; + final int totalVerts = NUM_VERTS_X * NUM_VERTS_Y; + + final int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1); + + vertices = ByteBuffer.allocateDirect(totalVerts * vertStride).order(ByteOrder.nativeOrder()); + ByteBuffer gIndices = ByteBuffer.allocateDirect(totalTriangles * 3 * 4).order(ByteOrder.nativeOrder()); + + Vector3f tmp = new Vector3f(); + for (int i = 0; i < NUM_VERTS_X; i++) { + for (int j = 0; j < NUM_VERTS_Y; j++) { + float wl = 0.2f; + // height set to zero, but can also use curved landscape, just uncomment out the code + float height = 0f; // 20f * (float)Math.sin(i * wl) * (float)Math.cos(j * wl); + + //#ifdef FORCE_ZAXIS_UP + //m_vertices[i+j*NUM_VERTS_X].setValue( + // (i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE, + // (j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE, + // height + // ); + //#else + tmp.set( + (i - NUM_VERTS_X * 0.5f) * TRIANGLE_SIZE, + height, + (j - NUM_VERTS_Y * 0.5f) * TRIANGLE_SIZE); + + int index = i + j * NUM_VERTS_X; + vertices.putFloat((index * 3 + 0) * 4, tmp.x); + vertices.putFloat((index * 3 + 1) * 4, tmp.y); + vertices.putFloat((index * 3 + 2) * 4, tmp.z); + //#endif + } + } + + //int index=0; + gIndices.clear(); + for (int i = 0; i < NUM_VERTS_X - 1; i++) { + for (int j = 0; j < NUM_VERTS_Y - 1; j++) { + gIndices.putInt(j * NUM_VERTS_X + i); + gIndices.putInt(j * NUM_VERTS_X + i + 1); + gIndices.putInt((j + 1) * NUM_VERTS_X + i + 1); + + gIndices.putInt(j * NUM_VERTS_X + i); + gIndices.putInt((j + 1) * NUM_VERTS_X + i + 1); + gIndices.putInt((j + 1) * NUM_VERTS_X + i); + } + } + gIndices.flip(); + + indexVertexArrays = new TriangleIndexVertexArray(totalTriangles, + gIndices, + indexStride, + totalVerts, vertices, vertStride); + + boolean useQuantizedAabbCompression = true; + groundShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression); + + tr.origin.set(0, -4.5f, 0); + + //#else +// //testing btHeightfieldTerrainShape +// int width=128; +// int length=128; +// unsigned char* heightfieldData = new unsigned char[width*length]; +// { +// for (int i=0;i<width*length;i++) +// { +// heightfieldData[i]=0; +// } +// } +// +// char* filename="heightfield128x128.raw"; +// FILE* heightfieldFile = fopen(filename,"r"); +// if (!heightfieldFile) +// { +// filename="../../heightfield128x128.raw"; +// heightfieldFile = fopen(filename,"r"); +// } +// if (heightfieldFile) +// { +// int numBytes =fread(heightfieldData,1,width*length,heightfieldFile); +// //btAssert(numBytes); +// if (!numBytes) +// { +// printf("couldn't read heightfield at %s\n",filename); +// } +// fclose (heightfieldFile); +// } +// +// +// btScalar maxHeight = 20000.f; +// +// bool useFloatDatam=false; +// bool flipQuadEdges=false; +// +// btHeightfieldTerrainShape* heightFieldShape = new btHeightfieldTerrainShape(width,length,heightfieldData,maxHeight,upIndex,useFloatDatam,flipQuadEdges);; +// groundShape = heightFieldShape; +// +// heightFieldShape->setUseDiamondSubdivision(true); +// +// btVector3 localScaling(20,20,20); +// localScaling[upIndex]=1.f; +// groundShape->setLocalScaling(localScaling); +// +// tr.setOrigin(btVector3(0,-64.5f,0)); +// + //#endif + + collisionShapes.add(groundShape); + + // create ground object + localCreateRigidBody(0, tr, groundShape); + + //#ifdef FORCE_ZAXIS_UP + // // indexRightAxis = 0; + // // indexUpAxis = 2; + // // indexForwardAxis = 1; + //btCollisionShape* chassisShape = new btBoxShape(btVector3(1.f,2.f, 0.5f)); + //btCompoundShape* compound = new btCompoundShape(); + //btTransform localTrans; + //localTrans.setIdentity(); + // //localTrans effectively shifts the center of mass with respect to the chassis + //localTrans.setOrigin(btVector3(0,0,1)); + //#else + CollisionShape chassisShape = new BoxShape(new Vector3f(1.0f, 0.5f, 2.0f)); + collisionShapes.add(chassisShape); + + CompoundShape compound = new CompoundShape(); + collisionShapes.add(compound); + Transform localTrans = new Transform(); + localTrans.setIdentity(); + // localTrans effectively shifts the center of mass with respect to the chassis + localTrans.origin.set(0, 1, 0); + //#endif + + compound.addChildShape(localTrans, chassisShape); + + tr.origin.set(0, 0, 0); + + carChassis = localCreateRigidBody(800, tr, compound); //chassisShape); + //m_carChassis->setDamping(0.2,0.2); + + clientResetScene(); + + // create vehicle + { + vehicleRayCaster = new DefaultVehicleRaycaster(dynamicsWorld); + vehicle = new RaycastVehicle(tuning, carChassis, vehicleRayCaster); + + // never deactivate the vehicle + carChassis.setActivationState(CollisionObject.DISABLE_DEACTIVATION); + + dynamicsWorld.addVehicle(vehicle); + + float connectionHeight = 1.2f; + + boolean isFrontWheel = true; + + // choose coordinate system + vehicle.setCoordinateSystem(rightIndex, upIndex, forwardIndex); + + //#ifdef FORCE_ZAXIS_UP + //btVector3 connectionPointCS0(CUBE_HALF_EXTENTS-(0.3*wheelWidth),2*CUBE_HALF_EXTENTS-wheelRadius, connectionHeight); + //#else + Vector3f connectionPointCS0 = new Vector3f(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2f * CUBE_HALF_EXTENTS - wheelRadius); + //#endif + + vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel); + //#ifdef FORCE_ZAXIS_UP + //connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),2*CUBE_HALF_EXTENTS-wheelRadius, connectionHeight); + //#else + connectionPointCS0.set(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 2f * CUBE_HALF_EXTENTS - wheelRadius); + //#endif + + vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel); + //#ifdef FORCE_ZAXIS_UP + //connectionPointCS0 = btVector3(-CUBE_HALF_EXTENTS+(0.3*wheelWidth),-2*CUBE_HALF_EXTENTS+wheelRadius, connectionHeight); + //#else + connectionPointCS0.set(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2f * CUBE_HALF_EXTENTS + wheelRadius); + //#endif //FORCE_ZAXIS_UP + isFrontWheel = false; + vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel); + //#ifdef FORCE_ZAXIS_UP + //connectionPointCS0 = btVector3(CUBE_HALF_EXTENTS-(0.3*wheelWidth),-2*CUBE_HALF_EXTENTS+wheelRadius, connectionHeight); + //#else + connectionPointCS0.set(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2f * CUBE_HALF_EXTENTS + wheelRadius); + //#endif + vehicle.addWheel(connectionPointCS0, wheelDirectionCS0, wheelAxleCS, suspensionRestLength, wheelRadius, tuning, isFrontWheel); + + for (int i = 0; i < vehicle.getNumWheels(); i++) { + WheelInfo wheel = vehicle.getWheelInfo(i); + wheel.suspensionStiffness = suspensionStiffness; + wheel.wheelsDampingRelaxation = suspensionDamping; + wheel.wheelsDampingCompression = suspensionCompression; + wheel.frictionSlip = wheelFriction; + wheel.rollInfluence = rollInfluence; + } + } + + setCameraDistance(26.f); + } + + // to be implemented by the demo + @Override + public void renderme() { + updateCamera(); + + CylinderShapeX wheelShape = new CylinderShapeX(new Vector3f(wheelWidth, wheelRadius, wheelRadius)); + Vector3f wheelColor = new Vector3f(1, 0, 0); + + for (int i = 0; i < vehicle.getNumWheels(); i++) { + // synchronize the wheels with the (interpolated) chassis worldtransform + vehicle.updateWheelTransform(i, true); + // draw wheels (cylinders) + Transform trans = vehicle.getWheelInfo(i).worldTransform; + GLShapeDrawer.drawOpenGL(glsrt, gl, trans, wheelShape, wheelColor, getDebugMode()); + } + + super.renderme(); + } + + @Override + public void display(GLAutoDrawable drawable) { + gl = drawable.getGL(); + gl.glClear(gl.GL_COLOR_BUFFER_BIT | gl.GL_DEPTH_BUFFER_BIT); + + { + int wheelIndex = 2; + vehicle.applyEngineForce(gEngineForce,wheelIndex); + vehicle.setBrake(gBreakingForce,wheelIndex); + wheelIndex = 3; + vehicle.applyEngineForce(gEngineForce,wheelIndex); + vehicle.setBrake(gBreakingForce,wheelIndex); + + wheelIndex = 0; + vehicle.setSteeringValue(gVehicleSteering,wheelIndex); + wheelIndex = 1; + vehicle.setSteeringValue(gVehicleSteering,wheelIndex); + } + + float dt = clock.getTimeMicroseconds() * 0.000001f; + clock.reset(); + if (dynamicsWorld != null) + { + // during idle mode, just run 1 simulation step maximum + int maxSimSubSteps = idle ? 1 : 2; + if (isIdle()) + dt = 1f/420f; + + int numSimSteps = dynamicsWorld.stepSimulation(dt,maxSimSubSteps); + // optional but useful: debug drawing + dynamicsWorld.debugDrawWorld(); + + //#define VERBOSE_FEEDBACK + //#ifdef VERBOSE_FEEDBACK + //if (!numSimSteps) + // printf("Interpolated transforms\n"); + //else + //{ + // if (numSimSteps > maxSimSubSteps) + // { + // //detect dropping frames + // printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps); + // } else + // { + // printf("Simulated (%i) steps\n",numSimSteps); + // } + //} + //#endif //VERBOSE_FEEDBACK + } + + //#ifdef USE_QUICKPROF + //btProfiler::beginBlock("render"); + //#endif //USE_QUICKPROF + + renderme(); + + //#ifdef USE_QUICKPROF + //btProfiler::endBlock("render"); + //#endif + } + + @Override + public void clientResetScene() { + gVehicleSteering = 0f; + Transform tr = new Transform(); + tr.setIdentity(); + carChassis.setCenterOfMassTransform(tr); + carChassis.setLinearVelocity(new Vector3f(0, 0, 0)); + carChassis.setAngularVelocity(new Vector3f(0, 0, 0)); + dynamicsWorld.getBroadphase().getOverlappingPairCache().cleanProxyFromPairs(carChassis.getBroadphaseHandle(), getDynamicsWorld().getDispatcher()); + if (vehicle != null) { + vehicle.resetSuspension(); + for (int i = 0; i < vehicle.getNumWheels(); i++) { + // synchronize the wheels with the (interpolated) chassis worldtransform + vehicle.updateWheelTransform(i, true); + } + } + } + + @Override + public void specialKeyboard(int key) { + // printf("key = %i x=%i y=%i\n",key,x,y); + + switch (key) { + case KeyEvent.VK_LEFT: { + gVehicleSteering += steeringIncrement; + if (gVehicleSteering > steeringClamp) { + gVehicleSteering = steeringClamp; + } + break; + } + case KeyEvent.VK_RIGHT: { + gVehicleSteering -= steeringIncrement; + if (gVehicleSteering < -steeringClamp) { + gVehicleSteering = -steeringClamp; + } + break; + } + case KeyEvent.VK_UP: { + gEngineForce = maxEngineForce; + gBreakingForce = 0.f; + break; + } + case KeyEvent.VK_DOWN: { + gBreakingForce = maxBreakingForce; + gEngineForce = 0.f; + break; + } + default: + super.specialKeyboard(key); + break; + } + + //glutPostRedisplay(); + } + + @Override + public void updateCamera() + { + + // //#define DISABLE_CAMERA 1 + //#ifdef DISABLE_CAMERA + //DemoApplication::updateCamera(); + //return; + //#endif //DISABLE_CAMERA + + gl.glMatrixMode(gl.GL_PROJECTION); + gl.glLoadIdentity(); + + Transform chassisWorldTrans = new Transform(); + + // look at the vehicle + carChassis.getMotionState().getWorldTransform(chassisWorldTrans); + cameraTargetPosition.set(chassisWorldTrans.origin); + + // interpolate the camera height + //#ifdef FORCE_ZAXIS_UP + //m_cameraPosition[2] = (15.0*m_cameraPosition[2] + m_cameraTargetPosition[2] + m_cameraHeight)/16.0; + //#else + cameraPosition.y = (15.0f*cameraPosition.y + cameraTargetPosition.y + cameraHeight) / 16.0f; + //#endif + + Vector3f camToObject = new Vector3f(); + camToObject.sub(cameraTargetPosition, cameraPosition); + + // keep distance between min and max distance + float cameraDistance = camToObject.length(); + float correctionFactor = 0f; + if (cameraDistance < minCameraDistance) + { + correctionFactor = 0.15f*(minCameraDistance-cameraDistance)/cameraDistance; + } + if (cameraDistance > maxCameraDistance) + { + correctionFactor = 0.15f*(maxCameraDistance-cameraDistance)/cameraDistance; + } + Vector3f tmp = new Vector3f(); + tmp.scale(correctionFactor, camToObject); + cameraPosition.sub(tmp); + + // update OpenGL camera settings + gl.glFrustum(-1.0, 1.0, -1.0, 1.0, 1.0, 10000.0); + + glu.gluLookAt(cameraPosition.x,cameraPosition.y,cameraPosition.z, + cameraTargetPosition.x,cameraTargetPosition.y, cameraTargetPosition.z, + cameraUp.x,cameraUp.y,cameraUp.z); + gl.glMatrixMode(gl.GL_MODELVIEW); + } + + public static void main(String[] args) { + VehicleDemo vehicleDemo = new VehicleDemo(); + vehicleDemo.initPhysics(); + + JOGL.main(args, 640, 480, "Bullet Vehicle Demo", vehicleDemo); + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/DiscreteDynamicsWorld.java b/src/jbullet/src/javabullet/dynamics/DiscreteDynamicsWorld.java new file mode 100644 index 0000000..237d4ec --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/DiscreteDynamicsWorld.java @@ -0,0 +1,985 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics; + +import java.util.ArrayList; +import java.util.Comparator; +import java.util.List; +import javabullet.BulletGlobals; +import javabullet.collision.broadphase.BroadphaseInterface; +import javabullet.collision.broadphase.CollisionFilterGroups; +import javabullet.collision.broadphase.Dispatcher; +import javabullet.collision.broadphase.DispatcherInfo; +import javabullet.collision.dispatch.CollisionConfiguration; +import javabullet.collision.dispatch.CollisionObject; +import javabullet.collision.dispatch.CollisionWorld; +import javabullet.collision.dispatch.SimulationIslandManager; +import javabullet.collision.narrowphase.PersistentManifold; +import javabullet.collision.shapes.CollisionShape; +import javabullet.collision.shapes.InternalTriangleIndexCallback; +import javabullet.collision.shapes.TriangleCallback; +import javabullet.dynamics.constraintsolver.ConstraintSolver; +import javabullet.dynamics.constraintsolver.ContactSolverInfo; +import javabullet.dynamics.constraintsolver.SequentialImpulseConstraintSolver; +import javabullet.dynamics.constraintsolver.TypedConstraint; +import javabullet.dynamics.vehicle.RaycastVehicle; +import javabullet.linearmath.DebugDrawModes; +import javabullet.linearmath.IDebugDraw; +import javabullet.linearmath.MiscUtil; +import javabullet.linearmath.ScalarUtil; +import javabullet.linearmath.Transform; +import javabullet.linearmath.TransformUtil; +import javax.vecmath.Vector3f; + +/** + * DiscreteDynamicsWorld provides discrete rigid body simulation. + * Those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController. + * + * @author jezek2 + */ +public class DiscreteDynamicsWorld extends DynamicsWorld { + + protected ConstraintSolver constraintSolver; + protected SimulationIslandManager islandManager; + protected final List<TypedConstraint> constraints = new ArrayList<TypedConstraint>(); + protected final Vector3f gravity = new Vector3f(0f, -10f, 0f); + + //for variable timesteps + protected float localTime = 1f / 60f; + //for variable timesteps + + protected boolean ownsIslandManager; + protected boolean ownsConstraintSolver; + + protected ContactSolverInfo solverInfo = new ContactSolverInfo(); + protected List<RaycastVehicle> vehicles = new ArrayList<RaycastVehicle>(); + protected int profileTimings = 0; + + public DiscreteDynamicsWorld(Dispatcher dispatcher, BroadphaseInterface pairCache, ConstraintSolver constraintSolver, CollisionConfiguration collisionConfiguration) { + super(dispatcher, pairCache, collisionConfiguration); + this.constraintSolver = constraintSolver; + + if (this.constraintSolver == null) { + this.constraintSolver = new SequentialImpulseConstraintSolver(); + ownsConstraintSolver = true; + } + else { + ownsConstraintSolver = false; + } + + { + islandManager = new SimulationIslandManager(); + } + + ownsIslandManager = true; + } + + protected void saveKinematicState(float timeStep) { + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + RigidBody body = RigidBody.upcast(colObj); + if (body != null) { + //Transform predictedTrans = new Transform(); + if (body.getActivationState() != CollisionObject.ISLAND_SLEEPING) { + if (body.isKinematicObject()) { + // to calculate velocities next frame + body.saveKinematicState(timeStep); + } + } + } + } + } + + @Override + public void debugDrawWorld() { + stack.vectors.push(); + try { + if (getDebugDrawer() != null && (getDebugDrawer().getDebugMode() & (DebugDrawModes.DRAW_WIREFRAME | DebugDrawModes.DRAW_AABB)) != 0) { + int i; + + // todo: iterate over awake simulation islands! + for (i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + if (getDebugDrawer() != null && (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_WIREFRAME) != 0) { + Vector3f color = stack.vectors.get(255f, 255f, 255f); + switch (colObj.getActivationState()) { + case CollisionObject.ACTIVE_TAG: + color.set(255f, 255f, 255f); + break; + case CollisionObject.ISLAND_SLEEPING: + color.set(0f, 255f, 0f); + break; + case CollisionObject.WANTS_DEACTIVATION: + color.set(0f, 255f, 255f); + break; + case CollisionObject.DISABLE_DEACTIVATION: + color.set(255f, 0f, 0f); + break; + case CollisionObject.DISABLE_SIMULATION: + color.set(255f, 255f, 0f); + break; + default: { + color.set(255f, 0f, 0f); + } + } + + debugDrawObject(colObj.getWorldTransform(), colObj.getCollisionShape(), color); + } + if (debugDrawer != null && (debugDrawer.getDebugMode() & DebugDrawModes.DRAW_AABB) != 0) { + Vector3f minAabb = stack.vectors.get(), maxAabb = stack.vectors.get(); + Vector3f colorvec = stack.vectors.get(1f, 0f, 0f); + colObj.getCollisionShape().getAabb(colObj.getWorldTransform(), minAabb, maxAabb); + debugDrawer.drawAabb(minAabb, maxAabb, colorvec); + } + } + + Vector3f wheelColor = stack.vectors.get(); + Vector3f wheelPosWS = stack.vectors.get(); + Vector3f axle = stack.vectors.get(); + Vector3f tmp = stack.vectors.get(); + + for (i = 0; i < vehicles.size(); i++) { + for (int v = 0; v < vehicles.get(i).getNumWheels(); v++) { + wheelColor.set(0, 255, 255); + if (vehicles.get(i).getWheelInfo(v).raycastInfo.isInContact) { + wheelColor.set(0, 0, 255); + } + else { + wheelColor.set(255, 0, 255); + } + + wheelPosWS.set(vehicles.get(i).getWheelInfo(v).worldTransform.origin); + + axle.set( + vehicles.get(i).getWheelInfo(v).worldTransform.basis.getElement(0, vehicles.get(i).getRightAxis()), + vehicles.get(i).getWheelInfo(v).worldTransform.basis.getElement(1, vehicles.get(i).getRightAxis()), + vehicles.get(i).getWheelInfo(v).worldTransform.basis.getElement(2, vehicles.get(i).getRightAxis())); + + + //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS + //debug wheels (cylinders) + tmp.add(wheelPosWS, axle); + debugDrawer.drawLine(wheelPosWS, tmp, wheelColor); + debugDrawer.drawLine(wheelPosWS, vehicles.get(i).getWheelInfo(v).raycastInfo.contactPointWS, wheelColor); + } + } + } + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void clearForces() { + // todo: iterate over awake simulation islands! + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + + RigidBody body = RigidBody.upcast(colObj); + if (body != null) { + body.clearForces(); + } + } + } + + /** + * Apply gravity, call this once per timestep. + */ + public void applyGravity() { + // todo: iterate over awake simulation islands! + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + + RigidBody body = RigidBody.upcast(colObj); + if (body != null && body.isActive()) { + body.applyGravity(); + } + } + } + + protected void synchronizeMotionStates() { + stack.transforms.push(); + try { + Transform interpolatedTransform = stack.transforms.get(); + + // todo: iterate over awake simulation islands! + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + + RigidBody body = RigidBody.upcast(colObj); + if (body != null && body.getMotionState() != null && !body.isStaticOrKinematicObject()) { + // we need to call the update at least once, even for sleeping objects + // otherwise the 'graphics' transform never updates properly + // so todo: add 'dirty' flag + //if (body->getActivationState() != ISLAND_SLEEPING) + { + TransformUtil.integrateTransform(body.getInterpolationWorldTransform(), + body.getInterpolationLinearVelocity(), body.getInterpolationAngularVelocity(), localTime, interpolatedTransform); + body.getMotionState().setWorldTransform(interpolatedTransform); + } + } + } + + if (getDebugDrawer() != null && (getDebugDrawer().getDebugMode() & DebugDrawModes.DRAW_WIREFRAME) != 0) { + for (int i = 0; i < vehicles.size(); i++) { + for (int v = 0; v < vehicles.get(i).getNumWheels(); v++) { + // synchronize the wheels with the (interpolated) chassis worldtransform + vehicles.get(i).updateWheelTransform(v, true); + } + } + } + } + finally { + stack.transforms.pop(); + } + } + + public long nanoTime() { + // JAU: Orig: return System.nanoTime(); + return System.currentTimeMillis()*1000000; + } + + @Override + public int stepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) { + startProfiling(timeStep); + + long t0 = nanoTime(); + + BulletGlobals.pushProfile("stepSimulation"); + try { + int numSimulationSubSteps = 0; + + if (maxSubSteps != 0) { + // fixed timestep with interpolation + localTime += timeStep; + if (localTime >= fixedTimeStep) { + numSimulationSubSteps = (int) (localTime / fixedTimeStep); + localTime -= numSimulationSubSteps * fixedTimeStep; + } + } + else { + //variable timestep + fixedTimeStep = timeStep; + localTime = timeStep; + if (ScalarUtil.fuzzyZero(timeStep)) { + numSimulationSubSteps = 0; + maxSubSteps = 0; + } + else { + numSimulationSubSteps = 1; + maxSubSteps = 1; + } + } + + // process some debugging flags + if (getDebugDrawer() != null) { + BulletGlobals.gDisableDeactivation = (getDebugDrawer().getDebugMode() & DebugDrawModes.NO_DEACTIVATION) != 0; + } + if (numSimulationSubSteps != 0) { + saveKinematicState(fixedTimeStep); + + applyGravity(); + + // clamp the number of substeps, to prevent simulation grinding spiralling down to a halt + int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps; + + for (int i = 0; i < clampedSimulationSteps; i++) { + internalSingleStepSimulation(fixedTimeStep); + synchronizeMotionStates(); + } + } + + synchronizeMotionStates(); + + clearForces(); + + // TODO: CProfileManager::Increment_Frame_Counter(); + + return numSimulationSubSteps; + } + finally { + BulletGlobals.popProfile(); + + BulletGlobals.stepSimulationTime = (nanoTime() - t0) / 1000000; + } + } + + protected void internalSingleStepSimulation(float timeStep) { + BulletGlobals.pushProfile("internalSingleStepSimulation"); + try { + // apply gravity, predict motion + predictUnconstraintMotion(timeStep); + + DispatcherInfo dispatchInfo = getDispatchInfo(); + + dispatchInfo.timeStep = timeStep; + dispatchInfo.stepCount = 0; + dispatchInfo.debugDraw = getDebugDrawer(); + + // perform collision detection + performDiscreteCollisionDetection(); + + calculateSimulationIslands(); + + getSolverInfo().timeStep = timeStep; + + // solve contact and other joint constraints + solveConstraints(getSolverInfo()); + + //CallbackTriggers(); + + // integrate transforms + integrateTransforms(timeStep); + + // update vehicle simulation + updateVehicles(timeStep); + + updateActivationState(timeStep); + } + finally { + BulletGlobals.popProfile(); + } + } + + @Override + public void setGravity(Vector3f gravity) { + this.gravity.set(gravity); + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + RigidBody body = RigidBody.upcast(colObj); + if (body != null) { + body.setGravity(gravity); + } + } + } + + @Override + public void removeRigidBody(RigidBody body) { + removeCollisionObject(body); + } + + @Override + public void addRigidBody(RigidBody body) { + if (!body.isStaticOrKinematicObject()) { + body.setGravity(gravity); + } + + if (body.getCollisionShape() != null) { + boolean isDynamic = !(body.isStaticObject() || body.isKinematicObject()); + short collisionFilterGroup = isDynamic ? (short) CollisionFilterGroups.DEFAULT_FILTER : (short) CollisionFilterGroups.STATIC_FILTER; + short collisionFilterMask = isDynamic ? (short) CollisionFilterGroups.ALL_FILTER : (short) (CollisionFilterGroups.ALL_FILTER ^ CollisionFilterGroups.STATIC_FILTER); + + addCollisionObject(body, collisionFilterGroup, collisionFilterMask); + } + } + + public void addRigidBody(RigidBody body, short group, short mask) { + if (!body.isStaticOrKinematicObject()) { + body.setGravity(gravity); + } + + if (body.getCollisionShape() != null) { + addCollisionObject(body, group, mask); + } + } + + protected void updateVehicles(float timeStep) { + BulletGlobals.pushProfile("updateVehicles"); + try { + for (int i = 0; i < vehicles.size(); i++) { + RaycastVehicle vehicle = vehicles.get(i); + vehicle.updateVehicle(timeStep); + } + } + finally { + BulletGlobals.popProfile(); + } + } + + protected void updateActivationState(float timeStep) { + BulletGlobals.pushProfile("updateActivationState"); + try { + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + RigidBody body = RigidBody.upcast(colObj); + if (body != null) { + body.updateDeactivation(timeStep); + + if (body.wantsSleeping()) { + if (body.isStaticOrKinematicObject()) { + body.setActivationState(CollisionObject.ISLAND_SLEEPING); + } + else { + if (body.getActivationState() == CollisionObject.ACTIVE_TAG) { + body.setActivationState(CollisionObject.WANTS_DEACTIVATION); + } + } + } + else { + if (body.getActivationState() != CollisionObject.DISABLE_DEACTIVATION) { + body.setActivationState(CollisionObject.ACTIVE_TAG); + } + } + } + } + } + finally { + BulletGlobals.popProfile(); + } + } + + @Override + public void addConstraint(TypedConstraint constraint, boolean disableCollisionsBetweenLinkedBodies) { + constraints.add(constraint); + if (disableCollisionsBetweenLinkedBodies) { + constraint.getRigidBodyA().addConstraintRef(constraint); + constraint.getRigidBodyB().addConstraintRef(constraint); + } + } + + @Override + public void removeConstraint(TypedConstraint constraint) { + constraints.remove(constraint); + constraint.getRigidBodyA().removeConstraintRef(constraint); + constraint.getRigidBodyB().removeConstraintRef(constraint); + } + + @Override + public void addVehicle(RaycastVehicle vehicle) { + vehicles.add(vehicle); + } + + @Override + public void removeVehicle(RaycastVehicle vehicle) { + vehicles.remove(vehicle); + } + + private static int getConstraintIslandId(TypedConstraint lhs) { + int islandId; + + CollisionObject rcolObj0 = lhs.getRigidBodyA(); + CollisionObject rcolObj1 = lhs.getRigidBodyB(); + islandId = rcolObj0.getIslandTag() >= 0 ? rcolObj0.getIslandTag() : rcolObj1.getIslandTag(); + return islandId; + } + + private static class InplaceSolverIslandCallback implements SimulationIslandManager.IslandCallback { + public ContactSolverInfo solverInfo; + public ConstraintSolver solver; + public List<TypedConstraint> sortedConstraints; + public int numConstraints; + public IDebugDraw debugDrawer; + //public StackAlloc* m_stackAlloc; + public Dispatcher dispatcher; + + public void init(ContactSolverInfo solverInfo, ConstraintSolver solver, List<TypedConstraint> sortedConstraints, int numConstraints, IDebugDraw debugDrawer, Dispatcher dispatcher) { + this.solverInfo = solverInfo; + this.solver = solver; + this.sortedConstraints = sortedConstraints; + this.numConstraints = numConstraints; + this.debugDrawer = debugDrawer; + this.dispatcher = dispatcher; + } + + public void processIsland(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifolds, int manifolds_offset, int numManifolds, int islandId) { + if (islandId < 0) { + // we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + solver.solveGroup(bodies, numBodies, manifolds, manifolds_offset, numManifolds, sortedConstraints, 0, numConstraints, solverInfo, debugDrawer/*,m_stackAlloc*/, dispatcher); + } + else { + // also add all non-contact constraints/joints for this island + //List<TypedConstraint> startConstraint = null; + int startConstraint_idx = -1; + int numCurConstraints = 0; + int i; + + // find the first constraint for this island + for (i = 0; i < numConstraints; i++) { + if (getConstraintIslandId(sortedConstraints.get(i)) == islandId) { + //startConstraint = &m_sortedConstraints[i]; + //startConstraint = sortedConstraints.subList(i, sortedConstraints.size()); + startConstraint_idx = i; + break; + } + } + // count the number of constraints in this island + for (; i < numConstraints; i++) { + if (getConstraintIslandId(sortedConstraints.get(i)) == islandId) { + numCurConstraints++; + } + } + + solver.solveGroup(bodies, numBodies, manifolds, manifolds_offset, numManifolds, sortedConstraints, startConstraint_idx, numCurConstraints, solverInfo, debugDrawer/*,m_stackAlloc*/, dispatcher); + } + } + } + + private List<TypedConstraint> sortedConstraints = new ArrayList<TypedConstraint>(); + private InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(); + + protected void solveConstraints(ContactSolverInfo solverInfo) { + BulletGlobals.pushProfile("solveConstraints"); + try { + // sorted version of all btTypedConstraint, based on islandId + sortedConstraints.clear(); + for (int i=0; i<constraints.size(); i++) { + sortedConstraints.add(constraints.get(i)); + } + //Collections.sort(sortedConstraints, sortConstraintOnIslandPredicate); + MiscUtil.heapSort(sortedConstraints, sortConstraintOnIslandPredicate); + + List<TypedConstraint> constraintsPtr = getNumConstraints() != 0 ? sortedConstraints : null; + + solverCallback.init(solverInfo, constraintSolver, constraintsPtr, sortedConstraints.size(), debugDrawer/*,m_stackAlloc*/, dispatcher1); + + constraintSolver.prepareSolve(getCollisionWorld().getNumCollisionObjects(), getCollisionWorld().getDispatcher().getNumManifolds()); + + // solve all the constraints for this island + islandManager.buildAndProcessIslands(getCollisionWorld().getDispatcher(), getCollisionWorld().getCollisionObjectArray(), solverCallback); + + constraintSolver.allSolved(solverInfo, debugDrawer/*, m_stackAlloc*/); + } + finally { + BulletGlobals.popProfile(); + } + } + + protected void calculateSimulationIslands() { + BulletGlobals.pushProfile("calculateSimulationIslands"); + try { + getSimulationIslandManager().updateActivationState(getCollisionWorld(), getCollisionWorld().getDispatcher()); + + { + int i; + int numConstraints = constraints.size(); + for (i = 0; i < numConstraints; i++) { + TypedConstraint constraint = constraints.get(i); + + RigidBody colObj0 = constraint.getRigidBodyA(); + RigidBody colObj1 = constraint.getRigidBodyB(); + + if (((colObj0 != null) && ((colObj0).mergesSimulationIslands())) && + ((colObj1 != null) && ((colObj1).mergesSimulationIslands()))) { + if (colObj0.isActive() || colObj1.isActive()) { + + getSimulationIslandManager().getUnionFind().unite((colObj0).getIslandTag(), + (colObj1).getIslandTag()); + } + } + } + } + + // Store the island id in each body + getSimulationIslandManager().storeIslandActivationState(getCollisionWorld()); + } + finally { + BulletGlobals.popProfile(); + } + } + + protected void integrateTransforms(float timeStep) { + BulletGlobals.pushProfile("integrateTransforms"); + stack.transforms.push(); + try { + Transform predictedTrans = stack.transforms.get(); + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + RigidBody body = RigidBody.upcast(colObj); + if (body != null) { + if (body.isActive() && (!body.isStaticOrKinematicObject())) { + body.predictIntegratedTransform(timeStep, predictedTrans); + body.proceedToTransform(predictedTrans); + } + } + } + } + finally { + stack.transforms.pop(); + BulletGlobals.popProfile(); + } + } + + protected void predictUnconstraintMotion(float timeStep) { + BulletGlobals.pushProfile("predictUnconstraintMotion"); + try { + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + RigidBody body = RigidBody.upcast(colObj); + if (body != null) { + if (!body.isStaticOrKinematicObject()) { + if (body.isActive()) { + body.integrateVelocities(timeStep); + // damping + body.applyDamping(timeStep); + + body.predictIntegratedTransform(timeStep, body.getInterpolationWorldTransform()); + } + } + } + } + } + finally { + BulletGlobals.popProfile(); + } + } + + protected void startProfiling(float timeStep) { + // TODO + } + + protected void debugDrawSphere(float radius, Transform transform, Vector3f color) { + stack.vectors.push(); + try { + Vector3f start = stack.vectors.get(transform.origin); + + Vector3f xoffs = stack.vectors.get(radius, 0, 0); + transform.basis.transform(xoffs); + Vector3f yoffs = stack.vectors.get(0, radius, 0); + transform.basis.transform(yoffs); + Vector3f zoffs = stack.vectors.get(0, 0, radius); + transform.basis.transform(zoffs); + + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + // XY + tmp1.sub(start, xoffs); + tmp2.add(start, yoffs); + getDebugDrawer().drawLine(tmp1, tmp2, color); + tmp1.add(start, yoffs); + tmp2.add(start, xoffs); + getDebugDrawer().drawLine(tmp1, tmp2, color); + tmp1.add(start, xoffs); + tmp2.sub(start, yoffs); + getDebugDrawer().drawLine(tmp1, tmp2, color); + tmp1.sub(start, yoffs); + tmp2.sub(start, xoffs); + getDebugDrawer().drawLine(tmp1, tmp2, color); + + // XZ + tmp1.sub(start, xoffs); + tmp2.add(start, zoffs); + getDebugDrawer().drawLine(tmp1, tmp2, color); + tmp1.add(start, zoffs); + tmp2.add(start, xoffs); + getDebugDrawer().drawLine(tmp1, tmp2, color); + tmp1.add(start, xoffs); + tmp2.sub(start, zoffs); + getDebugDrawer().drawLine(tmp1, tmp2, color); + tmp1.sub(start, zoffs); + tmp2.sub(start, xoffs); + getDebugDrawer().drawLine(tmp1, tmp2, color); + + // YZ + tmp1.sub(start, yoffs); + tmp2.add(start, zoffs); + getDebugDrawer().drawLine(tmp1, tmp2, color); + tmp1.add(start, zoffs); + tmp2.add(start, yoffs); + getDebugDrawer().drawLine(tmp1, tmp2, color); + tmp1.add(start, yoffs); + tmp2.sub(start, zoffs); + getDebugDrawer().drawLine(tmp1, tmp2, color); + tmp1.sub(start, zoffs); + tmp2.sub(start, yoffs); + getDebugDrawer().drawLine(tmp1, tmp2, color); + } + finally { + stack.vectors.pop(); + } + } + + public void debugDrawObject(Transform worldTransform, CollisionShape shape, Vector3f color) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + + // Draw a small simplex at the center of the object + { + Vector3f start = stack.vectors.get(worldTransform.origin); + + tmp.set(1f, 0f, 0f); + worldTransform.basis.transform(tmp); + tmp.add(start); + getDebugDrawer().drawLine(start, tmp, stack.vectors.get(1f, 0f, 0f)); + + tmp.set(0f, 1f, 0f); + worldTransform.basis.transform(tmp); + tmp.add(start); + getDebugDrawer().drawLine(start, tmp, stack.vectors.get(0f, 1f, 0f)); + + tmp.set(0f, 0f, 1f); + worldTransform.basis.transform(tmp); + tmp.add(start); + getDebugDrawer().drawLine(start, tmp, stack.vectors.get(0f, 0f, 1f)); + } + + // TODO: debugDrawObject + // if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) + // { + // const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape); + // for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--) + // { + // btTransform childTrans = compoundShape->getChildTransform(i); + // const btCollisionShape* colShape = compoundShape->getChildShape(i); + // debugDrawObject(worldTransform*childTrans,colShape,color); + // } + // + // } else + // { + // switch (shape->getShapeType()) + // { + // + // case SPHERE_SHAPE_PROXYTYPE: + // { + // const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); + // btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin + // + // debugDrawSphere(radius, worldTransform, color); + // break; + // } + // case MULTI_SPHERE_SHAPE_PROXYTYPE: + // { + // const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); + // + // for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) + // { + // btTransform childTransform = worldTransform; + // childTransform.getOrigin() += multiSphereShape->getSpherePosition(i); + // debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color); + // } + // + // break; + // } + // case CAPSULE_SHAPE_PROXYTYPE: + // { + // const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape); + // + // btScalar radius = capsuleShape->getRadius(); + // btScalar halfHeight = capsuleShape->getHalfHeight(); + // + // // Draw the ends + // { + // btTransform childTransform = worldTransform; + // childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0); + // debugDrawSphere(radius, childTransform, color); + // } + // + // { + // btTransform childTransform = worldTransform; + // childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0); + // debugDrawSphere(radius, childTransform, color); + // } + // + // // Draw some additional lines + // btVector3 start = worldTransform.getOrigin(); + // getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color); + // getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color); + // getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color); + // getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), color); + // + // break; + // } + // case CONE_SHAPE_PROXYTYPE: + // { + // const btConeShape* coneShape = static_cast<const btConeShape*>(shape); + // btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); + // btScalar height = coneShape->getHeight();//+coneShape->getMargin(); + // btVector3 start = worldTransform.getOrigin(); + // + // int upAxis= coneShape->getConeUpIndex(); + // + // + // btVector3 offsetHeight(0,0,0); + // offsetHeight[upAxis] = height * btScalar(0.5); + // btVector3 offsetRadius(0,0,0); + // offsetRadius[(upAxis+1)%3] = radius; + // btVector3 offset2Radius(0,0,0); + // offset2Radius[(upAxis+2)%3] = radius; + // + // getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); + // getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); + // getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color); + // getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color); + // + // + // + // break; + // + // } + // case CYLINDER_SHAPE_PROXYTYPE: + // { + // const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); + // int upAxis = cylinder->getUpAxis(); + // btScalar radius = cylinder->getRadius(); + // btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; + // btVector3 start = worldTransform.getOrigin(); + // btVector3 offsetHeight(0,0,0); + // offsetHeight[upAxis] = halfHeight; + // btVector3 offsetRadius(0,0,0); + // offsetRadius[(upAxis+1)%3] = radius; + // getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color); + // getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color); + // break; + // } + // default: + // { + // + // if (shape->isConcave()) + // { + // btConcaveShape* concaveMesh = (btConcaveShape*) shape; + // + // //todo pass camera, for some culling + // btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + // btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + // + // DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + // concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); + // + // } + // + // if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + // { + // btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; + // //todo: pass camera for some culling + // btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30)); + // btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30)); + // //DebugDrawcallback drawCallback; + // DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + // convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + // } + // + // + // /// for polyhedral shapes + // if (shape->isPolyhedral()) + // { + // btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; + // + // int i; + // for (i=0;i<polyshape->getNumEdges();i++) + // { + // btPoint3 a,b; + // polyshape->getEdge(i,a,b); + // btVector3 wa = worldTransform * a; + // btVector3 wb = worldTransform * b; + // getDebugDrawer()->drawLine(wa,wb,color); + // + // } + // + // + // } + // } + // } + // } + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void setConstraintSolver(ConstraintSolver solver) { + if (ownsConstraintSolver) { + //btAlignedFree( m_constraintSolver); + } + ownsConstraintSolver = false; + constraintSolver = solver; + } + + @Override + public ConstraintSolver getConstraintSolver() { + return constraintSolver; + } + + @Override + public int getNumConstraints() { + return constraints.size(); + } + + @Override + public TypedConstraint getConstraint(int index) { + return constraints.get(index); + } + + public SimulationIslandManager getSimulationIslandManager() { + return islandManager; + } + + public CollisionWorld getCollisionWorld() { + return this; + } + + @Override + public DynamicsWorldType getWorldType() { + return DynamicsWorldType.DISCRETE_DYNAMICS_WORLD; + } + + public ContactSolverInfo getSolverInfo() { + return solverInfo; + } + + //////////////////////////////////////////////////////////////////////////// + + private static final Comparator<TypedConstraint> sortConstraintOnIslandPredicate = new Comparator<TypedConstraint>() { + public int compare(TypedConstraint lhs, TypedConstraint rhs) { + int rIslandId0, lIslandId0; + rIslandId0 = getConstraintIslandId(rhs); + lIslandId0 = getConstraintIslandId(lhs); + return lIslandId0 < rIslandId0? -1 : +1; + } + }; + + private static class DebugDrawcallback implements TriangleCallback, InternalTriangleIndexCallback { + private IDebugDraw debugDrawer; + private final Vector3f color = new Vector3f(); + private final Transform worldTrans = new Transform(); + + public DebugDrawcallback(IDebugDraw debugDrawer, Transform worldTrans, Vector3f color) { + this.debugDrawer = debugDrawer; + this.worldTrans.set(worldTrans); + this.color.set(color); + } + + public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) { + processTriangle(triangle,partId,triangleIndex); + } + + private final Vector3f wv0 = new Vector3f(),wv1 = new Vector3f(),wv2 = new Vector3f(); + + public void processTriangle(Vector3f[] triangle, int partId, int triangleIndex) { + wv0.set(triangle[0]); + worldTrans.transform(wv0); + wv1.set(triangle[1]); + worldTrans.transform(wv1); + wv2.set(triangle[2]); + worldTrans.transform(wv2); + + debugDrawer.drawLine(wv0, wv1, color); + debugDrawer.drawLine(wv1, wv2, color); + debugDrawer.drawLine(wv2, wv0, color); + } + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/DynamicsWorld.java b/src/jbullet/src/javabullet/dynamics/DynamicsWorld.java new file mode 100644 index 0000000..ef9c324 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/DynamicsWorld.java @@ -0,0 +1,104 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics; + +import javabullet.collision.broadphase.BroadphaseInterface; +import javabullet.collision.broadphase.Dispatcher; +import javabullet.collision.dispatch.CollisionConfiguration; +import javabullet.collision.dispatch.CollisionWorld; +import javabullet.dynamics.constraintsolver.ConstraintSolver; +import javabullet.dynamics.constraintsolver.TypedConstraint; +import javabullet.dynamics.vehicle.RaycastVehicle; +import javax.vecmath.Vector3f; + +/** + * DynamicsWorld is the baseclass for several dynamics implementation, basic, discrete, parallel, and continuous. + * + * @author jezek2 + */ +public abstract class DynamicsWorld extends CollisionWorld { + + public DynamicsWorld(Dispatcher dispatcher, BroadphaseInterface broadphasePairCache, CollisionConfiguration collisionConfiguration) { + super(dispatcher, broadphasePairCache, collisionConfiguration); + } + + public final int stepSimulation(float timeStep) { + return stepSimulation(timeStep, 1, 1f / 60f); + } + + public final int stepSimulation(float timeStep, int maxSubSteps) { + return stepSimulation(timeStep, maxSubSteps, 1f / 60f); + } + + /** + * stepSimulation proceeds the simulation over timeStep units. + * if maxSubSteps > 0, it will interpolate time steps. + */ + public abstract int stepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep); + + public abstract void debugDrawWorld(); + + public final void addConstraint(TypedConstraint constraint) { + addConstraint(constraint, false); + } + + public void addConstraint(TypedConstraint constraint, boolean disableCollisionsBetweenLinkedBodies) { + } + + public void removeConstraint(TypedConstraint constraint) { + } + + public void addVehicle(RaycastVehicle vehicle) { + } + + public void removeVehicle(RaycastVehicle vehicle) { + } + + /** + * Once a rigidbody is added to the dynamics world, it will get this gravity assigned. + * Existing rigidbodies in the world get gravity assigned too, during this method. + */ + public abstract void setGravity(Vector3f gravity); + + public abstract void addRigidBody(RigidBody body); + + public abstract void removeRigidBody(RigidBody body); + + public abstract void setConstraintSolver(ConstraintSolver solver); + + public abstract ConstraintSolver getConstraintSolver(); + + public int getNumConstraints() { + return 0; + } + + public TypedConstraint getConstraint(int index) { + return null; + } + + public abstract DynamicsWorldType getWorldType(); + + public abstract void clearForces(); + +} diff --git a/src/jbullet/src/javabullet/dynamics/DynamicsWorldType.java b/src/jbullet/src/javabullet/dynamics/DynamicsWorldType.java new file mode 100644 index 0000000..011977d --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/DynamicsWorldType.java @@ -0,0 +1,34 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics; + +/** + * + * @author jezek2 + */ +public enum DynamicsWorldType { + SIMPLE_DYNAMICS_WORLD, + DISCRETE_DYNAMICS_WORLD, + CONTINUOUS_DYNAMICS_WORLD +} diff --git a/src/jbullet/src/javabullet/dynamics/RigidBody.java b/src/jbullet/src/javabullet/dynamics/RigidBody.java new file mode 100644 index 0000000..6e1a632 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/RigidBody.java @@ -0,0 +1,639 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics; + +import java.util.ArrayList; +import java.util.List; +import javabullet.BulletGlobals; +import javabullet.collision.broadphase.BroadphaseProxy; +import javabullet.collision.dispatch.CollisionFlags; +import javabullet.collision.dispatch.CollisionObject; +import javabullet.collision.shapes.CollisionShape; +import javabullet.dynamics.constraintsolver.TypedConstraint; +import javabullet.linearmath.MatrixUtil; +import javabullet.linearmath.MiscUtil; +import javabullet.linearmath.MotionState; +import javabullet.linearmath.Transform; +import javabullet.linearmath.TransformUtil; +import javax.vecmath.Matrix3f; +import javax.vecmath.Quat4f; +import javax.vecmath.Vector3f; + +/** + * RigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape. + * It is recommended for performance and memory use to share btCollisionShape objects whenever possible.<p> + * + * There are 3 types of rigid bodies:<br> + * - A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics.<br> + * - B) Fixed objects with zero mass. They are not moving (basically collision objects)<br> + * - C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.<p> + * + * Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time.<p> + * Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects). + * + * @author jezek2 + */ +public class RigidBody extends CollisionObject { + + private static final float MAX_ANGVEL = BulletGlobals.SIMD_HALF_PI; + + private final Matrix3f invInertiaTensorWorld = new Matrix3f(); + private final Vector3f linearVelocity = new Vector3f(); + private final Vector3f angularVelocity = new Vector3f(); + private float inverseMass; + private float angularFactor; + + private final Vector3f gravity = new Vector3f(); + private final Vector3f invInertiaLocal = new Vector3f(); + private final Vector3f totalForce = new Vector3f(); + private final Vector3f totalTorque = new Vector3f(); + + private float linearDamping; + private float angularDamping; + + private boolean additionalDamping; + private float additionalDampingFactor; + private float additionalLinearDampingThresholdSqr; + private float additionalAngularDampingThresholdSqr; + private float additionalAngularDampingFactor; + + private float linearSleepingThreshold; + private float angularSleepingThreshold; + + // optionalMotionState allows to automatic synchronize the world transform for active objects + private MotionState optionalMotionState; + + // keep track of typed constraints referencing this rigid body + private final List<TypedConstraint> constraintRefs = new ArrayList<TypedConstraint>(); + + // for experimental overriding of friction/contact solver func + public int contactSolverType; + public int frictionSolverType; + + private static int uniqueId = 0; + public int debugBodyId; + + public RigidBody(RigidBodyConstructionInfo constructionInfo) { + setupRigidBody(constructionInfo); + } + + public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape) { + this(mass, motionState, collisionShape, BulletGlobals.ZERO_VECTOR3); + } + + public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape, Vector3f localInertia) { + RigidBodyConstructionInfo cinfo = new RigidBodyConstructionInfo(mass, motionState, collisionShape, localInertia); + setupRigidBody(cinfo); + } + + private void setupRigidBody(RigidBodyConstructionInfo constructionInfo) { + linearVelocity.set(0f, 0f, 0f); + angularVelocity.set(0f, 0f, 0f); + angularFactor = 1f; + gravity.set(0f, 0f, 0f); + totalForce.set(0f, 0f, 0f); + totalTorque.set(0f, 0f, 0f); + linearDamping = 0f; + angularDamping = 0.5f; + linearSleepingThreshold = constructionInfo.linearSleepingThreshold; + angularSleepingThreshold = constructionInfo.angularSleepingThreshold; + optionalMotionState = constructionInfo.motionState; + contactSolverType = 0; + frictionSolverType = 0; + additionalDamping = constructionInfo.additionalDamping; + + additionalLinearDampingThresholdSqr = constructionInfo.additionalLinearDampingThresholdSqr; + additionalAngularDampingThresholdSqr = constructionInfo.additionalAngularDampingThresholdSqr; + additionalAngularDampingFactor = constructionInfo.additionalAngularDampingFactor; + + if (optionalMotionState != null) + { + optionalMotionState.getWorldTransform(worldTransform); + } else + { + worldTransform.set(constructionInfo.startWorldTransform); + } + + interpolationWorldTransform.set(worldTransform); + interpolationLinearVelocity.set(0f, 0f, 0f); + interpolationAngularVelocity.set(0f, 0f, 0f); + + // moved to CollisionObject + friction = constructionInfo.friction; + restitution = constructionInfo.restitution; + + collisionShape = constructionInfo.collisionShape; + debugBodyId = uniqueId++; + + // internalOwner is to allow upcasting from collision object to rigid body + internalOwner = this; + + setMassProps(constructionInfo.mass, constructionInfo.localInertia); + setDamping(constructionInfo.linearDamping, constructionInfo.angularDamping); + updateInertiaTensor(); + } + + public void destroy() { + // No constraints should point to this rigidbody + // Remove constraints from the dynamics world before you delete the related rigidbodies. + assert (constraintRefs.size() == 0); + } + + public void proceedToTransform(Transform newTrans) { + setCenterOfMassTransform(newTrans); + } + + /** + * To keep collision detection and dynamics separate we don't store a rigidbody pointer, + * but a rigidbody is derived from CollisionObject, so we can safely perform an upcast. + */ + public static RigidBody upcast(CollisionObject colObj) { + return (RigidBody) colObj.getInternalOwner(); + } + + /** + * Continuous collision detection needs prediction. + */ + public void predictIntegratedTransform(float timeStep, Transform predictedTransform) { + TransformUtil.integrateTransform(worldTransform, linearVelocity, angularVelocity, timeStep, predictedTransform); + } + + public void saveKinematicState(float timeStep) { + //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities + if (timeStep != 0f) { + //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform + if (getMotionState() != null) { + getMotionState().getWorldTransform(worldTransform); + } + //Vector3f linVel = new Vector3f(), angVel = new Vector3f(); + + TransformUtil.calculateVelocity(interpolationWorldTransform, worldTransform, timeStep, linearVelocity, angularVelocity); + interpolationLinearVelocity.set(linearVelocity); + interpolationAngularVelocity.set(angularVelocity); + interpolationWorldTransform.set(worldTransform); + //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ()); + } + } + + public void applyGravity() { + if (isStaticOrKinematicObject()) + return; + + applyCentralForce(gravity); + } + + public void setGravity(Vector3f acceleration) { + if (inverseMass != 0f) { + gravity.scale(1f / inverseMass, acceleration); + } + } + + public Vector3f getGravity() { + return gravity; + } + + public void setDamping(float lin_damping, float ang_damping) { + linearDamping = MiscUtil.GEN_clamped(lin_damping, 0f, 1f); + angularDamping = MiscUtil.GEN_clamped(ang_damping, 0f, 1f); + } + + /** + * Damps the velocity, using the given linearDamping and angularDamping. + */ + public void applyDamping(float timeStep) { + stack.vectors.push(); + try { + linearVelocity.scale(MiscUtil.GEN_clamped((1f - timeStep * linearDamping), 0f, 1f)); + angularVelocity.scale(MiscUtil.GEN_clamped((1f - timeStep * angularDamping), 0f, 1f)); + + if (additionalDamping) { + // Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc. + // Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete + if ((angularVelocity.lengthSquared() < additionalAngularDampingThresholdSqr) && + (linearVelocity.lengthSquared() < additionalLinearDampingThresholdSqr)) { + angularVelocity.scale(additionalDampingFactor); + linearVelocity.scale(additionalDampingFactor); + } + + float speed = linearVelocity.length(); + if (speed < linearDamping) { + float dampVel = 0.005f; + if (speed > dampVel) { + Vector3f dir = stack.vectors.get(linearVelocity); + dir.normalize(); + dir.scale(dampVel); + linearVelocity.sub(dir); + } + else { + linearVelocity.set(0f, 0f, 0f); + } + } + + float angSpeed = angularVelocity.length(); + if (angSpeed < angularDamping) { + float angDampVel = 0.005f; + if (angSpeed > angDampVel) { + Vector3f dir = stack.vectors.get(angularVelocity); + dir.normalize(); + dir.scale(angDampVel); + angularVelocity.sub(dir); + } + else { + angularVelocity.set(0f, 0f, 0f); + } + } + } + } + finally { + stack.vectors.pop(); + } + } + + public void setMassProps(float mass, Vector3f inertia) { + if (mass == 0f) { + collisionFlags |= CollisionFlags.STATIC_OBJECT; + inverseMass = 0f; + } + else { + collisionFlags &= (~CollisionFlags.STATIC_OBJECT); + inverseMass = 1f / mass; + } + + invInertiaLocal.set(inertia.x != 0f ? 1f / inertia.x : 0f, + inertia.y != 0f ? 1f / inertia.y : 0f, + inertia.z != 0f ? 1f / inertia.z : 0f); + } + + public float getInvMass() { + return inverseMass; + } + + public Matrix3f getInvInertiaTensorWorld() { + return invInertiaTensorWorld; + } + + public void integrateVelocities(float step) { + if (isStaticOrKinematicObject()) { + return; + } + + stack.vectors.push(); + try { + linearVelocity.scaleAdd(inverseMass * step, totalForce, linearVelocity); + Vector3f tmp = stack.vectors.get(totalTorque); + invInertiaTensorWorld.transform(tmp); + angularVelocity.scaleAdd(step, tmp, angularVelocity); + + // clamp angular velocity. collision calculations will fail on higher angular velocities + float angvel = angularVelocity.length(); + if (angvel * step > MAX_ANGVEL) { + angularVelocity.scale((MAX_ANGVEL / step) / angvel); + } + } + finally { + stack.vectors.pop(); + } + } + + public void setCenterOfMassTransform(Transform xform) { + if (isStaticOrKinematicObject()) { + interpolationWorldTransform.set(worldTransform); + } + else { + interpolationWorldTransform.set(xform); + } + interpolationLinearVelocity.set(getLinearVelocity()); + interpolationAngularVelocity.set(getAngularVelocity()); + worldTransform.set(xform); + updateInertiaTensor(); + } + + public void applyCentralForce(Vector3f force) { + totalForce.add(force); + } + + public Vector3f getInvInertiaDiagLocal() { + return invInertiaLocal; + } + + public void setInvInertiaDiagLocal(Vector3f diagInvInertia) { + invInertiaLocal.set(diagInvInertia); + } + + public void setSleepingThresholds(float linear, float angular) { + linearSleepingThreshold = linear; + angularSleepingThreshold = angular; + } + + public void applyTorque(Vector3f torque) { + totalTorque.add(torque); + } + + public void applyForce(Vector3f force, Vector3f rel_pos) { + stack.vectors.push(); + try { + applyCentralForce(force); + Vector3f tmp = stack.vectors.get(); + tmp.cross(rel_pos, force); + applyTorque(tmp); + } + finally { + stack.vectors.pop(); + } + } + + public void applyCentralImpulse(Vector3f impulse) { + linearVelocity.scaleAdd(inverseMass, impulse, linearVelocity); + } + + public void applyTorqueImpulse(Vector3f torque) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(torque); + invInertiaTensorWorld.transform(tmp); + angularVelocity.add(tmp); + } + finally { + stack.vectors.pop(); + } + } + + public void applyImpulse(Vector3f impulse, Vector3f rel_pos) { + stack.vectors.push(); + try { + if (inverseMass != 0f) { + applyCentralImpulse(impulse); + if (angularFactor != 0f) { + Vector3f tmp = stack.vectors.get(); + tmp.cross(rel_pos, impulse); + tmp.scale(angularFactor); + applyTorqueImpulse(tmp); + } + } + } + finally { + stack.vectors.pop(); + } + } + + /** + * Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position. + */ + public void internalApplyImpulse(Vector3f linearComponent, Vector3f angularComponent, float impulseMagnitude) { + if (inverseMass != 0f) { + linearVelocity.scaleAdd(impulseMagnitude, linearComponent, linearVelocity); + if (angularFactor != 0f) { + angularVelocity.scaleAdd(impulseMagnitude * angularFactor, angularComponent, angularVelocity); + } + } + } + + public void clearForces() { + totalForce.set(0f, 0f, 0f); + totalTorque.set(0f, 0f, 0f); + } + + public void updateInertiaTensor() { + stack.matrices.push(); + try { + Matrix3f mat1 = stack.matrices.get(); + MatrixUtil.scale(mat1, worldTransform.basis, invInertiaLocal); + + Matrix3f mat2 = stack.matrices.get(worldTransform.basis); + mat2.transpose(); + + invInertiaTensorWorld.mul(mat1, mat2); + } + finally { + stack.matrices.pop(); + } + } + + public Vector3f getCenterOfMassPosition() { + return worldTransform.origin; + } + + public Quat4f getOrientation() { + stack.quats.push(); + try { + Quat4f orn = stack.quats.get(); + MatrixUtil.getRotation(worldTransform.basis, orn); + return stack.quats.returning(orn); + } + finally { + stack.quats.pop(); + } + } + + public Transform getCenterOfMassTransform() { + return worldTransform; + } + + public Vector3f getLinearVelocity() { + return linearVelocity; + } + + public Vector3f getAngularVelocity() { + return angularVelocity; + } + + public void setLinearVelocity(Vector3f lin_vel) { + assert (collisionFlags != CollisionFlags.STATIC_OBJECT); + linearVelocity.set(lin_vel); + } + + public void setAngularVelocity(Vector3f ang_vel) { + assert (collisionFlags != CollisionFlags.STATIC_OBJECT); + angularVelocity.set(ang_vel); + } + + public Vector3f getVelocityInLocalPoint(Vector3f rel_pos) { + stack.vectors.push(); + try { + // we also calculate lin/ang velocity for kinematic objects + Vector3f vec = stack.vectors.get(); + vec.cross(angularVelocity, rel_pos); + vec.add(linearVelocity); + return stack.vectors.returning(vec); + + //for kinematic objects, we could also use use: + // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep; + } + finally { + stack.vectors.pop(); + } + } + + public void translate(Vector3f v) { + worldTransform.origin.add(v); + } + + public void getAabb(Vector3f aabbMin, Vector3f aabbMax) { + getCollisionShape().getAabb(worldTransform, aabbMin, aabbMax); + } + + public float computeImpulseDenominator(Vector3f pos, Vector3f normal) { + stack.vectors.push(); + try { + Vector3f r0 = stack.vectors.get(); + r0.sub(pos, getCenterOfMassPosition()); + + Vector3f c0 = stack.vectors.get(); + c0.cross(r0, normal); + + Vector3f tmp = stack.vectors.get(); + MatrixUtil.transposeTransform(tmp, c0, getInvInertiaTensorWorld()); + + Vector3f vec = stack.vectors.get(); + vec.cross(tmp, r0); + + return inverseMass + normal.dot(vec); + } + finally { + stack.vectors.pop(); + } + } + + public float computeAngularImpulseDenominator(Vector3f axis) { + stack.vectors.push(); + try { + Vector3f vec = stack.vectors.get(); + MatrixUtil.transposeTransform(vec, axis, getInvInertiaTensorWorld()); + return axis.dot(vec); + } + finally { + stack.vectors.pop(); + } + } + + public void updateDeactivation(float timeStep) { + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION)) { + return; + } + + if ((getLinearVelocity().lengthSquared() < linearSleepingThreshold * linearSleepingThreshold) && + (getAngularVelocity().lengthSquared() < angularSleepingThreshold * angularSleepingThreshold)) { + deactivationTime += timeStep; + } + else { + deactivationTime = 0f; + setActivationState(0); + } + } + + public boolean wantsSleeping() { + if (getActivationState() == DISABLE_DEACTIVATION) { + return false; + } + + // disable deactivation + if (BulletGlobals.gDisableDeactivation || (BulletGlobals.gDeactivationTime == 0f)) { + return false; + } + + if ((getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION)) { + return true; + } + + if (deactivationTime > BulletGlobals.gDeactivationTime) { + return true; + } + return false; + } + + public BroadphaseProxy getBroadphaseProxy() { + return broadphaseHandle; + } + + public void setNewBroadphaseProxy(BroadphaseProxy broadphaseProxy) { + this.broadphaseHandle = broadphaseProxy; + } + + public MotionState getMotionState() { + return optionalMotionState; + } + + public void setMotionState(MotionState motionState) { + this.optionalMotionState = motionState; + if (optionalMotionState != null) { + motionState.getWorldTransform(worldTransform); + } + } + + public void setAngularFactor(float angFac) { + angularFactor = angFac; + } + + public float getAngularFactor() { + return angularFactor; + } + + /** + * Is this rigidbody added to a CollisionWorld/DynamicsWorld/Broadphase? + */ + public boolean isInWorld() { + return (getBroadphaseProxy() != null); + } + + @Override + public boolean checkCollideWithOverride(CollisionObject co) { + // TODO: change to cast + RigidBody otherRb = RigidBody.upcast(co); + if (otherRb == null) { + return true; + } + + for (int i = 0; i < constraintRefs.size(); ++i) { + TypedConstraint c = constraintRefs.get(i); + if (c.getRigidBodyA() == otherRb || c.getRigidBodyB() == otherRb) { + return false; + } + } + + return true; + } + + public void addConstraintRef(TypedConstraint c) { + int index = constraintRefs.indexOf(c); + if (index == -1) { + constraintRefs.add(c); + } + + checkCollideWith = true; + } + + public void removeConstraintRef(TypedConstraint c) { + constraintRefs.remove(c); + checkCollideWith = (constraintRefs.size() > 0); + } + + public TypedConstraint getConstraintRef(int index) { + return constraintRefs.get(index); + } + + public int getNumConstraintRefs() { + return constraintRefs.size(); + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/RigidBodyConstructionInfo.java b/src/jbullet/src/javabullet/dynamics/RigidBodyConstructionInfo.java new file mode 100644 index 0000000..420eebc --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/RigidBodyConstructionInfo.java @@ -0,0 +1,92 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics; + +import javabullet.BulletGlobals; +import javabullet.collision.shapes.CollisionShape; +import javabullet.linearmath.MotionState; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * RigidBodyConstructionInfo provides information to create a rigid body.<p> + * + * Setting mass to zero creates a fixed (non-dynamic) rigid body. For dynamic objects, you can + * use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument).<p> + * + * You can use the motion state to synchronize the world transform between physics and graphics objects. + * And if the motion state is provided, the rigid body will initialize its initial world transform from + * the motion state, startWorldTransform is only used when you don't provide a motion state. + * + * @author jezek2 + */ +public class RigidBodyConstructionInfo { + + public float mass; + + /** + * When a motionState is provided, the rigid body will initialize its world transform + * from the motion state. In this case, startWorldTransform is ignored. + */ + public MotionState motionState; + public final Transform startWorldTransform = new Transform(); + + public CollisionShape collisionShape; + public final Vector3f localInertia = new Vector3f(); + public float linearDamping = 0f; + public float angularDamping = 0f; + + /** Best simulation results when friction is non-zero. */ + public float friction = 0.5f; + /** Best simulation results using zero restitution. */ + public float restitution = 0f; + + public float linearSleepingThreshold = 0.8f; + public float angularSleepingThreshold = 1.0f; + + /** + * Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc. + * Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics + * system has improved, this should become obsolete. + */ + public boolean additionalDamping = false; + public float additionalDampingFactor = 0.005f; + public float additionalLinearDampingThresholdSqr = 0.01f; + public float additionalAngularDampingThresholdSqr = 0.01f; + public float additionalAngularDampingFactor = 0.01f; + + public RigidBodyConstructionInfo(float mass, MotionState motionState, CollisionShape collisionShape) { + this(mass, motionState, collisionShape, BulletGlobals.ZERO_VECTOR3); + } + + public RigidBodyConstructionInfo(float mass, MotionState motionState, CollisionShape collisionShape, Vector3f localInertia) { + this.mass = mass; + this.motionState = motionState; + this.collisionShape = collisionShape; + this.localInertia.set(localInertia); + + startWorldTransform.setIdentity(); + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/SimpleDynamicsWorld.java b/src/jbullet/src/javabullet/dynamics/SimpleDynamicsWorld.java new file mode 100644 index 0000000..33a4ca8 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/SimpleDynamicsWorld.java @@ -0,0 +1,236 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics; + +import java.util.List; +import javabullet.collision.broadphase.BroadphaseInterface; +import javabullet.collision.broadphase.Dispatcher; +import javabullet.collision.broadphase.DispatcherInfo; +import javabullet.collision.dispatch.CollisionConfiguration; +import javabullet.collision.dispatch.CollisionDispatcher; +import javabullet.collision.dispatch.CollisionObject; +import javabullet.collision.narrowphase.PersistentManifold; +import javabullet.dynamics.constraintsolver.ConstraintSolver; +import javabullet.dynamics.constraintsolver.ContactSolverInfo; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * SimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds. + * Please use DiscreteDynamicsWorld instead (or ContinuousDynamicsWorld once it is finished). + * + * @author jezek2 + */ +public class SimpleDynamicsWorld extends DynamicsWorld { + + protected ConstraintSolver constraintSolver; + protected boolean ownsConstraintSolver; + protected final Vector3f gravity = new Vector3f(0f, 0f, -10f); + + public SimpleDynamicsWorld(Dispatcher dispatcher, BroadphaseInterface pairCache, ConstraintSolver constraintSolver, CollisionConfiguration collisionConfiguration) { + super(dispatcher, pairCache, collisionConfiguration); + this.constraintSolver = constraintSolver; + this.ownsConstraintSolver = false; + } + + protected void predictUnconstraintMotion(float timeStep) { + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + RigidBody body = RigidBody.upcast(colObj); + if (body != null) { + if (!body.isStaticObject()) { + if (body.isActive()) { + body.applyGravity(); + body.integrateVelocities(timeStep); + body.applyDamping(timeStep); + body.predictIntegratedTransform(timeStep, body.getInterpolationWorldTransform()); + } + } + } + } + } + + protected void integrateTransforms(float timeStep) { + stack.transforms.push(); + try { + Transform predictedTrans = stack.transforms.get(); + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + RigidBody body = RigidBody.upcast(colObj); + if (body != null) { + if (body.isActive() && (!body.isStaticObject())) { + body.predictIntegratedTransform(timeStep, predictedTrans); + body.proceedToTransform(predictedTrans); + } + } + } + } + finally { + stack.transforms.pop(); + } + } + + /** + * maxSubSteps/fixedTimeStep for interpolation is currently ignored for SimpleDynamicsWorld, use DiscreteDynamicsWorld instead. + */ + @Override + public int stepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep) { + // apply gravity, predict motion + predictUnconstraintMotion(timeStep); + + DispatcherInfo dispatchInfo = getDispatchInfo(); + dispatchInfo.timeStep = timeStep; + dispatchInfo.stepCount = 0; + dispatchInfo.debugDraw = getDebugDrawer(); + + // perform collision detection + performDiscreteCollisionDetection(); + + // solve contact constraints + int numManifolds = dispatcher1.getNumManifolds(); + if (numManifolds != 0) + { + List<PersistentManifold> manifoldPtr = ((CollisionDispatcher)dispatcher1).getInternalManifoldPointer(); + + ContactSolverInfo infoGlobal = new ContactSolverInfo(); + infoGlobal.timeStep = timeStep; + constraintSolver.prepareSolve(0,numManifolds); + constraintSolver.solveGroup(null,0,manifoldPtr, 0, numManifolds, null,0,0,infoGlobal,debugDrawer/*, m_stackAlloc*/,dispatcher1); + constraintSolver.allSolved(infoGlobal,debugDrawer/*, m_stackAlloc*/); + } + + // integrate transforms + integrateTransforms(timeStep); + + updateAabbs(); + + synchronizeMotionStates(); + + clearForces(); + + return 1; + } + + @Override + public void clearForces() { + // todo: iterate over awake simulation islands! + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + + RigidBody body = RigidBody.upcast(colObj); + if (body != null) { + body.clearForces(); + } + } + } + + @Override + public void setGravity(Vector3f gravity) { + this.gravity.set(gravity); + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + RigidBody body = RigidBody.upcast(colObj); + if (body != null) { + body.setGravity(gravity); + } + } + } + + @Override + public void addRigidBody(RigidBody body) { + body.setGravity(gravity); + + if (body.getCollisionShape() != null) { + addCollisionObject(body); + } + } + + @Override + public void removeRigidBody(RigidBody body) { + removeCollisionObject(body); + } + + @Override + public void updateAabbs() { + stack.pushCommonMath(); + try { + Transform predictedTrans = stack.transforms.get(); + Vector3f minAabb = stack.vectors.get(), maxAabb = stack.vectors.get(); + + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + RigidBody body = RigidBody.upcast(colObj); + if (body != null) { + if (body.isActive() && (!body.isStaticObject())) { + colObj.getCollisionShape().getAabb(colObj.getWorldTransform(), minAabb, maxAabb); + BroadphaseInterface bp = getBroadphase(); + bp.setAabb(body.getBroadphaseHandle(), minAabb, maxAabb, dispatcher1); + } + } + } + } + finally { + stack.popCommonMath(); + } + } + + public void synchronizeMotionStates() { + // todo: iterate over awake simulation islands! + for (int i = 0; i < collisionObjects.size(); i++) { + CollisionObject colObj = collisionObjects.get(i); + RigidBody body = RigidBody.upcast(colObj); + if (body != null && body.getMotionState() != null) { + if (body.getActivationState() != CollisionObject.ISLAND_SLEEPING) { + body.getMotionState().setWorldTransform(body.getWorldTransform()); + } + } + } + } + + @Override + public void setConstraintSolver(ConstraintSolver solver) { + if (ownsConstraintSolver) { + //btAlignedFree(m_constraintSolver); + } + + ownsConstraintSolver = false; + constraintSolver = solver; + } + + @Override + public ConstraintSolver getConstraintSolver() { + return constraintSolver; + } + + @Override + public void debugDrawWorld() { + // TODO: throw new UnsupportedOperationException("Not supported yet."); + } + + @Override + public DynamicsWorldType getWorldType() { + throw new UnsupportedOperationException("Not supported yet."); + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ConeTwistConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ConeTwistConstraint.java new file mode 100644 index 0000000..4bf81c1 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ConeTwistConstraint.java @@ -0,0 +1,419 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + * + * Written by: Marcus Hennix + */ + +package javabullet.dynamics.constraintsolver; + +import javabullet.BulletGlobals; +import javabullet.dynamics.RigidBody; +import javabullet.linearmath.QuaternionUtil; +import javabullet.linearmath.ScalarUtil; +import javabullet.linearmath.Transform; +import javabullet.linearmath.TransformUtil; +import javax.vecmath.Matrix3f; +import javax.vecmath.Quat4f; +import javax.vecmath.Vector3f; + +/** + * ConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc). + * + * @author jezek2 + */ +public class ConeTwistConstraint extends TypedConstraint { + + private JacobianEntry[] jac/*[3]*/ = new JacobianEntry[] { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() }; //3 orthogonal linear constraints + + private final Transform rbAFrame = new Transform(); + private final Transform rbBFrame = new Transform(); + + private float limitSoftness; + private float biasFactor; + private float relaxationFactor; + + private float swingSpan1; + private float swingSpan2; + private float twistSpan; + + private final Vector3f swingAxis = new Vector3f(); + private final Vector3f twistAxis = new Vector3f(); + + private float kSwing; + private float kTwist; + + private float twistLimitSign; + private float swingCorrection; + private float twistCorrection; + + private float accSwingLimitImpulse; + private float accTwistLimitImpulse; + + private boolean angularOnly = false; + private boolean solveTwistLimit; + private boolean solveSwingLimit; + + public ConeTwistConstraint() { + super(TypedConstraintType.CONETWIST_CONSTRAINT_TYPE); + } + + public ConeTwistConstraint(RigidBody rbA, RigidBody rbB, Transform rbAFrame, Transform rbBFrame) { + super(TypedConstraintType.CONETWIST_CONSTRAINT_TYPE, rbA, rbB); + this.rbAFrame.set(rbAFrame); + this.rbBFrame.set(rbBFrame); + + // flip axis for correct angles + this.rbBFrame.basis.m10 *= -1f; + this.rbBFrame.basis.m11 *= -1f; + this.rbBFrame.basis.m12 *= -1f; + + swingSpan1 = 1e30f; + swingSpan2 = 1e30f; + twistSpan = 1e30f; + biasFactor = 0.3f; + relaxationFactor = 1.0f; + + solveTwistLimit = false; + solveSwingLimit = false; + } + + public ConeTwistConstraint(RigidBody rbA, Transform rbAFrame) { + super(TypedConstraintType.CONETWIST_CONSTRAINT_TYPE, rbA); + this.rbAFrame.set(rbAFrame); + this.rbBFrame.set(this.rbAFrame); + + // flip axis for correct angles + this.rbBFrame.basis.m10 *= -1f; + this.rbBFrame.basis.m11 *= -1f; + this.rbBFrame.basis.m12 *= -1f; + + this.rbBFrame.basis.m20 *= -1f; + this.rbBFrame.basis.m21 *= -1f; + this.rbBFrame.basis.m22 *= -1f; + + swingSpan1 = 1e30f; + swingSpan2 = 1e30f; + twistSpan = 1e30f; + biasFactor = 0.3f; + relaxationFactor = 1.0f; + + solveTwistLimit = false; + solveSwingLimit = false; + } + + @Override + public void buildJacobian() { + stack.pushCommonMath(); + stack.quats.push(); + try { + Vector3f tmp = stack.vectors.get(); + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + appliedImpulse = 0f; + + // set bias, sign, clear accumulator + swingCorrection = 0f; + twistLimitSign = 0f; + solveTwistLimit = false; + solveSwingLimit = false; + accTwistLimitImpulse = 0f; + accSwingLimitImpulse = 0f; + + if (!angularOnly) { + Vector3f pivotAInW = stack.vectors.get(rbAFrame.origin); + rbA.getCenterOfMassTransform().transform(pivotAInW); + + Vector3f pivotBInW = stack.vectors.get(rbBFrame.origin); + rbB.getCenterOfMassTransform().transform(pivotBInW); + + Vector3f relPos = stack.vectors.get(); + relPos.sub(pivotBInW, pivotAInW); + + // TODO: stack + Vector3f[] normal/*[3]*/ = new Vector3f[]{stack.vectors.get(), stack.vectors.get(), stack.vectors.get()}; + if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) { + normal[0].normalize(relPos); + } + else { + normal[0].set(1f, 0f, 0f); + } + + TransformUtil.planeSpace1(normal[0], normal[1], normal[2]); + + for (int i = 0; i < 3; i++) { + Matrix3f mat1 = stack.matrices.get(rbA.getCenterOfMassTransform().basis); + mat1.transpose(); + + Matrix3f mat2 = stack.matrices.get(rbB.getCenterOfMassTransform().basis); + mat2.transpose(); + + tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition()); + tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition()); + + jac[i].init( + mat1, + mat2, + tmp1, + tmp2, + normal[i], + rbA.getInvInertiaDiagLocal(), + rbA.getInvMass(), + rbB.getInvInertiaDiagLocal(), + rbB.getInvMass()); + } + } + + Vector3f b1Axis1 = stack.vectors.get(), b1Axis2 = stack.vectors.get(), b1Axis3 = stack.vectors.get(); + Vector3f b2Axis1 = stack.vectors.get(), b2Axis2 = stack.vectors.get(); + + rbAFrame.basis.getColumn(0, b1Axis1); + getRigidBodyA().getCenterOfMassTransform().basis.transform(b1Axis1); + + rbBFrame.basis.getColumn(0, b2Axis1); + getRigidBodyB().getCenterOfMassTransform().basis.transform(b2Axis1); + + float swing1 = 0f, swing2 = 0f; + + // Get Frame into world space + if (swingSpan1 >= 0.05f) { + rbAFrame.basis.getColumn(1, b1Axis2); + getRigidBodyA().getCenterOfMassTransform().basis.transform(b1Axis2); + swing1 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis2), b2Axis1.dot(b1Axis1)); + } + + if (swingSpan2 >= 0.05f) { + rbAFrame.basis.getColumn(2, b1Axis3); + getRigidBodyA().getCenterOfMassTransform().basis.transform(b1Axis3); + swing2 = ScalarUtil.atan2Fast(b2Axis1.dot(b1Axis3), b2Axis1.dot(b1Axis1)); + } + + float RMaxAngle1Sq = 1.0f / (swingSpan1 * swingSpan1); + float RMaxAngle2Sq = 1.0f / (swingSpan2 * swingSpan2); + float EllipseAngle = Math.abs(swing1) * RMaxAngle1Sq + Math.abs(swing2) * RMaxAngle2Sq; + + if (EllipseAngle > 1.0f) { + swingCorrection = EllipseAngle - 1.0f; + solveSwingLimit = true; + + // Calculate necessary axis & factors + tmp1.scale(b2Axis1.dot(b1Axis2), b1Axis2); + tmp2.scale(b2Axis1.dot(b1Axis3), b1Axis3); + tmp.add(tmp1, tmp2); + swingAxis.cross(b2Axis1, tmp); + swingAxis.normalize(); + + float swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f; + swingAxis.scale(swingAxisSign); + + kSwing = 1f / (getRigidBodyA().computeAngularImpulseDenominator(swingAxis) + + getRigidBodyB().computeAngularImpulseDenominator(swingAxis)); + + } + + // Twist limits + if (twistSpan >= 0f) { + //Vector3f b2Axis2 = stack.vectors.get(); + rbBFrame.basis.getColumn(1, b2Axis2); + getRigidBodyB().getCenterOfMassTransform().basis.transform(b2Axis2); + + Quat4f rotationArc = stack.quats.get(QuaternionUtil.shortestArcQuat(b2Axis1, b1Axis1)); + Vector3f TwistRef = stack.vectors.get(QuaternionUtil.quatRotate(rotationArc, b2Axis2)); + float twist = ScalarUtil.atan2Fast(TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2)); + + float lockedFreeFactor = (twistSpan > 0.05f) ? limitSoftness : 0f; + if (twist <= -twistSpan * lockedFreeFactor) { + twistCorrection = -(twist + twistSpan); + solveTwistLimit = true; + + twistAxis.add(b2Axis1, b1Axis1); + twistAxis.scale(0.5f); + twistAxis.normalize(); + twistAxis.scale(-1.0f); + + kTwist = 1f / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis) + + getRigidBodyB().computeAngularImpulseDenominator(twistAxis)); + + } + else if (twist > twistSpan * lockedFreeFactor) { + twistCorrection = (twist - twistSpan); + solveTwistLimit = true; + + twistAxis.add(b2Axis1, b1Axis1); + twistAxis.scale(0.5f); + twistAxis.normalize(); + + kTwist = 1f / (getRigidBodyA().computeAngularImpulseDenominator(twistAxis) + + getRigidBodyB().computeAngularImpulseDenominator(twistAxis)); + } + } + } + finally { + stack.popCommonMath(); + stack.quats.pop(); + } + } + + @Override + public void solveConstraint(float timeStep) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + Vector3f pivotAInW = stack.vectors.get(rbAFrame.origin); + rbA.getCenterOfMassTransform().transform(pivotAInW); + + Vector3f pivotBInW = stack.vectors.get(rbBFrame.origin); + rbB.getCenterOfMassTransform().transform(pivotBInW); + + float tau = 0.3f; + + // linear part + if (!angularOnly) { + Vector3f rel_pos1 = stack.vectors.get(); + rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition()); + + Vector3f rel_pos2 = stack.vectors.get(); + rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition()); + + Vector3f vel1 = stack.vectors.get(rbA.getVelocityInLocalPoint(rel_pos1)); + Vector3f vel2 = stack.vectors.get(rbB.getVelocityInLocalPoint(rel_pos2)); + Vector3f vel = stack.vectors.get(); + vel.sub(vel1, vel2); + + for (int i = 0; i < 3; i++) { + Vector3f normal = jac[i].linearJointAxis; + float jacDiagABInv = 1f / jac[i].getDiagonal(); + + float rel_vel; + rel_vel = normal.dot(vel); + // positional error (zeroth order error) + tmp.sub(pivotAInW, pivotBInW); + float depth = -(tmp).dot(normal); // this is the error projected on the normal + float impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + appliedImpulse += impulse; + Vector3f impulse_vector = stack.vectors.get(); + impulse_vector.scale(impulse, normal); + + tmp.sub(pivotAInW, rbA.getCenterOfMassPosition()); + rbA.applyImpulse(impulse_vector, tmp); + + tmp.negate(impulse_vector); + tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition()); + rbB.applyImpulse(tmp, tmp2); + } + } + + { + // solve angular part + Vector3f angVelA = getRigidBodyA().getAngularVelocity(); + Vector3f angVelB = getRigidBodyB().getAngularVelocity(); + + // solve swing limit + if (solveSwingLimit) { + tmp.sub(angVelB, angVelA); + float amplitude = ((tmp).dot(swingAxis) * relaxationFactor * relaxationFactor + swingCorrection * (1f / timeStep) * biasFactor); + float impulseMag = amplitude * kSwing; + + // Clamp the accumulated impulse + float temp = accSwingLimitImpulse; + accSwingLimitImpulse = Math.max(accSwingLimitImpulse + impulseMag, 0.0f); + impulseMag = accSwingLimitImpulse - temp; + + Vector3f impulse = stack.vectors.get(); + impulse.scale(impulseMag, swingAxis); + + rbA.applyTorqueImpulse(impulse); + + tmp.negate(impulse); + rbB.applyTorqueImpulse(tmp); + } + + // solve twist limit + if (solveTwistLimit) { + tmp.sub(angVelB, angVelA); + float amplitude = ((tmp).dot(twistAxis) * relaxationFactor * relaxationFactor + twistCorrection * (1f / timeStep) * biasFactor); + float impulseMag = amplitude * kTwist; + + // Clamp the accumulated impulse + float temp = accTwistLimitImpulse; + accTwistLimitImpulse = Math.max(accTwistLimitImpulse + impulseMag, 0.0f); + impulseMag = accTwistLimitImpulse - temp; + + Vector3f impulse = stack.vectors.get(); + impulse.scale(impulseMag, twistAxis); + + rbA.applyTorqueImpulse(impulse); + + tmp.negate(impulse); + rbB.applyTorqueImpulse(tmp); + } + } + } + finally { + stack.vectors.pop(); + } + } + + public void updateRHS(float timeStep) { + } + + public void setAngularOnly(boolean angularOnly) { + this.angularOnly = angularOnly; + } + + public void setLimit(float _swingSpan1, float _swingSpan2, float _twistSpan) { + setLimit(_swingSpan1, _swingSpan2, _twistSpan, 0.8f, 0.3f, 1.0f); + } + + public void setLimit(float _swingSpan1, float _swingSpan2, float _twistSpan, float _softness, float _biasFactor, float _relaxationFactor) { + swingSpan1 = _swingSpan1; + swingSpan2 = _swingSpan2; + twistSpan = _twistSpan; + + limitSoftness = _softness; + biasFactor = _biasFactor; + relaxationFactor = _relaxationFactor; + } + + public Transform getAFrame() { + return rbAFrame; + } + + public Transform getBFrame() { + return rbBFrame; + } + + public boolean getSolveTwistLimit() { + return solveTwistLimit; + } + + public boolean getSolveSwingLimit() { + return solveTwistLimit; + } + + public float getTwistLimitSign() { + return twistLimitSign; + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ConstraintPersistentData.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ConstraintPersistentData.java new file mode 100644 index 0000000..b8e7220 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ConstraintPersistentData.java @@ -0,0 +1,90 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class ConstraintPersistentData { + + /** total applied impulse during most recent frame */ + public float appliedImpulse = 0f; + public float prevAppliedImpulse = 0f; + public float accumulatedTangentImpulse0 = 0f; + public float accumulatedTangentImpulse1 = 0f; + + public float jacDiagABInv = 0f; + public float jacDiagABInvTangent0; + public float jacDiagABInvTangent1; + public int persistentLifeTime = 0; + public float restitution = 0f; + public float friction = 0f; + public float penetration = 0f; + public final Vector3f frictionWorldTangential0 = new Vector3f(); + public final Vector3f frictionWorldTangential1 = new Vector3f(); + + public final Vector3f frictionAngularComponent0A = new Vector3f(); + public final Vector3f frictionAngularComponent0B = new Vector3f(); + public final Vector3f frictionAngularComponent1A = new Vector3f(); + public final Vector3f frictionAngularComponent1B = new Vector3f(); + + //some data doesn't need to be persistent over frames: todo: clean/reuse this + public final Vector3f angularComponentA = new Vector3f(); + public final Vector3f angularComponentB = new Vector3f(); + + public ContactSolverFunc contactSolverFunc = null; + public ContactSolverFunc frictionSolverFunc = null; + + public void reset() { + appliedImpulse = 0f; + prevAppliedImpulse = 0f; + accumulatedTangentImpulse0 = 0f; + accumulatedTangentImpulse1 = 0f; + + jacDiagABInv = 0f; + jacDiagABInvTangent0 = 0f; + jacDiagABInvTangent1 = 0f; + persistentLifeTime = 0; + restitution = 0f; + friction = 0f; + penetration = 0f; + frictionWorldTangential0.set(0f, 0f, 0f); + frictionWorldTangential1.set(0f, 0f, 0f); + + frictionAngularComponent0A.set(0f, 0f, 0f); + frictionAngularComponent0B.set(0f, 0f, 0f); + frictionAngularComponent1A.set(0f, 0f, 0f); + frictionAngularComponent1B.set(0f, 0f, 0f); + + angularComponentA.set(0f, 0f, 0f); + angularComponentB.set(0f, 0f, 0f); + + contactSolverFunc = null; + frictionSolverFunc = null; + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ConstraintSolver.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ConstraintSolver.java new file mode 100644 index 0000000..84062c3 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ConstraintSolver.java @@ -0,0 +1,55 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +import java.util.List; +import javabullet.BulletStack; +import javabullet.collision.broadphase.Dispatcher; +import javabullet.collision.dispatch.CollisionObject; +import javabullet.collision.narrowphase.PersistentManifold; +import javabullet.linearmath.IDebugDraw; + +/** + * + * @author jezek2 + */ +public abstract class ConstraintSolver { + + protected final BulletStack stack = BulletStack.get(); + + public void prepareSolve (int numBodies, int numManifolds) {} + + /** + * Solve a group of constraints. + */ + public abstract float solveGroup(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifold, int manifold_offset, int numManifolds, List<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo info, IDebugDraw debugDrawer/*, btStackAlloc* stackAlloc*/, Dispatcher dispatcher); + + public void allSolved(ContactSolverInfo info, IDebugDraw debugDrawer/*, btStackAlloc* stackAlloc*/) {} + + /** + * Clear internal cached data and reset random seed. + */ + public abstract void reset(); + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactConstraint.java new file mode 100644 index 0000000..ea07897 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactConstraint.java @@ -0,0 +1,460 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +import javabullet.BulletGlobals; +import javabullet.BulletPool; +import javabullet.BulletStack; +import javabullet.ObjectPool; +import javabullet.collision.narrowphase.ManifoldPoint; +import javabullet.dynamics.RigidBody; +import javax.vecmath.Matrix3f; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class ContactConstraint { + + public static final ContactSolverFunc resolveSingleCollision = new ContactSolverFunc() { + public float invoke(RigidBody body1, RigidBody body2, ManifoldPoint contactPoint, ContactSolverInfo info) { + return resolveSingleCollision(body1, body2, contactPoint, info); + } + }; + + public static final ContactSolverFunc resolveSingleFriction = new ContactSolverFunc() { + public float invoke(RigidBody body1, RigidBody body2, ManifoldPoint contactPoint, ContactSolverInfo info) { + return resolveSingleFriction(body1, body2, contactPoint, info); + } + }; + + public static final ContactSolverFunc resolveSingleCollisionCombined = new ContactSolverFunc() { + public float invoke(RigidBody body1, RigidBody body2, ManifoldPoint contactPoint, ContactSolverInfo info) { + return resolveSingleCollisionCombined(body1, body2, contactPoint, info); + } + }; + + /** + * Bilateral constraint between two dynamic objects. + */ + public static void resolveSingleBilateral(RigidBody body1, Vector3f pos1, + RigidBody body2, Vector3f pos2, + float distance, Vector3f normal, float[] impulse, float timeStep) { + float normalLenSqr = normal.lengthSquared(); + assert (Math.abs(normalLenSqr) < 1.1f); + if (normalLenSqr > 1.1f) { + impulse[0] = 0f; + return; + } + + BulletStack stack = BulletStack.get(); + ObjectPool<JacobianEntry> jacobiansPool = BulletPool.get(JacobianEntry.class); + + stack.pushCommonMath(); + try { + Vector3f rel_pos1 = stack.vectors.get(); + rel_pos1.sub(pos1, body1.getCenterOfMassPosition()); + + Vector3f rel_pos2 = stack.vectors.get(); + rel_pos2.sub(pos2, body2.getCenterOfMassPosition()); + + //this jacobian entry could be re-used for all iterations + + Vector3f vel1 = stack.vectors.get(); + vel1.set(body1.getVelocityInLocalPoint(rel_pos1)); + + Vector3f vel2 = stack.vectors.get(); + vel2.set(body2.getVelocityInLocalPoint(rel_pos2)); + + Vector3f vel = stack.vectors.get(); + vel.sub(vel1, vel2); + + Matrix3f mat1 = stack.matrices.get(body1.getCenterOfMassTransform().basis); + mat1.transpose(); + + Matrix3f mat2 = stack.matrices.get(body2.getCenterOfMassTransform().basis); + mat2.transpose(); + + JacobianEntry jac = jacobiansPool.get(); + jac.init(mat1, mat2, + rel_pos1, rel_pos2, normal, body1.getInvInertiaDiagLocal(), body1.getInvMass(), + body2.getInvInertiaDiagLocal(), body2.getInvMass()); + + float jacDiagAB = jac.getDiagonal(); + float jacDiagABInv = 1f / jacDiagAB; + + Vector3f tmp1 = stack.vectors.get(body1.getAngularVelocity()); + mat1.transform(tmp1); + + Vector3f tmp2 = stack.vectors.get(body2.getAngularVelocity()); + mat2.transform(tmp2); + + float rel_vel = jac.getRelativeVelocity( + body1.getLinearVelocity(), + tmp1, + body2.getLinearVelocity(), + tmp2); + + jacobiansPool.release(jac); + + float a; + a = jacDiagABInv; + + + rel_vel = normal.dot(vel); + + // todo: move this into proper structure + float contactDamping = 0.2f; + + //#ifdef ONLY_USE_LINEAR_MASS + // btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass()); + // impulse = - contactDamping * rel_vel * massTerm; + //#else + float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; + impulse[0] = velocityImpulse; + //#endif + } + finally { + stack.popCommonMath(); + } + } + + /** + * Response between two dynamic objects with friction. + */ + public static float resolveSingleCollision( + RigidBody body1, + RigidBody body2, + ManifoldPoint contactPoint, + ContactSolverInfo solverInfo) { + + BulletStack stack = BulletStack.get(); + + stack.vectors.push(); + try { + Vector3f pos1_ = contactPoint.getPositionWorldOnA(); + Vector3f pos2_ = contactPoint.getPositionWorldOnB(); + Vector3f normal = contactPoint.normalWorldOnB; + + // constant over all iterations + Vector3f rel_pos1 = stack.vectors.get(); + rel_pos1.sub(pos1_, body1.getCenterOfMassPosition()); + + Vector3f rel_pos2 = stack.vectors.get(); + rel_pos2.sub(pos2_, body2.getCenterOfMassPosition()); + + Vector3f vel1 = stack.vectors.get(body1.getVelocityInLocalPoint(rel_pos1)); + Vector3f vel2 = stack.vectors.get(body2.getVelocityInLocalPoint(rel_pos2)); + Vector3f vel = stack.vectors.get(); + vel.sub(vel1, vel2); + + float rel_vel; + rel_vel = normal.dot(vel); + + float Kfps = 1f / solverInfo.timeStep; + + // btScalar damping = solverInfo.m_damping ; + float Kerp = solverInfo.erp; + float Kcor = Kerp * Kfps; + + ConstraintPersistentData cpd = (ConstraintPersistentData) contactPoint.userPersistentData; + assert (cpd != null); + float distance = cpd.penetration; + float positionalError = Kcor * -distance; + float velocityError = cpd.restitution - rel_vel; // * damping; + + float penetrationImpulse = positionalError * cpd.jacDiagABInv; + + float velocityImpulse = velocityError * cpd.jacDiagABInv; + + float normalImpulse = penetrationImpulse + velocityImpulse; + + // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse + float oldNormalImpulse = cpd.appliedImpulse; + float sum = oldNormalImpulse + normalImpulse; + cpd.appliedImpulse = 0f > sum ? 0f : sum; + + normalImpulse = cpd.appliedImpulse - oldNormalImpulse; + + //#ifdef USE_INTERNAL_APPLY_IMPULSE + Vector3f tmp = stack.vectors.get(); + if (body1.getInvMass() != 0f) { + tmp.scale(body1.getInvMass(), contactPoint.normalWorldOnB); + body1.internalApplyImpulse(tmp, cpd.angularComponentA, normalImpulse); + } + if (body2.getInvMass() != 0f) { + tmp.scale(body2.getInvMass(), contactPoint.normalWorldOnB); + body2.internalApplyImpulse(tmp, cpd.angularComponentB, -normalImpulse); + } + //#else //USE_INTERNAL_APPLY_IMPULSE + // body1.applyImpulse(normal*(normalImpulse), rel_pos1); + // body2.applyImpulse(-normal*(normalImpulse), rel_pos2); + //#endif //USE_INTERNAL_APPLY_IMPULSE + + return normalImpulse; + } + finally { + stack.vectors.pop(); + } + } + + public static float resolveSingleFriction( + RigidBody body1, + RigidBody body2, + ManifoldPoint contactPoint, + ContactSolverInfo solverInfo) { + + BulletStack stack = BulletStack.get(); + + stack.vectors.push(); + try { + Vector3f pos1 = contactPoint.getPositionWorldOnA(); + Vector3f pos2 = contactPoint.getPositionWorldOnB(); + + Vector3f rel_pos1 = stack.vectors.get(); + rel_pos1.sub(pos1, body1.getCenterOfMassPosition()); + + Vector3f rel_pos2 = stack.vectors.get(); + rel_pos2.sub(pos2, body2.getCenterOfMassPosition()); + + ConstraintPersistentData cpd = (ConstraintPersistentData) contactPoint.userPersistentData; + assert (cpd != null); + + float combinedFriction = cpd.friction; + + float limit = cpd.appliedImpulse * combinedFriction; + + if (cpd.appliedImpulse > 0f) //friction + { + //apply friction in the 2 tangential directions + + // 1st tangent + Vector3f vel1 = stack.vectors.get(); + vel1.set(body1.getVelocityInLocalPoint(rel_pos1)); + + Vector3f vel2 = stack.vectors.get(); + vel2.set(body2.getVelocityInLocalPoint(rel_pos2)); + + Vector3f vel = stack.vectors.get(); + vel.sub(vel1, vel2); + + float j1, j2; + + { + float vrel = cpd.frictionWorldTangential0.dot(vel); + + // calculate j that moves us to zero relative velocity + j1 = -vrel * cpd.jacDiagABInvTangent0; + float oldTangentImpulse = cpd.accumulatedTangentImpulse0; + cpd.accumulatedTangentImpulse0 = oldTangentImpulse + j1; + + cpd.accumulatedTangentImpulse0 = Math.min(cpd.accumulatedTangentImpulse0, limit); + cpd.accumulatedTangentImpulse0 = Math.max(cpd.accumulatedTangentImpulse0, -limit); + j1 = cpd.accumulatedTangentImpulse0 - oldTangentImpulse; + } + { + // 2nd tangent + + float vrel = cpd.frictionWorldTangential1.dot(vel); + + // calculate j that moves us to zero relative velocity + j2 = -vrel * cpd.jacDiagABInvTangent1; + float oldTangentImpulse = cpd.accumulatedTangentImpulse1; + cpd.accumulatedTangentImpulse1 = oldTangentImpulse + j2; + + cpd.accumulatedTangentImpulse1 = Math.min(cpd.accumulatedTangentImpulse1, limit); + cpd.accumulatedTangentImpulse1 = Math.max(cpd.accumulatedTangentImpulse1, -limit); + j2 = cpd.accumulatedTangentImpulse1 - oldTangentImpulse; + } + + //#ifdef USE_INTERNAL_APPLY_IMPULSE + Vector3f tmp = stack.vectors.get(); + + if (body1.getInvMass() != 0f) { + tmp.scale(body1.getInvMass(), cpd.frictionWorldTangential0); + body1.internalApplyImpulse(tmp, cpd.frictionAngularComponent0A, j1); + + tmp.scale(body1.getInvMass(), cpd.frictionWorldTangential1); + body1.internalApplyImpulse(tmp, cpd.frictionAngularComponent1A, j2); + } + if (body2.getInvMass() != 0f) { + tmp.scale(body2.getInvMass(), cpd.frictionWorldTangential0); + body2.internalApplyImpulse(tmp, cpd.frictionAngularComponent0B, -j1); + + tmp.scale(body2.getInvMass(), cpd.frictionWorldTangential1); + body2.internalApplyImpulse(tmp, cpd.frictionAngularComponent1B, -j2); + } + //#else //USE_INTERNAL_APPLY_IMPULSE + // body1.applyImpulse((j1 * cpd->m_frictionWorldTangential0)+(j2 * cpd->m_frictionWorldTangential1), rel_pos1); + // body2.applyImpulse((j1 * -cpd->m_frictionWorldTangential0)+(j2 * -cpd->m_frictionWorldTangential1), rel_pos2); + //#endif //USE_INTERNAL_APPLY_IMPULSE + } + return cpd.appliedImpulse; + } + finally { + stack.vectors.pop(); + } + } + + /** + * velocity + friction<br> + * response between two dynamic objects with friction + */ + public static float resolveSingleCollisionCombined( + RigidBody body1, + RigidBody body2, + ManifoldPoint contactPoint, + ContactSolverInfo solverInfo) { + + BulletStack stack = BulletStack.get(); + + stack.vectors.push(); + try { + Vector3f pos1 = contactPoint.getPositionWorldOnA(); + Vector3f pos2 = contactPoint.getPositionWorldOnB(); + Vector3f normal = contactPoint.normalWorldOnB; + + Vector3f rel_pos1 = stack.vectors.get(); + rel_pos1.sub(pos1, body1.getCenterOfMassPosition()); + + Vector3f rel_pos2 = stack.vectors.get(); + rel_pos2.sub(pos2, body2.getCenterOfMassPosition()); + + Vector3f vel1 = stack.vectors.get(body1.getVelocityInLocalPoint(rel_pos1)); + Vector3f vel2 = stack.vectors.get(body2.getVelocityInLocalPoint(rel_pos2)); + Vector3f vel = stack.vectors.get(); + vel.sub(vel1, vel2); + + float rel_vel; + rel_vel = normal.dot(vel); + + float Kfps = 1f / solverInfo.timeStep; + + //btScalar damping = solverInfo.m_damping ; + float Kerp = solverInfo.erp; + float Kcor = Kerp * Kfps; + + ConstraintPersistentData cpd = (ConstraintPersistentData) contactPoint.userPersistentData; + assert (cpd != null); + float distance = cpd.penetration; + float positionalError = Kcor * -distance; + float velocityError = cpd.restitution - rel_vel;// * damping; + + float penetrationImpulse = positionalError * cpd.jacDiagABInv; + + float velocityImpulse = velocityError * cpd.jacDiagABInv; + + float normalImpulse = penetrationImpulse + velocityImpulse; + + // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse + float oldNormalImpulse = cpd.appliedImpulse; + float sum = oldNormalImpulse + normalImpulse; + cpd.appliedImpulse = 0f > sum ? 0f : sum; + + normalImpulse = cpd.appliedImpulse - oldNormalImpulse; + + + //#ifdef USE_INTERNAL_APPLY_IMPULSE + Vector3f tmp = stack.vectors.get(); + if (body1.getInvMass() != 0f) { + tmp.scale(body1.getInvMass(), contactPoint.normalWorldOnB); + body1.internalApplyImpulse(tmp, cpd.angularComponentA, normalImpulse); + } + if (body2.getInvMass() != 0f) { + tmp.scale(body2.getInvMass(), contactPoint.normalWorldOnB); + body2.internalApplyImpulse(tmp, cpd.angularComponentB, -normalImpulse); + } + //#else //USE_INTERNAL_APPLY_IMPULSE + // body1.applyImpulse(normal*(normalImpulse), rel_pos1); + // body2.applyImpulse(-normal*(normalImpulse), rel_pos2); + //#endif //USE_INTERNAL_APPLY_IMPULSE + + { + //friction + vel1.set(body1.getVelocityInLocalPoint(rel_pos1)); + vel2.set(body2.getVelocityInLocalPoint(rel_pos2)); + vel.sub(vel1, vel2); + + rel_vel = normal.dot(vel); + + tmp.scale(rel_vel, normal); + Vector3f lat_vel = stack.vectors.get(); + lat_vel.sub(vel, tmp); + float lat_rel_vel = lat_vel.length(); + + float combinedFriction = cpd.friction; + + if (cpd.appliedImpulse > 0) { + if (lat_rel_vel > BulletGlobals.FLT_EPSILON) { + lat_vel.scale(1f / lat_rel_vel); + + Vector3f temp1 = stack.vectors.get(); + temp1.cross(rel_pos1, lat_vel); + body1.getInvInertiaTensorWorld().transform(temp1); + + Vector3f temp2 = stack.vectors.get(); + temp2.cross(rel_pos2, lat_vel); + body2.getInvInertiaTensorWorld().transform(temp2); + + Vector3f java_tmp1 = stack.vectors.get(); + java_tmp1.cross(temp1, rel_pos1); + + Vector3f java_tmp2 = stack.vectors.get(); + java_tmp2.cross(temp2, rel_pos2); + + tmp.add(java_tmp1, java_tmp2); + + float friction_impulse = lat_rel_vel / + (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(tmp)); + float normal_impulse = cpd.appliedImpulse * combinedFriction; + + friction_impulse = Math.min(friction_impulse, normal_impulse); + friction_impulse = Math.max(friction_impulse, -normal_impulse); + + tmp.scale(-friction_impulse, lat_vel); + body1.applyImpulse(tmp, rel_pos1); + + tmp.scale(friction_impulse, lat_vel); + body2.applyImpulse(tmp, rel_pos2); + } + } + } + + return normalImpulse; + } + finally { + stack.vectors.pop(); + } + } + + public static float resolveSingleFrictionEmpty( + RigidBody body1, + RigidBody body2, + ManifoldPoint contactPoint, + ContactSolverInfo solverInfo) { + return 0f; + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactConstraintEnum.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactConstraintEnum.java new file mode 100644 index 0000000..3382637 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactConstraintEnum.java @@ -0,0 +1,37 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +/** + * TODO: name + * + * @author jezek2 + */ +public enum ContactConstraintEnum { + DEFAULT_CONTACT_SOLVER_TYPE, + CONTACT_SOLVER_TYPE1, + CONTACT_SOLVER_TYPE2, + USER_CONTACT_SOLVER_TYPE1, + MAX_CONTACT_SOLVER_TYPES +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactSolverFunc.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactSolverFunc.java new file mode 100644 index 0000000..97799e1 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactSolverFunc.java @@ -0,0 +1,37 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +import javabullet.collision.narrowphase.ManifoldPoint; +import javabullet.dynamics.RigidBody; + +/** + * + * @author jezek2 + */ +public interface ContactSolverFunc { + + public float invoke(RigidBody body1, RigidBody body2, ManifoldPoint contactPoint, ContactSolverInfo info); + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactSolverInfo.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactSolverInfo.java new file mode 100644 index 0000000..7915516 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/ContactSolverInfo.java @@ -0,0 +1,57 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +/** + * + * @author jezek2 + */ +public class ContactSolverInfo { + + public float tau = 0.6f; + public float damping = 1f; + public float friction = 0.3f; + public float timeStep; + public float restitution = 0f; + public int numIterations = 10; + public float maxErrorReduction = 20f; + public float sor = 1.3f; + public float erp = 0.4f; + + public ContactSolverInfo() { + } + + public ContactSolverInfo(ContactSolverInfo g) { + tau = g.tau; + damping = g.damping; + friction = g.friction; + timeStep = g.timeStep; + restitution = g.restitution; + numIterations = g.numIterations; + maxErrorReduction = g.maxErrorReduction; + sor = g.sor; + erp = g.erp; + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/Generic6DofConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/Generic6DofConstraint.java new file mode 100644 index 0000000..46e9b93 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/Generic6DofConstraint.java @@ -0,0 +1,505 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +/* +2007-09-09 +btGeneric6DofConstraint Refactored by Francisco Le�n +email: [email protected] +http://gimpact.sf.net +*/ + +package javabullet.dynamics.constraintsolver; + +import javabullet.BulletGlobals; +import javabullet.dynamics.RigidBody; +import javabullet.linearmath.MatrixUtil; +import javabullet.linearmath.Transform; + + +/// +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Matrix3f; +import javax.vecmath.Vector3f; +/*! + +*/ +/** + * Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space.<p> + * Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'. + * Currently this limit supports rotational motors.<br> + * <ul> + * <li>For Linear limits, use Generic6DofConstraint.setLinearUpperLimit, Generic6DofConstraint.setLinearLowerLimit. You can set the parameters with the TranslationalLimitMotor structure accsesible through the Generic6DofConstraint.getTranslationalLimitMotor method. + * At this moment translational motors are not supported. May be in the future.</li> + * + * <li>For Angular limits, use the RotationalLimitMotor structure for configuring the limit. + * This is accessible through Generic6DofConstraint.getLimitMotor method, + * This brings support for limit parameters and motors.</li> + * + * <li>Angulars limits have these possible ranges: + * <table border=1 > + * <tr> + * <td><b>AXIS</b></td> + * <td><b>MIN ANGLE</b></td> + * <td><b>MAX ANGLE</b></td> + * </tr><tr> + * <td>X</td> + * <td>-PI</td> + * <td>PI</td> + * </tr><tr> + * <td>Y</td> + * <td>-PI/2</td> + * <td>PI/2</td> + * </tr><tr> + * <td>Z</td> + * <td>-PI/2</td> + * <td>PI/2</td> + * </tr> + * </table> + * </li> + * </ul> + * + * @author jezek2 + */ +public class Generic6DofConstraint extends TypedConstraint { + + protected final Transform frameInA = new Transform(); //!< the constraint space w.r.t body A + protected final Transform frameInB = new Transform(); //!< the constraint space w.r.t body B + + protected final JacobianEntry[] jacLinear/*[3]*/ = new JacobianEntry[] { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() }; //!< 3 orthogonal linear constraints + protected final JacobianEntry[] jacAng/*[3]*/ = new JacobianEntry[] { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() }; //!< 3 orthogonal angular constraints + + protected final TranslationalLimitMotor linearLimits = new TranslationalLimitMotor(); + + protected final RotationalLimitMotor[] angularLimits/*[3]*/ = new RotationalLimitMotor[] { new RotationalLimitMotor(), new RotationalLimitMotor(), new RotationalLimitMotor() }; + + protected float timeStep; + protected final Transform calculatedTransformA = new Transform(); + protected final Transform calculatedTransformB = new Transform(); + protected final Vector3f calculatedAxisAngleDiff = new Vector3f(); + protected final Vector3f[] calculatedAxis/*[3]*/ = new Vector3f[] { new Vector3f(), new Vector3f(), new Vector3f() }; + + protected boolean useLinearReferenceFrameA; + + public Generic6DofConstraint() { + super(TypedConstraintType.D6_CONSTRAINT_TYPE); + useLinearReferenceFrameA = true; + } + + public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Transform frameInA, Transform frameInB, boolean useLinearReferenceFrameA) { + super(TypedConstraintType.D6_CONSTRAINT_TYPE, rbA, rbB); + this.frameInA.set(frameInA); + this.frameInB.set(frameInB); + this.useLinearReferenceFrameA = useLinearReferenceFrameA; + } + + private static float getMatrixElem(Matrix3f mat, int index) { + int i = index % 3; + int j = index / 3; + return mat.getElement(i, j); + } + + /** + * MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html + */ + private static boolean matrixToEulerXYZ(Matrix3f mat, Vector3f xyz) { + // // rot = cy*cz -cy*sz sy + // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx + // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy + // + + if (getMatrixElem(mat, 2) < 1.0f) { + if (getMatrixElem(mat, 2) > -1.0f) { + xyz.x = (float) Math.atan2(-getMatrixElem(mat, 5), getMatrixElem(mat, 8)); + xyz.y = (float) Math.asin(getMatrixElem(mat, 2)); + xyz.z = (float) Math.atan2(-getMatrixElem(mat, 1), getMatrixElem(mat, 0)); + return true; + } + else { + // WARNING. Not unique. XA - ZA = -atan2(r10,r11) + xyz.x = -(float) Math.atan2(getMatrixElem(mat, 3), getMatrixElem(mat, 4)); + xyz.y = -BulletGlobals.SIMD_HALF_PI; + xyz.z = 0.0f; + return false; + } + } + else { + // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11) + xyz.x = (float) Math.atan2(getMatrixElem(mat, 3), getMatrixElem(mat, 4)); + xyz.y = BulletGlobals.SIMD_HALF_PI; + xyz.z = 0.0f; + } + + return false; + } + + /** + * Calcs the euler angles between the two bodies. + */ + protected void calculateAngleInfo() { + stack.pushCommonMath(); + try { + Matrix3f mat = stack.matrices.get(); + + Matrix3f relative_frame = stack.matrices.get(); + mat.set(calculatedTransformA.basis); + MatrixUtil.invert(mat); + relative_frame.mul(mat, calculatedTransformB.basis); + + matrixToEulerXYZ(relative_frame, calculatedAxisAngleDiff); + + // in euler angle mode we do not actually constrain the angular velocity + // along the axes axis[0] and axis[2] (although we do use axis[1]) : + // + // to get constrain w2-w1 along ...not + // ------ --------------------- ------ + // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0] + // d(angle[1])/dt = 0 ax[1] + // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] + // + // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. + // to prove the result for angle[0], write the expression for angle[0] from + // GetInfo1 then take the derivative. to prove this for angle[2] it is + // easier to take the euler rate expression for d(angle[2])/dt with respect + // to the components of w and set that to 0. + + Vector3f axis0 = stack.vectors.get(); + calculatedTransformB.basis.getColumn(0, axis0); + + Vector3f axis2 = stack.vectors.get(); + calculatedTransformA.basis.getColumn(2, axis2); + + calculatedAxis[1].cross(axis2, axis0); + calculatedAxis[0].cross(calculatedAxis[1], axis2); + calculatedAxis[2].cross(axis0, calculatedAxis[1]); + + // if(m_debugDrawer) + // { + // + // char buff[300]; + // sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ", + // m_calculatedAxisAngleDiff[0], + // m_calculatedAxisAngleDiff[1], + // m_calculatedAxisAngleDiff[2]); + // m_debugDrawer->reportErrorWarning(buff); + // } + } + finally { + stack.popCommonMath(); + } + } + + /** + * Calcs global transform of the offsets.<p> + * Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. + * + * See also: Generic6DofConstraint.getCalculatedTransformA, Generic6DofConstraint.getCalculatedTransformB, Generic6DofConstraint.calculateAngleInfo + */ + public void calculateTransforms() { + calculatedTransformA.set(rbA.getCenterOfMassTransform()); + calculatedTransformA.mul(frameInA); + + calculatedTransformB.set(rbB.getCenterOfMassTransform()); + calculatedTransformB.mul(frameInB); + + calculateAngleInfo(); + } + + protected void buildLinearJacobian(/*JacobianEntry jacLinear*/int jacLinear_index, Vector3f normalWorld, Vector3f pivotAInW, Vector3f pivotBInW) { + stack.pushCommonMath(); + try { + Matrix3f mat1 = stack.matrices.get(rbA.getCenterOfMassTransform().basis); + mat1.transpose(); + + Matrix3f mat2 = stack.matrices.get(rbB.getCenterOfMassTransform().basis); + mat2.transpose(); + + Vector3f tmp1 = stack.vectors.get(); + tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition()); + + Vector3f tmp2 = stack.vectors.get(); + tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition()); + + jacLinear[jacLinear_index].init( + mat1, + mat2, + tmp1, + tmp2, + normalWorld, + rbA.getInvInertiaDiagLocal(), + rbA.getInvMass(), + rbB.getInvInertiaDiagLocal(), + rbB.getInvMass()); + } + finally { + stack.popCommonMath(); + } + } + + protected void buildAngularJacobian(/*JacobianEntry jacAngular*/int jacAngular_index, Vector3f jointAxisW) { + stack.matrices.push(); + try { + Matrix3f mat1 = stack.matrices.get(rbA.getCenterOfMassTransform().basis); + mat1.transpose(); + + Matrix3f mat2 = stack.matrices.get(rbB.getCenterOfMassTransform().basis); + mat2.transpose(); + + jacAng[jacAngular_index].init(jointAxisW, + mat1, + mat2, + rbA.getInvInertiaDiagLocal(), + rbB.getInvInertiaDiagLocal()); + } + finally { + stack.matrices.pop(); + } + } + + /** + * Test angular limit.<p> + * Calculates angular correction and returns true if limit needs to be corrected. + * Generic6DofConstraint.buildJacobian must be called previously. + */ + public boolean testAngularLimitMotor(int axis_index) { + float angle = VectorUtil.getCoord(calculatedAxisAngleDiff, axis_index); + + // test limits + angularLimits[axis_index].testLimitValue(angle); + return angularLimits[axis_index].needApplyTorques(); + } + + @Override + public void buildJacobian() { + stack.vectors.push(); + try { + // calculates transform + calculateTransforms(); + + Vector3f pivotAInW = calculatedTransformA.origin; + Vector3f pivotBInW = calculatedTransformB.origin; + + Vector3f rel_pos1 = stack.vectors.get(); + rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition()); + + Vector3f rel_pos2 = stack.vectors.get(); + rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition()); + + Vector3f normalWorld = stack.vectors.get(); + int i; + // linear part + for (i = 0; i < 3; i++) { + if (linearLimits.isLimited(i)) { + if (useLinearReferenceFrameA) { + calculatedTransformA.basis.getColumn(i, normalWorld); + } + else { + calculatedTransformB.basis.getColumn(i, normalWorld); + } + + buildLinearJacobian( + /*jacLinear[i]*/i, normalWorld, + pivotAInW, pivotBInW); + + } + } + + // angular part + for (i = 0; i < 3; i++) { + // calculates error angle + if (testAngularLimitMotor(i)) { + normalWorld.set(this.getAxis(i)); + // Create angular atom + buildAngularJacobian(/*jacAng[i]*/i, normalWorld); + } + } + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void solveConstraint(float timeStep) { + stack.vectors.push(); + try { + this.timeStep = timeStep; + + //calculateTransforms(); + + int i; + + // linear + + Vector3f pointInA = stack.vectors.get(calculatedTransformA.origin); + Vector3f pointInB = stack.vectors.get(calculatedTransformB.origin); + + float jacDiagABInv; + Vector3f linear_axis = stack.vectors.get(); + for (i = 0; i < 3; i++) { + if (linearLimits.isLimited(i)) { + jacDiagABInv = 1f / jacLinear[i].getDiagonal(); + + if (useLinearReferenceFrameA) { + calculatedTransformA.basis.getColumn(i, linear_axis); + } + else { + calculatedTransformB.basis.getColumn(i, linear_axis); + } + + linearLimits.solveLinearAxis( + this.timeStep, + jacDiagABInv, + rbA, pointInA, + rbB, pointInB, + i, linear_axis); + + } + } + + // angular + Vector3f angular_axis = stack.vectors.get(); + float angularJacDiagABInv; + for (i = 0; i < 3; i++) { + if (angularLimits[i].needApplyTorques()) { + // get axis + angular_axis.set(getAxis(i)); + + angularJacDiagABInv = 1f / jacAng[i].getDiagonal(); + + angularLimits[i].solveAngularLimits(this.timeStep, angular_axis, angularJacDiagABInv, rbA, rbB); + } + } + } + finally { + stack.vectors.pop(); + } + } + + + public void updateRHS(float timeStep) { + } + + /** + * Get the rotation axis in global coordinates. + * Generic6DofConstraint.buildJacobian must be called previously. + */ + public Vector3f getAxis(int axis_index) { + return calculatedAxis[axis_index]; + } + + /** + * Get the relative Euler angle. + * Generic6DofConstraint.buildJacobian must be called previously. + */ + public float getAngle(int axis_index) { + return VectorUtil.getCoord(calculatedAxisAngleDiff, axis_index); + } + + /** + * Gets the global transform of the offset for body A.<p> + * See also: Generic6DofConstraint.getFrameOffsetA, Generic6DofConstraint.getFrameOffsetB, Generic6DofConstraint.calculateAngleInfo. + */ + public Transform getCalculatedTransformA() { + return calculatedTransformA; + } + + /** + * Gets the global transform of the offset for body B.<p> + * See also: Generic6DofConstraint.getFrameOffsetA, Generic6DofConstraint.getFrameOffsetB, Generic6DofConstraint.calculateAngleInfo. + */ + public Transform getCalculatedTransformB() { + return calculatedTransformB; + } + + public Transform getFrameOffsetA() { + return frameInA; + } + + public Transform getFrameOffsetB() { + return frameInB; + } + + public void setLinearLowerLimit(Vector3f linearLower) { + linearLimits.lowerLimit.set(linearLower); + } + + public void setLinearUpperLimit(Vector3f linearUpper) { + linearLimits.upperLimit.set(linearUpper); + } + + public void setAngularLowerLimit(Vector3f angularLower) { + angularLimits[0].loLimit = angularLower.x; + angularLimits[1].loLimit = angularLower.y; + angularLimits[2].loLimit = angularLower.z; + } + + public void setAngularUpperLimit(Vector3f angularUpper) { + angularLimits[0].hiLimit = angularUpper.x; + angularLimits[1].hiLimit = angularUpper.y; + angularLimits[2].hiLimit = angularUpper.z; + } + + /** + * Retrieves the angular limit informacion. + */ + public RotationalLimitMotor getRotationalLimitMotor(int index) { + return angularLimits[index]; + } + + /** + * Retrieves the limit informacion. + */ + public TranslationalLimitMotor getTranslationalLimitMotor() { + return linearLimits; + } + + /** + * first 3 are linear, next 3 are angular + */ + public void setLimit(int axis, float lo, float hi) { + if (axis < 3) { + VectorUtil.setCoord(linearLimits.lowerLimit, axis, lo); + VectorUtil.setCoord(linearLimits.upperLimit, axis, hi); + } + else { + angularLimits[axis - 3].loLimit = lo; + angularLimits[axis - 3].hiLimit = hi; + } + } + + /** + * Test limit.<p> + * - free means upper < lower,<br> + * - locked means upper == lower<br> + * - limited means upper > lower<br> + * - limitIndex: first 3 are linear, next 3 are angular + */ + public boolean isLimited(int limitIndex) { + if (limitIndex < 3) { + return linearLimits.isLimited(limitIndex); + + } + return angularLimits[limitIndex - 3].isLimited(); + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/HingeConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/HingeConstraint.java new file mode 100644 index 0000000..a514c3e --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/HingeConstraint.java @@ -0,0 +1,619 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +/* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */ + +package javabullet.dynamics.constraintsolver; + +import javabullet.BulletGlobals; +import javabullet.dynamics.RigidBody; +import javabullet.linearmath.QuaternionUtil; +import javabullet.linearmath.ScalarUtil; +import javabullet.linearmath.Transform; +import javabullet.linearmath.TransformUtil; +import javax.vecmath.Matrix3f; +import javax.vecmath.Quat4f; +import javax.vecmath.Vector3f; + +/** + * Hinge constraint between two rigidbodies each with a pivotpoint that descibes + * the axis location in local space. Axis defines the orientation of the hinge axis. + * + * @author jezek2 + */ +public class HingeConstraint extends TypedConstraint { + + private JacobianEntry[] jac/*[3]*/ = new JacobianEntry[] { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() }; // 3 orthogonal linear constraints + private JacobianEntry[] jacAng/*[3]*/ = new JacobianEntry[] { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() }; // 2 orthogonal angular constraints+ 1 for limit/motor + + private final Transform rbAFrame = new Transform(); // constraint axii. Assumes z is hinge axis. + private final Transform rbBFrame = new Transform(); + + private float motorTargetVelocity; + private float maxMotorImpulse; + + private float limitSoftness; + private float biasFactor; + private float relaxationFactor; + + private float lowerLimit; + private float upperLimit; + + private float kHinge; + + private float limitSign; + private float correction; + + private float accLimitImpulse; + + private boolean angularOnly; + private boolean enableAngularMotor; + private boolean solveLimit; + + public HingeConstraint() { + super(TypedConstraintType.HINGE_CONSTRAINT_TYPE); + enableAngularMotor = false; + } + + public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3f pivotInA, Vector3f pivotInB, Vector3f axisInA, Vector3f axisInB) { + super(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA, rbB); + angularOnly = false; + enableAngularMotor = false; + + stack.vectors.push(); + stack.quats.push(); + try { + rbAFrame.origin.set(pivotInA); + + // since no frame is given, assume this to be zero angle and just pick rb transform axis + Vector3f rbAxisA1 = stack.vectors.get(); + rbA.getCenterOfMassTransform().basis.getColumn(0, rbAxisA1); + + float projection = rbAxisA1.dot(axisInA); + if (projection > BulletGlobals.FLT_EPSILON) { + rbAxisA1.scale(projection); + rbAxisA1.sub(axisInA); + } + else { + rbA.getCenterOfMassTransform().basis.getColumn(1, rbAxisA1); + } + + Vector3f rbAxisA2 = stack.vectors.get(); + rbAxisA2.cross(rbAxisA1, axisInA); + + rbAFrame.basis.setRow(0, rbAxisA1.x, rbAxisA2.x, axisInA.x); + rbAFrame.basis.setRow(1, rbAxisA1.y, rbAxisA2.y, axisInA.y); + rbAFrame.basis.setRow(2, rbAxisA1.z, rbAxisA2.z, axisInA.z); + + Quat4f rotationArc = stack.quats.get(QuaternionUtil.shortestArcQuat(axisInA, axisInB)); + Vector3f rbAxisB1 = stack.vectors.get(QuaternionUtil.quatRotate(rotationArc, rbAxisA1)); + Vector3f rbAxisB2 = stack.vectors.get(); + rbAxisB2.cross(rbAxisB1, axisInB); + + rbBFrame.origin.set(pivotInB); + rbBFrame.basis.setRow(0, rbAxisB1.x, rbAxisB2.x, -axisInB.x); + rbBFrame.basis.setRow(1, rbAxisB1.y, rbAxisB2.y, -axisInB.y); + rbBFrame.basis.setRow(2, rbAxisB1.z, rbAxisB2.z, -axisInB.z); + + // start with free + lowerLimit = 1e30f; + upperLimit = -1e30f; + biasFactor = 0.3f; + relaxationFactor = 1.0f; + limitSoftness = 0.9f; + solveLimit = false; + } + finally { + stack.vectors.pop(); + stack.quats.pop(); + } + } + + public HingeConstraint(RigidBody rbA, Vector3f pivotInA, Vector3f axisInA) { + super(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA); + angularOnly = false; + enableAngularMotor = false; + + stack.vectors.push(); + stack.quats.push(); + try { + // since no frame is given, assume this to be zero angle and just pick rb transform axis + // fixed axis in worldspace + Vector3f rbAxisA1 = stack.vectors.get(); + rbA.getCenterOfMassTransform().basis.getColumn(0, rbAxisA1); + + float projection = rbAxisA1.dot(axisInA); + if (projection > BulletGlobals.FLT_EPSILON) { + rbAxisA1.scale(projection); + rbAxisA1.sub(axisInA); + } + else { + rbA.getCenterOfMassTransform().basis.getColumn(1, rbAxisA1); + } + + Vector3f rbAxisA2 = stack.vectors.get(); + rbAxisA2.cross(axisInA, rbAxisA1); + + rbAFrame.origin.set(pivotInA); + rbAFrame.basis.setRow(0, rbAxisA1.x, rbAxisA2.x, axisInA.x); + rbAFrame.basis.setRow(1, rbAxisA1.y, rbAxisA2.y, axisInA.y); + rbAFrame.basis.setRow(2, rbAxisA1.z, rbAxisA2.z, axisInA.z); + + Vector3f axisInB = stack.vectors.get(); + axisInB.negate(axisInA); + rbA.getCenterOfMassTransform().basis.transform(axisInB); + + Quat4f rotationArc = stack.quats.get(QuaternionUtil.shortestArcQuat(axisInA, axisInB)); + Vector3f rbAxisB1 = stack.vectors.get(QuaternionUtil.quatRotate(rotationArc, rbAxisA1)); + Vector3f rbAxisB2 = stack.vectors.get(); + rbAxisB2.cross(axisInB, rbAxisB1); + + rbBFrame.origin.set(pivotInA); + rbA.getCenterOfMassTransform().transform(rbBFrame.origin); + rbBFrame.basis.setRow(0, rbAxisB1.x, rbAxisB2.x, axisInB.x); + rbBFrame.basis.setRow(1, rbAxisB1.y, rbAxisB2.y, axisInB.y); + rbBFrame.basis.setRow(2, rbAxisB1.z, rbAxisB2.z, axisInB.z); + + // start with free + lowerLimit = 1e30f; + upperLimit = -1e30f; + biasFactor = 0.3f; + relaxationFactor = 1.0f; + limitSoftness = 0.9f; + solveLimit = false; + } + finally { + stack.vectors.pop(); + stack.quats.pop(); + } + } + + public HingeConstraint(RigidBody rbA, RigidBody rbB, Transform rbAFrame, Transform rbBFrame) { + super(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA, rbB); + this.rbAFrame.set(rbAFrame); + this.rbBFrame.set(rbBFrame); + angularOnly = false; + enableAngularMotor = false; + + // flip axis + this.rbBFrame.basis.m02 *= -1f; + this.rbBFrame.basis.m12 *= -1f; + this.rbBFrame.basis.m22 *= -1f; + + // start with free + lowerLimit = 1e30f; + upperLimit = -1e30f; + biasFactor = 0.3f; + relaxationFactor = 1.0f; + limitSoftness = 0.9f; + solveLimit = false; + } + + public HingeConstraint(RigidBody rbA, Transform rbAFrame) { + super(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA); + this.rbAFrame.set(rbAFrame); + this.rbBFrame.set(rbAFrame); + angularOnly = false; + enableAngularMotor = false; + + // not providing rigidbody B means implicitly using worldspace for body B + + // flip axis + this.rbBFrame.basis.m02 *= -1f; + this.rbBFrame.basis.m12 *= -1f; + this.rbBFrame.basis.m22 *= -1f; + + this.rbBFrame.origin.set(this.rbAFrame.origin); + rbA.getCenterOfMassTransform().transform(this.rbBFrame.origin); + + // start with free + lowerLimit = 1e30f; + upperLimit = -1e30f; + biasFactor = 0.3f; + relaxationFactor = 1.0f; + limitSoftness = 0.9f; + solveLimit = false; + } + + @Override + public void buildJacobian() { + stack.pushCommonMath(); + try { + Vector3f tmp = stack.vectors.get(); + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + Matrix3f mat1 = stack.matrices.get(); + Matrix3f mat2 = stack.matrices.get(); + + appliedImpulse = 0f; + + if (!angularOnly) { + Vector3f pivotAInW = stack.vectors.get(rbAFrame.origin); + rbA.getCenterOfMassTransform().transform(pivotAInW); + + Vector3f pivotBInW = stack.vectors.get(rbBFrame.origin); + rbB.getCenterOfMassTransform().transform(pivotBInW); + + Vector3f relPos = stack.vectors.get(); + relPos.sub(pivotBInW, pivotAInW); + + Vector3f[] normal/*[3]*/ = new Vector3f[]{stack.vectors.get(), stack.vectors.get(), stack.vectors.get()}; + if (relPos.lengthSquared() > BulletGlobals.FLT_EPSILON) { + normal[0].set(relPos); + normal[0].normalize(); + } + else { + normal[0].set(1f, 0f, 0f); + } + + TransformUtil.planeSpace1(normal[0], normal[1], normal[2]); + + for (int i = 0; i < 3; i++) { + mat1.transpose(rbA.getCenterOfMassTransform().basis); + mat2.transpose(rbB.getCenterOfMassTransform().basis); + + tmp1.sub(pivotAInW, rbA.getCenterOfMassPosition()); + tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition()); + + jac[i].init( + mat1, + mat2, + tmp1, + tmp2, + normal[i], + rbA.getInvInertiaDiagLocal(), + rbA.getInvMass(), + rbB.getInvInertiaDiagLocal(), + rbB.getInvMass()); + } + } + + // calculate two perpendicular jointAxis, orthogonal to hingeAxis + // these two jointAxis require equal angular velocities for both bodies + + // this is unused for now, it's a todo + Vector3f jointAxis0local = stack.vectors.get(); + Vector3f jointAxis1local = stack.vectors.get(); + + rbAFrame.basis.getColumn(2, tmp); + TransformUtil.planeSpace1(tmp, jointAxis0local, jointAxis1local); + + // TODO: check this + //getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2); + + Vector3f jointAxis0 = stack.vectors.get(jointAxis0local); + getRigidBodyA().getCenterOfMassTransform().basis.transform(jointAxis0); + + Vector3f jointAxis1 = stack.vectors.get(jointAxis1local); + getRigidBodyA().getCenterOfMassTransform().basis.transform(jointAxis1); + + Vector3f hingeAxisWorld = stack.vectors.get(); + rbAFrame.basis.getColumn(2, hingeAxisWorld); + getRigidBodyA().getCenterOfMassTransform().basis.transform(hingeAxisWorld); + + mat1.transpose(rbA.getCenterOfMassTransform().basis); + mat2.transpose(rbB.getCenterOfMassTransform().basis); + jacAng[0].init(jointAxis0, + mat1, + mat2, + rbA.getInvInertiaDiagLocal(), + rbB.getInvInertiaDiagLocal()); + + // JAVA NOTE: reused mat1 and mat2, as recomputation is not needed + jacAng[1].init(jointAxis1, + mat1, + mat2, + rbA.getInvInertiaDiagLocal(), + rbB.getInvInertiaDiagLocal()); + + // JAVA NOTE: reused mat1 and mat2, as recomputation is not needed + jacAng[2].init(hingeAxisWorld, + mat1, + mat2, + rbA.getInvInertiaDiagLocal(), + rbB.getInvInertiaDiagLocal()); + + // Compute limit information + float hingeAngle = getHingeAngle(); + + //set bias, sign, clear accumulator + correction = 0f; + limitSign = 0f; + solveLimit = false; + accLimitImpulse = 0f; + + if (lowerLimit < upperLimit) { + if (hingeAngle <= lowerLimit * limitSoftness) { + correction = (lowerLimit - hingeAngle); + limitSign = 1.0f; + solveLimit = true; + } + else if (hingeAngle >= upperLimit * limitSoftness) { + correction = upperLimit - hingeAngle; + limitSign = -1.0f; + solveLimit = true; + } + } + + // Compute K = J*W*J' for hinge axis + Vector3f axisA = stack.vectors.get(); + rbAFrame.basis.getColumn(2, axisA); + getRigidBodyA().getCenterOfMassTransform().basis.transform(axisA); + + kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) + + getRigidBodyB().computeAngularImpulseDenominator(axisA)); + } + finally { + stack.popCommonMath(); + } + } + + @Override + public void solveConstraint(float timeStep) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + Vector3f pivotAInW = stack.vectors.get(rbAFrame.origin); + rbA.getCenterOfMassTransform().transform(pivotAInW); + + Vector3f pivotBInW = stack.vectors.get(rbBFrame.origin); + rbB.getCenterOfMassTransform().transform(pivotBInW); + + float tau = 0.3f; + + // linear part + if (!angularOnly) { + Vector3f rel_pos1 = stack.vectors.get(); + rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition()); + + Vector3f rel_pos2 = stack.vectors.get(); + rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition()); + + Vector3f vel1 = stack.vectors.get(rbA.getVelocityInLocalPoint(rel_pos1)); + Vector3f vel2 = stack.vectors.get(rbB.getVelocityInLocalPoint(rel_pos2)); + Vector3f vel = stack.vectors.get(); + vel.sub(vel1, vel2); + + for (int i = 0; i < 3; i++) { + Vector3f normal = jac[i].linearJointAxis; + float jacDiagABInv = 1f / jac[i].getDiagonal(); + + float rel_vel; + rel_vel = normal.dot(vel); + // positional error (zeroth order error) + tmp.sub(pivotAInW, pivotBInW); + float depth = -(tmp).dot(normal); // this is the error projected on the normal + float impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv; + appliedImpulse += impulse; + Vector3f impulse_vector = stack.vectors.get(); + impulse_vector.scale(impulse, normal); + + tmp.sub(pivotAInW, rbA.getCenterOfMassPosition()); + rbA.applyImpulse(impulse_vector, tmp); + + tmp.negate(impulse_vector); + tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition()); + rbB.applyImpulse(tmp, tmp2); + } + } + + + { + // solve angular part + + // get axes in world space + Vector3f axisA = stack.vectors.get(); + rbAFrame.basis.getColumn(2, axisA); + getRigidBodyA().getCenterOfMassTransform().basis.transform(axisA); + + Vector3f axisB = stack.vectors.get(); + rbBFrame.basis.getColumn(2, axisB); + getRigidBodyB().getCenterOfMassTransform().basis.transform(axisB); + + Vector3f angVelA = getRigidBodyA().getAngularVelocity(); + Vector3f angVelB = getRigidBodyB().getAngularVelocity(); + + Vector3f angVelAroundHingeAxisA = stack.vectors.get(); + angVelAroundHingeAxisA.scale(axisA.dot(angVelA), axisA); + + Vector3f angVelAroundHingeAxisB = stack.vectors.get(); + angVelAroundHingeAxisB.scale(axisB.dot(angVelB), axisB); + + Vector3f angAorthog = stack.vectors.get(); + angAorthog.sub(angVelA, angVelAroundHingeAxisA); + + Vector3f angBorthog = stack.vectors.get(); + angBorthog.sub(angVelB, angVelAroundHingeAxisB); + + Vector3f velrelOrthog = stack.vectors.get(); + velrelOrthog.sub(angAorthog, angBorthog); + + { + // solve orthogonal angular velocity correction + float relaxation = 1f; + float len = velrelOrthog.length(); + if (len > 0.00001f) { + Vector3f normal = stack.vectors.get(); + normal.normalize(velrelOrthog); + + float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) + + getRigidBodyB().computeAngularImpulseDenominator(normal); + // scale for mass and relaxation + // todo: expose this 0.9 factor to developer + velrelOrthog.scale((1f / denom) * relaxationFactor); + } + + // solve angular positional correction + // TODO: check + //Vector3f angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep); + Vector3f angularError = stack.vectors.get(); + angularError.cross(axisA, axisB); + angularError.negate(); + angularError.scale(1f / timeStep); + float len2 = angularError.length(); + if (len2 > 0.00001f) { + Vector3f normal2 = stack.vectors.get(); + normal2.normalize(angularError); + + float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) + + getRigidBodyB().computeAngularImpulseDenominator(normal2); + angularError.scale((1f / denom2) * relaxation); + } + + tmp.negate(velrelOrthog); + tmp.add(angularError); + rbA.applyTorqueImpulse(tmp); + + tmp.sub(velrelOrthog, angularError); + rbB.applyTorqueImpulse(tmp); + + // solve limit + if (solveLimit) { + tmp.sub(angVelB, angVelA); + float amplitude = ((tmp).dot(axisA) * relaxationFactor + correction * (1f / timeStep) * biasFactor) * limitSign; + + float impulseMag = amplitude * kHinge; + + // Clamp the accumulated impulse + float temp = accLimitImpulse; + accLimitImpulse = Math.max(accLimitImpulse + impulseMag, 0f); + impulseMag = accLimitImpulse - temp; + + Vector3f impulse = stack.vectors.get(); + impulse.scale(impulseMag * limitSign, axisA); + + rbA.applyTorqueImpulse(impulse); + + tmp.negate(impulse); + rbB.applyTorqueImpulse(tmp); + } + } + + // apply motor + if (enableAngularMotor) { + // todo: add limits too + Vector3f angularLimit = stack.vectors.get(0f, 0f, 0f); + + Vector3f velrel = stack.vectors.get(); + velrel.sub(angVelAroundHingeAxisA, angVelAroundHingeAxisB); + float projRelVel = velrel.dot(axisA); + + float desiredMotorVel = motorTargetVelocity; + float motor_relvel = desiredMotorVel - projRelVel; + + float unclippedMotorImpulse = kHinge * motor_relvel; + // todo: should clip against accumulated impulse + float clippedMotorImpulse = unclippedMotorImpulse > maxMotorImpulse ? maxMotorImpulse : unclippedMotorImpulse; + clippedMotorImpulse = clippedMotorImpulse < -maxMotorImpulse ? -maxMotorImpulse : clippedMotorImpulse; + Vector3f motorImp = stack.vectors.get(); + motorImp.scale(clippedMotorImpulse, axisA); + + tmp.add(motorImp, angularLimit); + rbA.applyTorqueImpulse(tmp); + + tmp.negate(motorImp); + tmp.sub(angularLimit); + rbB.applyTorqueImpulse(tmp); + } + } + } + finally { + stack.vectors.pop(); + } + } + + public void updateRHS(float timeStep) { + } + + public float getHingeAngle() { + stack.vectors.push(); + try { + Vector3f refAxis0 = stack.vectors.get(); + rbAFrame.basis.getColumn(0, refAxis0); + getRigidBodyA().getCenterOfMassTransform().basis.transform(refAxis0); + + Vector3f refAxis1 = stack.vectors.get(); + rbAFrame.basis.getColumn(1, refAxis1); + getRigidBodyA().getCenterOfMassTransform().basis.transform(refAxis1); + + Vector3f swingAxis = stack.vectors.get(); + rbBFrame.basis.getColumn(1, swingAxis); + getRigidBodyB().getCenterOfMassTransform().basis.transform(swingAxis); + + return ScalarUtil.atan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)); + } + finally { + stack.vectors.pop(); + } + } + + public void setAngularOnly(boolean angularOnly) { + this.angularOnly = angularOnly; + } + + public void enableAngularMotor(boolean enableMotor, float targetVelocity, float maxMotorImpulse) { + this.enableAngularMotor = enableMotor; + this.motorTargetVelocity = targetVelocity; + this.maxMotorImpulse = maxMotorImpulse; + } + + public void setLimit(float low, float high) { + setLimit(low, high, 0.9f, 0.3f, 1.0f); + } + + public void setLimit(float low, float high, float _softness, float _biasFactor, float _relaxationFactor) { + lowerLimit = low; + upperLimit = high; + + limitSoftness = _softness; + biasFactor = _biasFactor; + relaxationFactor = _relaxationFactor; + } + + public float getLowerLimit() { + return lowerLimit; + } + + public float getUpperLimit() { + return upperLimit; + } + + public Transform getAFrame() { + return rbAFrame; + } + + public Transform getBFrame() { + return rbBFrame; + } + + public boolean getSolveLimit() { + return solveLimit; + } + + public float getLimitSign() { + return limitSign; + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/JacobianEntry.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/JacobianEntry.java new file mode 100644 index 0000000..aca6ccc --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/JacobianEntry.java @@ -0,0 +1,231 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Matrix3f; +import javax.vecmath.Vector3f; + +//notes: +// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components +// which makes the btJacobianEntry memory layout 16 bytes +// if you only are interested in angular part, just feed massInvA and massInvB zero + +/** + * Jacobian entry is an abstraction that allows to describe constraints. + * It can be used in combination with a constraint solver. + * Can be used to relate the effect of an impulse to the constraint error. + * + * @author jezek2 + */ +public class JacobianEntry { + + protected final BulletStack stack = BulletStack.get(); + + public final Vector3f linearJointAxis = new Vector3f(); + public final Vector3f aJ = new Vector3f(); + public final Vector3f bJ = new Vector3f(); + public final Vector3f m_0MinvJt = new Vector3f(); + public final Vector3f m_1MinvJt = new Vector3f(); + // Optimization: can be stored in the w/last component of one of the vectors + public float Adiag; + + public JacobianEntry() { + } + + /** + * Constraint between two different rigidbodies. + */ + public void init(Matrix3f world2A, + Matrix3f world2B, + Vector3f rel_pos1, Vector3f rel_pos2, + Vector3f jointAxis, + Vector3f inertiaInvA, + float massInvA, + Vector3f inertiaInvB, + float massInvB) + { + linearJointAxis.set(jointAxis); + + aJ.cross(rel_pos1, linearJointAxis); + world2A.transform(aJ); + + bJ.set(linearJointAxis); + bJ.negate(); + bJ.cross(rel_pos2, bJ); + world2B.transform(bJ); + + VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ); + VectorUtil.mul(m_1MinvJt, inertiaInvB, bJ); + Adiag = massInvA + m_0MinvJt.dot(aJ) + massInvB + m_1MinvJt.dot(bJ); + + assert (Adiag > 0f); + } + + /** + * Angular constraint between two different rigidbodies. + */ + public void init(Vector3f jointAxis, + Matrix3f world2A, + Matrix3f world2B, + Vector3f inertiaInvA, + Vector3f inertiaInvB) + { + linearJointAxis.set(0f, 0f, 0f); + + aJ.set(jointAxis); + world2A.transform(aJ); + + bJ.set(jointAxis); + bJ.negate(); + world2B.transform(bJ); + + VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ); + VectorUtil.mul(m_1MinvJt, inertiaInvB, bJ); + Adiag = m_0MinvJt.dot(aJ) + m_1MinvJt.dot(bJ); + + assert (Adiag > 0f); + } + + /** + * Angular constraint between two different rigidbodies. + */ + public void init(Vector3f axisInA, + Vector3f axisInB, + Vector3f inertiaInvA, + Vector3f inertiaInvB) + { + linearJointAxis.set(0f, 0f, 0f); + aJ.set(axisInA); + + bJ.set(axisInB); + bJ.negate(); + + VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ); + VectorUtil.mul(m_1MinvJt, inertiaInvB, bJ); + Adiag = m_0MinvJt.dot(aJ) + m_1MinvJt.dot(bJ); + + assert (Adiag > 0f); + } + + /** + * Constraint on one rigidbody. + */ + public void init( + Matrix3f world2A, + Vector3f rel_pos1, Vector3f rel_pos2, + Vector3f jointAxis, + Vector3f inertiaInvA, + float massInvA) + { + linearJointAxis.set(jointAxis); + + aJ.cross(rel_pos1, jointAxis); + world2A.transform(aJ); + + bJ.set(jointAxis); + bJ.negate(); + bJ.cross(rel_pos2, bJ); + world2A.transform(bJ); + + VectorUtil.mul(m_0MinvJt, inertiaInvA, aJ); + m_1MinvJt.set(0f, 0f, 0f); + Adiag = massInvA + m_0MinvJt.dot(aJ); + + assert (Adiag > 0f); + } + + public float getDiagonal() { return Adiag; } + + /** + * For two constraints on the same rigidbody (for example vehicle friction). + */ + public float getNonDiagonal(JacobianEntry jacB, float massInvA) { + JacobianEntry jacA = this; + float lin = massInvA * jacA.linearJointAxis.dot(jacB.linearJointAxis); + float ang = jacA.m_0MinvJt.dot(jacB.aJ); + return lin + ang; + } + + /** + * For two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies). + */ + public float getNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB) { + stack.vectors.push(); + try { + JacobianEntry jacA = this; + + Vector3f lin = stack.vectors.get(); + VectorUtil.mul(lin, jacA.linearJointAxis, jacB.linearJointAxis); + + Vector3f ang0 = stack.vectors.get(); + VectorUtil.mul(ang0, jacA.m_0MinvJt, jacB.aJ); + + Vector3f ang1 = stack.vectors.get(); + VectorUtil.mul(ang1, jacA.m_1MinvJt, jacB.bJ); + + Vector3f lin0 = stack.vectors.get(); + lin0.scale(massInvA, lin); + + Vector3f lin1 = stack.vectors.get(); + lin1.scale(massInvB, lin); + + Vector3f sum = stack.vectors.get(); + VectorUtil.add(sum, ang0, ang1, lin0, lin1); + + return sum.x + sum.y + sum.z; + } + finally { + stack.vectors.pop(); + } + } + + public float getRelativeVelocity(Vector3f linvelA, Vector3f angvelA, Vector3f linvelB, Vector3f angvelB) { + stack.vectors.push(); + try { + Vector3f linrel = stack.vectors.get(); + linrel.sub(linvelA, linvelB); + + Vector3f angvela = stack.vectors.get(); + VectorUtil.mul(angvela, angvelA, aJ); + + Vector3f angvelb = stack.vectors.get(); + VectorUtil.mul(angvelb, angvelB, bJ); + + VectorUtil.mul(linrel, linrel, linearJointAxis); + + angvela.add(angvelb); + angvela.add(linrel); + + float rel_vel2 = angvela.x + angvela.y + angvela.z; + return rel_vel2 + BulletGlobals.FLT_EPSILON; + } + finally { + stack.vectors.pop(); + } + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/Point2PointConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/Point2PointConstraint.java new file mode 100644 index 0000000..0d64c25 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/Point2PointConstraint.java @@ -0,0 +1,197 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +import javabullet.dynamics.RigidBody; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Matrix3f; +import javax.vecmath.Vector3f; + +/** + * Point to point constraint between two rigidbodies each with a pivotpoint that descibes the 'ballsocket' location in local space. + * + * @author jezek2 + */ +public class Point2PointConstraint extends TypedConstraint { + + private final JacobianEntry[] jac = new JacobianEntry[]/*[3]*/ { new JacobianEntry(), new JacobianEntry(), new JacobianEntry() }; // 3 orthogonal linear constraints + + private final Vector3f pivotInA = new Vector3f(); + private final Vector3f pivotInB = new Vector3f(); + + public ConstraintSetting setting = new ConstraintSetting(); + + public Point2PointConstraint() { + super(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE); + } + + public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3f pivotInA, Vector3f pivotInB) { + super(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE, rbA, rbB); + this.pivotInA.set(pivotInA); + this.pivotInB.set(pivotInB); + } + + public Point2PointConstraint(RigidBody rbA, Vector3f pivotInA) { + super(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE, rbA); + this.pivotInA.set(pivotInA); + this.pivotInB.set(pivotInA); + rbA.getCenterOfMassTransform().transform(this.pivotInB); + } + + @Override + public void buildJacobian() { + stack.pushCommonMath(); + try { + appliedImpulse = 0f; + + Vector3f normal = stack.vectors.get(0f, 0f, 0f); + + Matrix3f tmpMat1 = stack.matrices.get(); + Matrix3f tmpMat2 = stack.matrices.get(); + Vector3f tmp1 = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + for (int i = 0; i < 3; i++) { + VectorUtil.setCoord(normal, i, 1f); + + tmpMat1.transpose(rbA.getCenterOfMassTransform().basis); + tmpMat2.transpose(rbB.getCenterOfMassTransform().basis); + + tmp1.set(pivotInA); + rbA.getCenterOfMassTransform().transform(tmp1); + tmp1.sub(rbA.getCenterOfMassPosition()); + + tmp2.set(pivotInB); + rbB.getCenterOfMassTransform().transform(tmp2); + tmp2.sub(rbB.getCenterOfMassPosition()); + + jac[i].init( + tmpMat1, + tmpMat2, + tmp1, + tmp2, + normal, + rbA.getInvInertiaDiagLocal(), + rbA.getInvMass(), + rbB.getInvInertiaDiagLocal(), + rbB.getInvMass()); + VectorUtil.setCoord(normal, i, 0f); + } + } + finally { + stack.popCommonMath(); + } + } + + @Override + public void solveConstraint(float timeStep) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + Vector3f tmp2 = stack.vectors.get(); + + Vector3f pivotAInW = stack.vectors.get(pivotInA); + rbA.getCenterOfMassTransform().transform(pivotAInW); + + Vector3f pivotBInW = stack.vectors.get(pivotInB); + rbB.getCenterOfMassTransform().transform(pivotBInW); + + Vector3f normal = stack.vectors.get(0f, 0f, 0f); + + //btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity(); + //btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity(); + + for (int i = 0; i < 3; i++) { + VectorUtil.setCoord(normal, i, 1f); + float jacDiagABInv = 1f / jac[i].getDiagonal(); + + Vector3f rel_pos1 = stack.vectors.get(); + rel_pos1.sub(pivotAInW, rbA.getCenterOfMassPosition()); + Vector3f rel_pos2 = stack.vectors.get(); + rel_pos2.sub(pivotBInW, rbB.getCenterOfMassPosition()); + // this jacobian entry could be re-used for all iterations + + Vector3f vel1 = stack.vectors.get(rbA.getVelocityInLocalPoint(rel_pos1)); + Vector3f vel2 = stack.vectors.get(rbB.getVelocityInLocalPoint(rel_pos2)); + Vector3f vel = stack.vectors.get(); + vel.sub(vel1, vel2); + + float rel_vel; + rel_vel = normal.dot(vel); + + /* + //velocity error (first order error) + btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, + m_rbB.getLinearVelocity(),angvelB); + */ + + // positional error (zeroth order error) + tmp.sub(pivotAInW, pivotBInW); + float depth = -tmp.dot(normal); //this is the error projected on the normal + + float impulse = depth * setting.tau / timeStep * jacDiagABInv - setting.damping * rel_vel * jacDiagABInv; + appliedImpulse += impulse; + Vector3f impulse_vector = stack.vectors.get(); + impulse_vector.scale(impulse, normal); + tmp.sub(pivotAInW, rbA.getCenterOfMassPosition()); + rbA.applyImpulse(impulse_vector, tmp); + tmp.negate(impulse_vector); + tmp2.sub(pivotBInW, rbB.getCenterOfMassPosition()); + rbB.applyImpulse(tmp, tmp2); + + VectorUtil.setCoord(normal, i, 0f); + } + } + finally { + stack.vectors.pop(); + } + } + + public void updateRHS(float timeStep) { + } + + public void setPivotA(Vector3f pivotA) { + pivotInA.set(pivotA); + } + + public void setPivotB(Vector3f pivotB) { + pivotInB.set(pivotB); + } + + public Vector3f getPivotInA() { + return pivotInA; + } + + public Vector3f getPivotInB() { + return pivotInB; + } + + //////////////////////////////////////////////////////////////////////////// + + public static class ConstraintSetting { + public float tau = 0.3f; + public float damping = 1f; + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/RotationalLimitMotor.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/RotationalLimitMotor.java new file mode 100644 index 0000000..73fa92e --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/RotationalLimitMotor.java @@ -0,0 +1,211 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +/* +2007-09-09 +btGeneric6DofConstraint Refactored by Francisco Le�n +email: [email protected] +http://gimpact.sf.net +*/ + +package javabullet.dynamics.constraintsolver; + +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javabullet.dynamics.RigidBody; +import javax.vecmath.Vector3f; + +/** + * Rotation Limit structure for generic joints. + * + * @author jezek2 + */ +public class RotationalLimitMotor { + + protected final BulletStack stack = BulletStack.get(); + + public float loLimit; //!< joint limit + public float hiLimit; //!< joint limit + public float targetVelocity; //!< target motor velocity + public float maxMotorForce; //!< max force on motor + public float maxLimitForce; //!< max force on limit + public float damping; //!< Damping. + public float limitSoftness; //! Relaxation factor + public float ERP; //!< Error tolerance factor when joint is at limit + public float bounce; //!< restitution factor + public boolean enableMotor; + + public float currentLimitError;//! How much is violated this limit + public int currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit + public float accumulatedImpulse; + + public RotationalLimitMotor() { + accumulatedImpulse = 0.f; + targetVelocity = 0; + maxMotorForce = 0.1f; + maxLimitForce = 300.0f; + loLimit = -BulletGlobals.SIMD_INFINITY; + hiLimit = BulletGlobals.SIMD_INFINITY; + ERP = 0.5f; + bounce = 0.0f; + damping = 1.0f; + limitSoftness = 0.5f; + currentLimit = 0; + currentLimitError = 0; + enableMotor = false; + } + + public RotationalLimitMotor(RotationalLimitMotor limot) { + targetVelocity = limot.targetVelocity; + maxMotorForce = limot.maxMotorForce; + limitSoftness = limot.limitSoftness; + loLimit = limot.loLimit; + hiLimit = limot.hiLimit; + ERP = limot.ERP; + bounce = limot.bounce; + currentLimit = limot.currentLimit; + currentLimitError = limot.currentLimitError; + enableMotor = limot.enableMotor; + } + + /** + * Is limited? + */ + public boolean isLimited() + { + if(loLimit>=hiLimit) return false; + return true; + } + + /** + * Need apply correction? + */ + public boolean needApplyTorques() + { + if(currentLimit == 0 && enableMotor == false) return false; + return true; + } + + /** + * Calculates error. Calculates currentLimit and currentLimitError. + */ + public int testLimitValue(float test_value) { + if (loLimit > hiLimit) { + currentLimit = 0; // Free from violation + return 0; + } + + if (test_value < loLimit) { + currentLimit = 1; // low limit violation + currentLimitError = test_value - loLimit; + return 1; + } + else if (test_value > hiLimit) { + currentLimit = 2; // High limit violation + currentLimitError = test_value - hiLimit; + return 2; + } + else { + currentLimit = 0; // Free from violation + return 0; + } + //return 0; + } + + /** + * Apply the correction impulses for two bodies. + */ + public float solveAngularLimits(float timeStep, Vector3f axis, float jacDiagABInv, RigidBody body0, RigidBody body1) { + if (needApplyTorques() == false) { + return 0.0f; + } + + stack.vectors.push(); + try { + float target_velocity = this.targetVelocity; + float maxMotorForce = this.maxMotorForce; + + // current error correction + if (currentLimit != 0) { + target_velocity = -ERP * currentLimitError / (timeStep); + maxMotorForce = maxLimitForce; + } + + maxMotorForce *= timeStep; + + // current velocity difference + Vector3f vel_diff = stack.vectors.get(body0.getAngularVelocity()); + if (body1 != null) { + vel_diff.sub(body1.getAngularVelocity()); + } + + float rel_vel = axis.dot(vel_diff); + + // correction velocity + float motor_relvel = limitSoftness * (target_velocity - damping * rel_vel); + + if (motor_relvel < BulletGlobals.FLT_EPSILON && motor_relvel > -BulletGlobals.FLT_EPSILON) { + return 0.0f; // no need for applying force + } + + // correction impulse + float unclippedMotorImpulse = (1 + bounce) * motor_relvel * jacDiagABInv; + + // clip correction impulse + float clippedMotorImpulse; + + // todo: should clip against accumulated impulse + if (unclippedMotorImpulse > 0.0f) { + clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse; + } + else { + clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse; + } + + // sort with accumulated impulses + float lo = -1e30f; + float hi = 1e30f; + + float oldaccumImpulse = accumulatedImpulse; + float sum = oldaccumImpulse + clippedMotorImpulse; + accumulatedImpulse = sum > hi ? 0f : sum < lo ? 0f : sum; + + clippedMotorImpulse = accumulatedImpulse - oldaccumImpulse; + + Vector3f motorImp = stack.vectors.get(); + motorImp.scale(clippedMotorImpulse, axis); + + body0.applyTorqueImpulse(motorImp); + if (body1 != null) { + motorImp.negate(); + body1.applyTorqueImpulse(motorImp); + } + + return clippedMotorImpulse; + } + finally { + stack.vectors.pop(); + } + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/SequentialImpulseConstraintSolver.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/SequentialImpulseConstraintSolver.java new file mode 100644 index 0000000..ba52ff0 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/SequentialImpulseConstraintSolver.java @@ -0,0 +1,1233 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +import java.util.ArrayList; +import java.util.List; +import javabullet.BulletGlobals; +import javabullet.BulletPool; +import javabullet.ContactDestroyedCallback; +import javabullet.ObjectPool; +import javabullet.collision.broadphase.Dispatcher; +import javabullet.collision.dispatch.CollisionObject; +import javabullet.collision.narrowphase.ManifoldPoint; +import javabullet.collision.narrowphase.PersistentManifold; +import javabullet.dynamics.RigidBody; +import javabullet.linearmath.IDebugDraw; +import javabullet.linearmath.MiscUtil; +import javabullet.linearmath.TransformUtil; +import javabullet.util.IntArrayList; +import javax.vecmath.Matrix3f; +import javax.vecmath.Vector3f; + +/** + * SequentialImpulseConstraintSolver uses a Propagation Method and Sequentially applies impulses. + * The approach is the 3D version of Erin Catto's GDC 2006 tutorial. See http://www.gphysics.com<p> + * + * Although Sequential Impulse is more intuitive, it is mathematically equivalent to Projected Successive Overrelaxation (iterative LCP). + * Applies impulses for combined restitution and penetration recovery and to simulate friction. + * + * @author jezek2 + */ +public class SequentialImpulseConstraintSolver extends ConstraintSolver { + + private static final int MAX_CONTACT_SOLVER_TYPES = ContactConstraintEnum.MAX_CONTACT_SOLVER_TYPES.ordinal(); + + private static final int SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS = 16384; + private static OrderIndex[] gOrder = new OrderIndex[SEQUENTIAL_IMPULSE_MAX_SOLVER_POINTS]; + + private static int totalCpd = 0; + + static { + for (int i=0; i<gOrder.length; i++) { + gOrder[i] = new OrderIndex(); + } + } + + //////////////////////////////////////////////////////////////////////////// + + private final ObjectPool<SolverBody> bodiesPool = BulletPool.get(SolverBody.class); + private final ObjectPool<SolverConstraint> constraintsPool = BulletPool.get(SolverConstraint.class); + private final ObjectPool<JacobianEntry> jacobiansPool = BulletPool.get(JacobianEntry.class); + + private final List<SolverBody> tmpSolverBodyPool = new ArrayList<SolverBody>(); + private final List<SolverConstraint> tmpSolverConstraintPool = new ArrayList<SolverConstraint>(); + private final List<SolverConstraint> tmpSolverFrictionConstraintPool = new ArrayList<SolverConstraint>(); + private final IntArrayList orderTmpConstraintPool = new IntArrayList(); + private final IntArrayList orderFrictionConstraintPool = new IntArrayList(); + + protected final ContactSolverFunc[][] contactDispatch = new ContactSolverFunc[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; + protected final ContactSolverFunc[][] frictionDispatch = new ContactSolverFunc[MAX_CONTACT_SOLVER_TYPES][MAX_CONTACT_SOLVER_TYPES]; + + // choose between several modes, different friction model etc. + protected int solverMode = SolverMode.SOLVER_RANDMIZE_ORDER | SolverMode.SOLVER_CACHE_FRIENDLY; // not using SOLVER_USE_WARMSTARTING, + // btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction + protected long btSeed2 = 0L; + + public SequentialImpulseConstraintSolver() { + BulletGlobals.gContactDestroyedCallback = new ContactDestroyedCallback() { + public boolean invoke(Object userPersistentData) { + assert (userPersistentData != null); + ConstraintPersistentData cpd = (ConstraintPersistentData) userPersistentData; + //btAlignedFree(cpd); + totalCpd--; + //printf("totalCpd = %i. DELETED Ptr %x\n",totalCpd,userPersistentData); + return true; + } + }; + + // initialize default friction/contact funcs + int i, j; + for (i = 0; i < MAX_CONTACT_SOLVER_TYPES; i++) { + for (j = 0; j < MAX_CONTACT_SOLVER_TYPES; j++) { + contactDispatch[i][j] = ContactConstraint.resolveSingleCollision; + frictionDispatch[i][j] = ContactConstraint.resolveSingleFriction; + } + } + } + + public long rand2() { + btSeed2 = (1664525L * btSeed2 + 1013904223L) & 0xffffffff; + return btSeed2; + } + + // See ODE: adam's all-int straightforward(?) dRandInt (0..n-1) + public int randInt2(int n) { + // seems good; xor-fold and modulus + long un = n; + long r = rand2(); + + // note: probably more aggressive than it needs to be -- might be + // able to get away without one or two of the innermost branches. + if (un <= 0x00010000L) { + r ^= (r >>> 16); + if (un <= 0x00000100L) { + r ^= (r >>> 8); + if (un <= 0x00000010L) { + r ^= (r >>> 4); + if (un <= 0x00000004L) { + r ^= (r >>> 2); + if (un <= 0x00000002L) { + r ^= (r >>> 1); + } + } + } + } + } + + // TODO: check modulo C vs Java mismatch + return (int) Math.abs(r % un); + } + + private void initSolverBody(SolverBody solverBody, CollisionObject collisionObject) { + RigidBody rb = RigidBody.upcast(collisionObject); + if (rb != null) { + solverBody.angularVelocity.set(rb.getAngularVelocity()); + solverBody.centerOfMassPosition.set(collisionObject.getWorldTransform().origin); + solverBody.friction = collisionObject.getFriction(); + solverBody.invMass = rb.getInvMass(); + solverBody.linearVelocity.set(rb.getLinearVelocity()); + solverBody.originalBody = rb; + solverBody.angularFactor = rb.getAngularFactor(); + } + else { + solverBody.angularVelocity.set(0f, 0f, 0f); + solverBody.centerOfMassPosition.set(collisionObject.getWorldTransform().origin); + solverBody.friction = collisionObject.getFriction(); + solverBody.invMass = 0f; + solverBody.linearVelocity.set(0f, 0f, 0f); + solverBody.originalBody = null; + solverBody.angularFactor = 1f; + } + } + + private float restitutionCurve(float rel_vel, float restitution) { + float rest = restitution * -rel_vel; + return rest; + } + + /** + * velocity + friction + * response between two dynamic objects with friction + */ + private float resolveSingleCollisionCombinedCacheFriendly( + SolverBody body1, + SolverBody body2, + SolverConstraint contactConstraint, + ContactSolverInfo solverInfo) { + stack.vectors.push(); + try { + float normalImpulse; + + // Optimized version of projected relative velocity, use precomputed cross products with normal + // body1.getVelocityInLocalPoint(contactConstraint.m_rel_posA,vel1); + // body2.getVelocityInLocalPoint(contactConstraint.m_rel_posB,vel2); + // btVector3 vel = vel1 - vel2; + // btScalar rel_vel = contactConstraint.m_contactNormal.dot(vel); + + float rel_vel; + float vel1Dotn = contactConstraint.contactNormal.dot(body1.linearVelocity) + contactConstraint.relpos1CrossNormal.dot(body1.angularVelocity); + float vel2Dotn = contactConstraint.contactNormal.dot(body2.linearVelocity) + contactConstraint.relpos2CrossNormal.dot(body2.angularVelocity); + + rel_vel = vel1Dotn - vel2Dotn; + + + float positionalError = contactConstraint.penetration; + float velocityError = contactConstraint.restitution - rel_vel; // * damping; + + float penetrationImpulse = positionalError * contactConstraint.jacDiagABInv; + float velocityImpulse = velocityError * contactConstraint.jacDiagABInv; + normalImpulse = penetrationImpulse + velocityImpulse; + + // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse + float oldNormalImpulse = contactConstraint.appliedImpulse; + float sum = oldNormalImpulse + normalImpulse; + contactConstraint.appliedImpulse = 0f > sum ? 0f : sum; + + float oldVelocityImpulse = contactConstraint.appliedVelocityImpulse; + float velocitySum = oldVelocityImpulse + velocityImpulse; + contactConstraint.appliedVelocityImpulse = 0f > velocitySum ? 0f : velocitySum; + + normalImpulse = contactConstraint.appliedImpulse - oldNormalImpulse; + + Vector3f tmp = stack.vectors.get(); + if (body1.invMass != 0f) { + tmp.scale(body1.invMass, contactConstraint.contactNormal); + body1.internalApplyImpulse(tmp, contactConstraint.angularComponentA, normalImpulse); + } + if (body2.invMass != 0f) { + tmp.scale(body2.invMass, contactConstraint.contactNormal); + body2.internalApplyImpulse(tmp, contactConstraint.angularComponentB, -normalImpulse); + } + + return normalImpulse; + } + finally { + stack.vectors.pop(); + } + } + + private float resolveSingleFrictionCacheFriendly( + SolverBody body1, + SolverBody body2, + SolverConstraint contactConstraint, + ContactSolverInfo solverInfo, + float appliedNormalImpulse) { + stack.vectors.push(); + try { + float combinedFriction = contactConstraint.friction; + + float limit = appliedNormalImpulse * combinedFriction; + + if (appliedNormalImpulse > 0f) //friction + { + + float j1; + { + + float rel_vel; + float vel1Dotn = contactConstraint.contactNormal.dot(body1.linearVelocity) + contactConstraint.relpos1CrossNormal.dot(body1.angularVelocity); + float vel2Dotn = contactConstraint.contactNormal.dot(body2.linearVelocity) + contactConstraint.relpos2CrossNormal.dot(body2.angularVelocity); + rel_vel = vel1Dotn - vel2Dotn; + + // calculate j that moves us to zero relative velocity + j1 = -rel_vel * contactConstraint.jacDiagABInv; + //#define CLAMP_ACCUMULATED_FRICTION_IMPULSE 1 + //#ifdef CLAMP_ACCUMULATED_FRICTION_IMPULSE + float oldTangentImpulse = contactConstraint.appliedImpulse; + contactConstraint.appliedImpulse = oldTangentImpulse + j1; + + if (limit < contactConstraint.appliedImpulse) { + contactConstraint.appliedImpulse = limit; + } + else { + if (contactConstraint.appliedImpulse < -limit) { + contactConstraint.appliedImpulse = -limit; + } + } + j1 = contactConstraint.appliedImpulse - oldTangentImpulse; + // #else + // if (limit < j1) + // { + // j1 = limit; + // } else + // { + // if (j1 < -limit) + // j1 = -limit; + // } + // #endif + + //GEN_set_min(contactConstraint.m_appliedImpulse, limit); + //GEN_set_max(contactConstraint.m_appliedImpulse, -limit); + } + + Vector3f tmp = stack.vectors.get(); + if (body1.invMass != 0f) { + tmp.scale(body1.invMass, contactConstraint.contactNormal); + body1.internalApplyImpulse(tmp, contactConstraint.angularComponentA, j1); + } + if (body2.invMass != 0f) { + tmp.scale(body2.invMass, contactConstraint.contactNormal); + body2.internalApplyImpulse(tmp, contactConstraint.angularComponentB, -j1); + } + + } + return 0f; + } + finally { + stack.vectors.pop(); + } + } + + protected void addFrictionConstraint(Vector3f normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, ManifoldPoint cp, Vector3f rel_pos1, Vector3f rel_pos2, CollisionObject colObj0, CollisionObject colObj1, float relaxation) { + stack.vectors.push(); + try { + RigidBody body0 = RigidBody.upcast(colObj0); + RigidBody body1 = RigidBody.upcast(colObj1); + + SolverConstraint solverConstraint = constraintsPool.get(); + tmpSolverFrictionConstraintPool.add(solverConstraint); + + solverConstraint.contactNormal.set(normalAxis); + + solverConstraint.solverBodyIdA = solverBodyIdA; + solverConstraint.solverBodyIdB = solverBodyIdB; + solverConstraint.constraintType = SolverConstraintType.SOLVER_FRICTION_1D; + solverConstraint.frictionIndex = frictionIndex; + + solverConstraint.friction = cp.combinedFriction; + + solverConstraint.appliedImpulse = 0f; + solverConstraint.appliedVelocityImpulse = 0f; + solverConstraint.penetration = 0f; + { + Vector3f ftorqueAxis1 = stack.vectors.get(); + ftorqueAxis1.cross(rel_pos1, solverConstraint.contactNormal); + solverConstraint.relpos1CrossNormal.set(ftorqueAxis1); + if (body0 != null) { + solverConstraint.angularComponentA.set(ftorqueAxis1); + body0.getInvInertiaTensorWorld().transform(solverConstraint.angularComponentA); + } + else { + solverConstraint.angularComponentA.set(0f, 0f, 0f); + } + } + { + Vector3f ftorqueAxis1 = stack.vectors.get(); + ftorqueAxis1.cross(rel_pos2, solverConstraint.contactNormal); + solverConstraint.relpos2CrossNormal.set(ftorqueAxis1); + if (body1 != null) { + solverConstraint.angularComponentB.set(ftorqueAxis1); + body1.getInvInertiaTensorWorld().transform(solverConstraint.angularComponentB); + } + else { + solverConstraint.angularComponentB.set(0f, 0f, 0f); + } + } + + //#ifdef COMPUTE_IMPULSE_DENOM + // btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); + // btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); + //#else + Vector3f vec = stack.vectors.get(); + float denom0 = 0f; + float denom1 = 0f; + if (body0 != null) { + vec.cross(solverConstraint.angularComponentA, rel_pos1); + denom0 = body0.getInvMass() + normalAxis.dot(vec); + } + if (body1 != null) { + vec.cross(solverConstraint.angularComponentB, rel_pos2); + denom1 = body1.getInvMass() + normalAxis.dot(vec); + } + //#endif //COMPUTE_IMPULSE_DENOM + + float denom = relaxation / (denom0 + denom1); + solverConstraint.jacDiagABInv = denom; + } + finally { + stack.vectors.pop(); + } + } + + public float solveGroupCacheFriendlySetup(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifoldPtr, int manifold_offset, int numManifolds, List<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer/*,btStackAlloc* stackAlloc*/) { + BulletGlobals.pushProfile("solveGroupCacheFriendlySetup"); + stack.vectors.push(); + try { + + if ((numConstraints + numManifolds) == 0) { + // printf("empty\n"); + return 0f; + } + PersistentManifold manifold = null; + CollisionObject colObj0 = null, colObj1 = null; + + //btRigidBody* rb0=0,*rb1=0; + + // //#ifdef FORCE_REFESH_CONTACT_MANIFOLDS + // + // BEGIN_PROFILE("refreshManifolds"); + // + // int i; + // + // + // + // for (i=0;i<numManifolds;i++) + // { + // manifold = manifoldPtr[i]; + // rb1 = (btRigidBody*)manifold->getBody1(); + // rb0 = (btRigidBody*)manifold->getBody0(); + // + // manifold->refreshContactPoints(rb0->getCenterOfMassTransform(),rb1->getCenterOfMassTransform()); + // + // } + // + // END_PROFILE("refreshManifolds"); + // //#endif //FORCE_REFESH_CONTACT_MANIFOLDS + + Vector3f color = stack.vectors.get(0f, 1f, 0f); + + //int sizeofSB = sizeof(btSolverBody); + //int sizeofSC = sizeof(btSolverConstraint); + + //if (1) + { + //if m_stackAlloc, try to pack bodies/constraints to speed up solving + // btBlock* sablock; + // sablock = stackAlloc->beginBlock(); + + // int memsize = 16; + // unsigned char* stackMemory = stackAlloc->allocate(memsize); + + + // todo: use stack allocator for this temp memory + int minReservation = numManifolds * 2; + + //m_tmpSolverBodyPool.reserve(minReservation); + + //don't convert all bodies, only the one we need so solver the constraints + /* + { + for (int i=0;i<numBodies;i++) + { + btRigidBody* rb = btRigidBody::upcast(bodies[i]); + if (rb && (rb->getIslandTag() >= 0)) + { + btAssert(rb->getCompanionId() < 0); + int solverBodyId = m_tmpSolverBodyPool.size(); + btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&solverBody,rb); + rb->setCompanionId(solverBodyId); + } + } + } + */ + + //m_tmpSolverConstraintPool.reserve(minReservation); + //m_tmpSolverFrictionConstraintPool.reserve(minReservation); + + { + int i; + + for (i = 0; i < numManifolds; i++) { + manifold = manifoldPtr.get(manifold_offset+i); + colObj0 = (CollisionObject) manifold.getBody0(); + colObj1 = (CollisionObject) manifold.getBody1(); + + int solverBodyIdA = -1; + int solverBodyIdB = -1; + + if (manifold.getNumContacts() != 0) { + if (colObj0.getIslandTag() >= 0) { + if (colObj0.getCompanionId() >= 0) { + // body has already been converted + solverBodyIdA = colObj0.getCompanionId(); + } + else { + solverBodyIdA = tmpSolverBodyPool.size(); + SolverBody solverBody = bodiesPool.get(); + tmpSolverBodyPool.add(solverBody); + initSolverBody(solverBody, colObj0); + colObj0.setCompanionId(solverBodyIdA); + } + } + else { + // create a static body + solverBodyIdA = tmpSolverBodyPool.size(); + SolverBody solverBody = bodiesPool.get(); + tmpSolverBodyPool.add(solverBody); + initSolverBody(solverBody, colObj0); + } + + if (colObj1.getIslandTag() >= 0) { + if (colObj1.getCompanionId() >= 0) { + solverBodyIdB = colObj1.getCompanionId(); + } + else { + solverBodyIdB = tmpSolverBodyPool.size(); + SolverBody solverBody = bodiesPool.get(); + tmpSolverBodyPool.add(solverBody); + initSolverBody(solverBody, colObj1); + colObj1.setCompanionId(solverBodyIdB); + } + } + else { + // create a static body + solverBodyIdB = tmpSolverBodyPool.size(); + SolverBody solverBody = bodiesPool.get(); + tmpSolverBodyPool.add(solverBody); + initSolverBody(solverBody, colObj1); + } + } + + Vector3f rel_pos1 = stack.vectors.get(); + Vector3f rel_pos2 = stack.vectors.get(); + float relaxation; + + for (int j = 0; j < manifold.getNumContacts(); j++) { + + ManifoldPoint cp = manifold.getContactPoint(j); + + if (debugDrawer != null) { + debugDrawer.drawContactPoint(cp.positionWorldOnB, cp.normalWorldOnB, cp.getDistance(), cp.getLifeTime(), color); + } + + if (cp.getDistance() <= 0f) { + Vector3f pos1 = cp.getPositionWorldOnA(); + Vector3f pos2 = cp.getPositionWorldOnB(); + + rel_pos1.sub(pos1, colObj0.getWorldTransform().origin); + rel_pos2.sub(pos2, colObj1.getWorldTransform().origin); + + relaxation = 1f; + float rel_vel; + Vector3f vel = stack.vectors.get(); + + int frictionIndex = tmpSolverConstraintPool.size(); + + { + SolverConstraint solverConstraint = constraintsPool.get(); + tmpSolverConstraintPool.add(solverConstraint); + RigidBody rb0 = RigidBody.upcast(colObj0); + RigidBody rb1 = RigidBody.upcast(colObj1); + + solverConstraint.solverBodyIdA = solverBodyIdA; + solverConstraint.solverBodyIdB = solverBodyIdB; + solverConstraint.constraintType = SolverConstraintType.SOLVER_CONTACT_1D; + + Vector3f torqueAxis0 = stack.vectors.get(); + torqueAxis0.cross(rel_pos1, cp.normalWorldOnB); + + if (rb0 != null) { + solverConstraint.angularComponentA.set(torqueAxis0); + rb0.getInvInertiaTensorWorld().transform(solverConstraint.angularComponentA); + } + else { + solverConstraint.angularComponentA.set(0f, 0f, 0f); + } + + Vector3f torqueAxis1 = stack.vectors.get(); + torqueAxis1.cross(rel_pos2, cp.normalWorldOnB); + + if (rb1 != null) { + solverConstraint.angularComponentB.set(torqueAxis1); + rb1.getInvInertiaTensorWorld().transform(solverConstraint.angularComponentB); + } + else { + solverConstraint.angularComponentB.set(0f, 0f, 0f); + } + + { + //#ifdef COMPUTE_IMPULSE_DENOM + //btScalar denom0 = rb0->computeImpulseDenominator(pos1,cp.m_normalWorldOnB); + //btScalar denom1 = rb1->computeImpulseDenominator(pos2,cp.m_normalWorldOnB); + //#else + Vector3f vec = stack.vectors.get(); + float denom0 = 0f; + float denom1 = 0f; + if (rb0 != null) { + vec.cross(solverConstraint.angularComponentA, rel_pos1); + denom0 = rb0.getInvMass() + cp.normalWorldOnB.dot(vec); + } + if (rb1 != null) { + vec.cross(solverConstraint.angularComponentB, rel_pos2); + denom1 = rb1.getInvMass() + cp.normalWorldOnB.dot(vec); + } + //#endif //COMPUTE_IMPULSE_DENOM + + float denom = relaxation / (denom0 + denom1); + solverConstraint.jacDiagABInv = denom; + } + + solverConstraint.contactNormal.set(cp.normalWorldOnB); + solverConstraint.relpos1CrossNormal.cross(rel_pos1, cp.normalWorldOnB); + solverConstraint.relpos2CrossNormal.cross(rel_pos2, cp.normalWorldOnB); + + Vector3f vel1 = rb0 != null ? stack.vectors.get(rb0.getVelocityInLocalPoint(rel_pos1)) : stack.vectors.get(0f, 0f, 0f); + Vector3f vel2 = rb1 != null ? stack.vectors.get(rb1.getVelocityInLocalPoint(rel_pos2)) : stack.vectors.get(0f, 0f, 0f); + + vel.sub(vel1, vel2); + + rel_vel = cp.normalWorldOnB.dot(vel); + + solverConstraint.penetration = cp.getDistance();///btScalar(infoGlobal.m_numIterations); + solverConstraint.friction = cp.combinedFriction; + solverConstraint.restitution = restitutionCurve(rel_vel, cp.combinedRestitution); + if (solverConstraint.restitution <= 0f) { + solverConstraint.restitution = 0f; + } + + float penVel = -solverConstraint.penetration / infoGlobal.timeStep; + solverConstraint.penetration *= -(infoGlobal.erp / infoGlobal.timeStep); + + if (solverConstraint.restitution > penVel) { + solverConstraint.penetration = 0f; + } + + solverConstraint.appliedImpulse = 0f; + solverConstraint.appliedVelocityImpulse = 0f; + } + + { + Vector3f frictionDir1 = stack.vectors.get(); + frictionDir1.scale(rel_vel, cp.normalWorldOnB); + frictionDir1.sub(vel, frictionDir1); + + float lat_rel_vel = frictionDir1.lengthSquared(); + if (lat_rel_vel > BulletGlobals.FLT_EPSILON)//0.0f) + { + frictionDir1.scale(1f / (float) Math.sqrt(lat_rel_vel)); + addFrictionConstraint(frictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + Vector3f frictionDir2 = stack.vectors.get(); + frictionDir2.cross(frictionDir1, cp.normalWorldOnB); + frictionDir2.normalize();//?? + addFrictionConstraint(frictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + } + else { + // re-calculate friction direction every frame, todo: check if this is really needed + Vector3f /*frictionDir1 = stack.vectors.get(),*/ frictionDir2 = stack.vectors.get(); + TransformUtil.planeSpace1(cp.normalWorldOnB, frictionDir1, frictionDir2); + addFrictionConstraint(frictionDir1, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + addFrictionConstraint(frictionDir2, solverBodyIdA, solverBodyIdB, frictionIndex, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation); + } + } + } + } + } + } + } + + // TODO: btContactSolverInfo info = infoGlobal; + + { + int j; + for (j = 0; j < numConstraints; j++) { + TypedConstraint constraint = constraints.get(constraints_offset+j); + constraint.buildJacobian(); + } + } + + + + int numConstraintPool = tmpSolverConstraintPool.size(); + int numFrictionPool = tmpSolverFrictionConstraintPool.size(); + + // todo: use stack allocator for such temporarily memory, same for solver bodies/constraints + MiscUtil.resize(orderTmpConstraintPool, numConstraintPool, 0); + MiscUtil.resize(orderFrictionConstraintPool, numFrictionPool, 0); + { + int i; + for (i = 0; i < numConstraintPool; i++) { + orderTmpConstraintPool.set(i, i); + } + for (i = 0; i < numFrictionPool; i++) { + orderFrictionConstraintPool.set(i, i); + } + } + + return 0f; + } + finally { + stack.vectors.pop(); + BulletGlobals.popProfile(); + } + } + + public float solveGroupCacheFriendlyIterations(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifoldPtr, int manifold_offset, int numManifolds, List<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer/*,btStackAlloc* stackAlloc*/) { + BulletGlobals.pushProfile("solveGroupCacheFriendlyIterations"); + try { + int numConstraintPool = tmpSolverConstraintPool.size(); + int numFrictionPool = tmpSolverFrictionConstraintPool.size(); + + // should traverse the contacts random order... + int iteration; + { + for (iteration = 0; iteration < infoGlobal.numIterations; iteration++) { + + int j; + if ((solverMode & SolverMode.SOLVER_RANDMIZE_ORDER) != 0) { + if ((iteration & 7) == 0) { + for (j = 0; j < numConstraintPool; ++j) { + int tmp = orderTmpConstraintPool.get(j); + int swapi = randInt2(j + 1); + orderTmpConstraintPool.set(j, orderTmpConstraintPool.get(swapi)); + orderTmpConstraintPool.set(swapi, tmp); + } + + for (j = 0; j < numFrictionPool; ++j) { + int tmp = orderFrictionConstraintPool.get(j); + int swapi = randInt2(j + 1); + orderFrictionConstraintPool.set(j, orderFrictionConstraintPool.get(swapi)); + orderFrictionConstraintPool.set(swapi, tmp); + } + } + } + + for (j = 0; j < numConstraints; j++) { + BulletGlobals.pushProfile("solveConstraint"); + try { + TypedConstraint constraint = constraints.get(constraints_offset+j); + // todo: use solver bodies, so we don't need to copy from/to btRigidBody + + if ((constraint.getRigidBodyA().getIslandTag() >= 0) && (constraint.getRigidBodyA().getCompanionId() >= 0)) { + tmpSolverBodyPool.get(constraint.getRigidBodyA().getCompanionId()).writebackVelocity(); + } + if ((constraint.getRigidBodyB().getIslandTag() >= 0) && (constraint.getRigidBodyB().getCompanionId() >= 0)) { + tmpSolverBodyPool.get(constraint.getRigidBodyB().getCompanionId()).writebackVelocity(); + } + + constraint.solveConstraint(infoGlobal.timeStep); + + if ((constraint.getRigidBodyA().getIslandTag() >= 0) && (constraint.getRigidBodyA().getCompanionId() >= 0)) { + tmpSolverBodyPool.get(constraint.getRigidBodyA().getCompanionId()).readVelocity(); + } + if ((constraint.getRigidBodyB().getIslandTag() >= 0) && (constraint.getRigidBodyB().getCompanionId() >= 0)) { + tmpSolverBodyPool.get(constraint.getRigidBodyB().getCompanionId()).readVelocity(); + } + } + finally { + BulletGlobals.popProfile(); + } + } + + { + BulletGlobals.pushProfile("resolveSingleCollisionCombinedCacheFriendly"); + try { + int numPoolConstraints = tmpSolverConstraintPool.size(); + for (j = 0; j < numPoolConstraints; j++) { + SolverConstraint solveManifold = tmpSolverConstraintPool.get(orderTmpConstraintPool.get(j)); + resolveSingleCollisionCombinedCacheFriendly(tmpSolverBodyPool.get(solveManifold.solverBodyIdA), + tmpSolverBodyPool.get(solveManifold.solverBodyIdB), solveManifold, infoGlobal); + } + } + finally { + BulletGlobals.popProfile(); + } + } + + { + BulletGlobals.pushProfile("resolveSingleFrictionCacheFriendly"); + try { + int numFrictionPoolConstraints = tmpSolverFrictionConstraintPool.size(); + + for (j = 0; j < numFrictionPoolConstraints; j++) { + SolverConstraint solveManifold = tmpSolverFrictionConstraintPool.get(orderFrictionConstraintPool.get(j)); + resolveSingleFrictionCacheFriendly(tmpSolverBodyPool.get(solveManifold.solverBodyIdA), + tmpSolverBodyPool.get(solveManifold.solverBodyIdB), solveManifold, infoGlobal, + tmpSolverConstraintPool.get(solveManifold.frictionIndex).appliedImpulse); + } + } + finally { + BulletGlobals.popProfile(); + } + } + } + } + + return 0f; + } + finally { + BulletGlobals.popProfile(); + } + } + + public float solveGroupCacheFriendly(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifoldPtr, int manifold_offset, int numManifolds, List<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer/*,btStackAlloc* stackAlloc*/) { + int i; + + solveGroupCacheFriendlySetup(bodies, numBodies, manifoldPtr, manifold_offset, numManifolds, constraints, constraints_offset, numConstraints, infoGlobal, debugDrawer/*, stackAlloc*/); + solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, manifold_offset, numManifolds, constraints, constraints_offset, numConstraints, infoGlobal, debugDrawer/*, stackAlloc*/); + + for (i = 0; i < tmpSolverBodyPool.size(); i++) { + SolverBody body = tmpSolverBodyPool.get(i); + body.writebackVelocity(); + bodiesPool.release(body); + } + + // printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size()); + + /* + printf("m_tmpSolverBodyPool.size() = %i\n",m_tmpSolverBodyPool.size()); + printf("m_tmpSolverConstraintPool.size() = %i\n",m_tmpSolverConstraintPool.size()); + printf("m_tmpSolverFrictionConstraintPool.size() = %i\n",m_tmpSolverFrictionConstraintPool.size()); + printf("m_tmpSolverBodyPool.capacity() = %i\n",m_tmpSolverBodyPool.capacity()); + printf("m_tmpSolverConstraintPool.capacity() = %i\n",m_tmpSolverConstraintPool.capacity()); + printf("m_tmpSolverFrictionConstraintPool.capacity() = %i\n",m_tmpSolverFrictionConstraintPool.capacity()); + */ + + tmpSolverBodyPool.clear(); + + for (i=0; i<tmpSolverConstraintPool.size(); i++) { + constraintsPool.release(tmpSolverConstraintPool.get(i)); + } + tmpSolverConstraintPool.clear(); + + for (i=0; i<tmpSolverFrictionConstraintPool.size(); i++) { + constraintsPool.release(tmpSolverFrictionConstraintPool.get(i)); + } + tmpSolverFrictionConstraintPool.clear(); + + return 0f; + } + + /** + * Sequentially applies impulses. + */ + @Override + public float solveGroup(List<CollisionObject> bodies, int numBodies, List<PersistentManifold> manifoldPtr, int manifold_offset, int numManifolds, List<TypedConstraint> constraints, int constraints_offset, int numConstraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer, Dispatcher dispatcher) { + BulletGlobals.pushProfile("solveGroup"); + try { + // TODO: solver cache friendly + if ((getSolverMode() & SolverMode.SOLVER_CACHE_FRIENDLY) != 0) { + // you need to provide at least some bodies + // SimpleDynamicsWorld needs to switch off SOLVER_CACHE_FRIENDLY + assert (bodies != null); + assert (numBodies != 0); + float value = solveGroupCacheFriendly(bodies, numBodies, manifoldPtr, manifold_offset, numManifolds, constraints, constraints_offset, numConstraints, infoGlobal, debugDrawer/*,stackAlloc*/); + return value; + } + + ContactSolverInfo info = new ContactSolverInfo(infoGlobal); + + int numiter = infoGlobal.numIterations; + + int totalPoints = 0; + { + short j; + for (j = 0; j < numManifolds; j++) { + PersistentManifold manifold = manifoldPtr.get(manifold_offset+j); + prepareConstraints(manifold, info, debugDrawer); + + for (short p = 0; p < manifoldPtr.get(manifold_offset+j).getNumContacts(); p++) { + gOrder[totalPoints].manifoldIndex = j; + gOrder[totalPoints].pointIndex = p; + totalPoints++; + } + } + } + + { + int j; + for (j = 0; j < numConstraints; j++) { + TypedConstraint constraint = constraints.get(constraints_offset+j); + constraint.buildJacobian(); + } + } + + // should traverse the contacts random order... + int iteration; + { + for (iteration = 0; iteration < numiter; iteration++) { + int j; + if ((solverMode & SolverMode.SOLVER_RANDMIZE_ORDER) != 0) { + if ((iteration & 7) == 0) { + for (j = 0; j < totalPoints; ++j) { + // JAVA NOTE: swaps references instead of copying values (but that's fine in this context) + OrderIndex tmp = gOrder[j]; + int swapi = randInt2(j + 1); + gOrder[j] = gOrder[swapi]; + gOrder[swapi] = tmp; + } + } + } + + for (j = 0; j < numConstraints; j++) { + TypedConstraint constraint = constraints.get(constraints_offset+j); + constraint.solveConstraint(info.timeStep); + } + + for (j = 0; j < totalPoints; j++) { + PersistentManifold manifold = manifoldPtr.get(manifold_offset+gOrder[j].manifoldIndex); + solve((RigidBody) manifold.getBody0(), + (RigidBody) manifold.getBody1(), manifold.getContactPoint(gOrder[j].pointIndex), info, iteration, debugDrawer); + } + + for (j = 0; j < totalPoints; j++) { + PersistentManifold manifold = manifoldPtr.get(manifold_offset+gOrder[j].manifoldIndex); + solveFriction((RigidBody) manifold.getBody0(), + (RigidBody) manifold.getBody1(), manifold.getContactPoint(gOrder[j].pointIndex), info, iteration, debugDrawer); + } + + } + } + + return 0f; + } + finally { + BulletGlobals.popProfile(); + } + } + + protected void prepareConstraints(PersistentManifold manifoldPtr, ContactSolverInfo info, IDebugDraw debugDrawer) { + stack.pushCommonMath(); + try { + RigidBody body0 = (RigidBody) manifoldPtr.getBody0(); + RigidBody body1 = (RigidBody) manifoldPtr.getBody1(); + + // only necessary to refresh the manifold once (first iteration). The integration is done outside the loop + { + //#ifdef FORCE_REFESH_CONTACT_MANIFOLDS + //manifoldPtr->refreshContactPoints(body0->getCenterOfMassTransform(),body1->getCenterOfMassTransform()); + //#endif //FORCE_REFESH_CONTACT_MANIFOLDS + int numpoints = manifoldPtr.getNumContacts(); + + BulletGlobals.gTotalContactPoints += numpoints; + + Vector3f color = stack.vectors.get(0f, 1f, 0f); + for (int i = 0; i < numpoints; i++) { + ManifoldPoint cp = manifoldPtr.getContactPoint(i); + if (cp.getDistance() <= 0f) { + Vector3f pos1 = cp.getPositionWorldOnA(); + Vector3f pos2 = cp.getPositionWorldOnB(); + + Vector3f rel_pos1 = stack.vectors.get(); + rel_pos1.sub(pos1, body0.getCenterOfMassPosition()); + + Vector3f rel_pos2 = stack.vectors.get(); + rel_pos2.sub(pos2, body1.getCenterOfMassPosition()); + + // this jacobian entry is re-used for all iterations + Matrix3f mat1 = stack.matrices.get(body0.getCenterOfMassTransform().basis); + mat1.transpose(); + + Matrix3f mat2 = stack.matrices.get(body1.getCenterOfMassTransform().basis); + mat2.transpose(); + + JacobianEntry jac = jacobiansPool.get(); + jac.init(mat1, mat2, + rel_pos1, rel_pos2, cp.normalWorldOnB, body0.getInvInertiaDiagLocal(), body0.getInvMass(), + body1.getInvInertiaDiagLocal(), body1.getInvMass()); + + float jacDiagAB = jac.getDiagonal(); + jacobiansPool.release(jac); + + ConstraintPersistentData cpd = (ConstraintPersistentData) cp.userPersistentData; + if (cpd != null) { + // might be invalid + cpd.persistentLifeTime++; + if (cpd.persistentLifeTime != cp.getLifeTime()) { + //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); + //new (cpd) btConstraintPersistentData; + cpd.reset(); + cpd.persistentLifeTime = cp.getLifeTime(); + + } + else { + //printf("Persistent: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime()); + } + } + else { + // todo: should this be in a pool? + //void* mem = btAlignedAlloc(sizeof(btConstraintPersistentData),16); + //cpd = new (mem)btConstraintPersistentData; + cpd = new ConstraintPersistentData(); + //assert(cpd != null); + + totalCpd++; + //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd); + cp.userPersistentData = cpd; + cpd.persistentLifeTime = cp.getLifeTime(); + //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime()); + } + assert (cpd != null); + + cpd.jacDiagABInv = 1f / jacDiagAB; + + // Dependent on Rigidbody A and B types, fetch the contact/friction response func + // perhaps do a similar thing for friction/restutution combiner funcs... + + cpd.frictionSolverFunc = frictionDispatch[body0.frictionSolverType][body1.frictionSolverType]; + cpd.contactSolverFunc = contactDispatch[body0.contactSolverType][body1.contactSolverType]; + + Vector3f vel1 = stack.vectors.get(body0.getVelocityInLocalPoint(rel_pos1)); + Vector3f vel2 = stack.vectors.get(body1.getVelocityInLocalPoint(rel_pos2)); + Vector3f vel = stack.vectors.get(); + vel.sub(vel1, vel2); + + float rel_vel; + rel_vel = cp.normalWorldOnB.dot(vel); + + float combinedRestitution = cp.combinedRestitution; + + cpd.penetration = cp.getDistance(); ///btScalar(info.m_numIterations); + cpd.friction = cp.combinedFriction; + cpd.restitution = restitutionCurve(rel_vel, combinedRestitution); + if (cpd.restitution <= 0f) { + cpd.restitution = 0f; + } + + // restitution and penetration work in same direction so + // rel_vel + + float penVel = -cpd.penetration / info.timeStep; + + if (cpd.restitution > penVel) { + cpd.penetration = 0f; + } + + float relaxation = info.damping; + if ((solverMode & SolverMode.SOLVER_USE_WARMSTARTING) != 0) { + cpd.appliedImpulse *= relaxation; + } + else { + cpd.appliedImpulse = 0f; + } + + // for friction + cpd.prevAppliedImpulse = cpd.appliedImpulse; + + // re-calculate friction direction every frame, todo: check if this is really needed + TransformUtil.planeSpace1(cp.normalWorldOnB, cpd.frictionWorldTangential0, cpd.frictionWorldTangential1); + + //#define NO_FRICTION_WARMSTART 1 + //#ifdef NO_FRICTION_WARMSTART + cpd.accumulatedTangentImpulse0 = 0f; + cpd.accumulatedTangentImpulse1 = 0f; + //#endif //NO_FRICTION_WARMSTART + float denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential0); + float denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential0); + float denom = relaxation / (denom0 + denom1); + cpd.jacDiagABInvTangent0 = denom; + + denom0 = body0.computeImpulseDenominator(pos1, cpd.frictionWorldTangential1); + denom1 = body1.computeImpulseDenominator(pos2, cpd.frictionWorldTangential1); + denom = relaxation / (denom0 + denom1); + cpd.jacDiagABInvTangent1 = denom; + + Vector3f totalImpulse = stack.vectors.get(); + //btVector3 totalImpulse = + // //#ifndef NO_FRICTION_WARMSTART + // //cpd->m_frictionWorldTangential0*cpd->m_accumulatedTangentImpulse0+ + // //cpd->m_frictionWorldTangential1*cpd->m_accumulatedTangentImpulse1+ + // //#endif //NO_FRICTION_WARMSTART + // cp.normalWorldOnB*cpd.appliedImpulse; + totalImpulse.scale(cpd.appliedImpulse, cp.normalWorldOnB); + + /// + { + Vector3f torqueAxis0 = stack.vectors.get(); + torqueAxis0.cross(rel_pos1, cp.normalWorldOnB); + + cpd.angularComponentA.set(torqueAxis0); + body0.getInvInertiaTensorWorld().transform(cpd.angularComponentA); + + Vector3f torqueAxis1 = stack.vectors.get(); + torqueAxis1.cross(rel_pos2, cp.normalWorldOnB); + + cpd.angularComponentB.set(torqueAxis1); + body1.getInvInertiaTensorWorld().transform(cpd.angularComponentB); + } + { + Vector3f ftorqueAxis0 = stack.vectors.get(); + ftorqueAxis0.cross(rel_pos1, cpd.frictionWorldTangential0); + + cpd.frictionAngularComponent0A.set(ftorqueAxis0); + body0.getInvInertiaTensorWorld().transform(cpd.frictionAngularComponent0A); + } + { + Vector3f ftorqueAxis1 = stack.vectors.get(); + ftorqueAxis1.cross(rel_pos1, cpd.frictionWorldTangential1); + + cpd.frictionAngularComponent1A.set(ftorqueAxis1); + body0.getInvInertiaTensorWorld().transform(cpd.frictionAngularComponent1A); + } + { + Vector3f ftorqueAxis0 = stack.vectors.get(); + ftorqueAxis0.cross(rel_pos2, cpd.frictionWorldTangential0); + + cpd.frictionAngularComponent0B.set(ftorqueAxis0); + body1.getInvInertiaTensorWorld().transform(cpd.frictionAngularComponent0B); + } + { + Vector3f ftorqueAxis1 = stack.vectors.get(); + ftorqueAxis1.cross(rel_pos2, cpd.frictionWorldTangential1); + + cpd.frictionAngularComponent1B.set(ftorqueAxis1); + body1.getInvInertiaTensorWorld().transform(cpd.frictionAngularComponent1B); + } + + /// + + // apply previous frames impulse on both bodies + body0.applyImpulse(totalImpulse, rel_pos1); + + Vector3f tmp = stack.vectors.get(totalImpulse); + tmp.negate(); + body1.applyImpulse(tmp, rel_pos2); + } + + } + } + } + finally { + stack.popCommonMath(); + } + } + + public float solveCombinedContactFriction(RigidBody body0, RigidBody body1, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDrawer) { + stack.vectors.push(); + try { + float maxImpulse = 0f; + + Vector3f color = stack.vectors.get(0f, 1f, 0f); + { + if (cp.getDistance() <= 0f) { + + if (iter == 0) { + if (debugDrawer != null) { + debugDrawer.drawContactPoint(cp.positionWorldOnB, cp.normalWorldOnB, cp.getDistance(), cp.getLifeTime(), color); + } + } + + { + //btConstraintPersistentData* cpd = (btConstraintPersistentData*) cp.m_userPersistentData; + float impulse = ContactConstraint.resolveSingleCollisionCombined(body0, body1, cp, info); + + if (maxImpulse < impulse) { + maxImpulse = impulse; + } + } + } + } + return maxImpulse; + } + finally { + stack.vectors.pop(); + } + } + + protected float solve(RigidBody body0, RigidBody body1, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDrawer) { + stack.vectors.push(); + try { + float maxImpulse = 0f; + + Vector3f color = stack.vectors.get(0f, 1f, 0f); + { + if (cp.getDistance() <= 0f) { + + if (iter == 0) { + if (debugDrawer != null) { + debugDrawer.drawContactPoint(cp.positionWorldOnB, cp.normalWorldOnB, cp.getDistance(), cp.getLifeTime(), color); + } + } + + { + ConstraintPersistentData cpd = (ConstraintPersistentData) cp.userPersistentData; + float impulse = cpd.contactSolverFunc.invoke(body0, body1, cp, info); + + if (maxImpulse < impulse) { + maxImpulse = impulse; + } + } + } + } + + return maxImpulse; + } + finally { + stack.vectors.pop(); + } + } + + protected float solveFriction(RigidBody body0, RigidBody body1, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDrawer) { + stack.vectors.push(); + try { + Vector3f color = stack.vectors.get(0f, 1f, 0f); + { + if (cp.getDistance() <= 0f) { + ConstraintPersistentData cpd = (ConstraintPersistentData) cp.userPersistentData; + cpd.frictionSolverFunc.invoke(body0, body1, cp, info); + } + } + return 0f; + } + finally { + stack.vectors.pop(); + } + } + + @Override + public void reset() { + btSeed2 = 0; + } + + /** + * Advanced: Override the default contact solving function for contacts, for certain types of rigidbody<br> + * See RigidBody.contactSolverType and RigidBody.frictionSolverType + */ + public void setContactSolverFunc(ContactSolverFunc func, int type0, int type1) { + contactDispatch[type0][type1] = func; + } + + /** + * Advanced: Override the default friction solving function for contacts, for certain types of rigidbody<br> + * See RigidBody.contactSolverType and RigidBody.frictionSolverType + */ + public void setFrictionSolverFunc(ContactSolverFunc func, int type0, int type1) { + frictionDispatch[type0][type1] = func; + } + + public int getSolverMode() { + return solverMode; + } + + public void setSolverMode(int solverMode) { + this.solverMode = solverMode; + } + + public void setRandSeed(long seed) { + btSeed2 = seed; + } + + public long getRandSeed() { + return btSeed2; + } + + //////////////////////////////////////////////////////////////////////////// + + private static class OrderIndex { + public int manifoldIndex; + public int pointIndex; + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverBody.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverBody.java new file mode 100644 index 0000000..92f59f7 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverBody.java @@ -0,0 +1,82 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +import javabullet.BulletStack; +import javabullet.dynamics.RigidBody; +import javax.vecmath.Vector3f; + +/** + * SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. + * + * @author jezek2 + */ +public class SolverBody { + + protected final BulletStack stack = BulletStack.get(); + + public final Vector3f angularVelocity = new Vector3f(); + public float angularFactor; + public float invMass; + public float friction; + public RigidBody originalBody; + public final Vector3f linearVelocity = new Vector3f(); + public final Vector3f centerOfMassPosition = new Vector3f(); + + public void getVelocityInLocalPoint(Vector3f rel_pos, Vector3f velocity) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + tmp.cross(angularVelocity, rel_pos); + velocity.add(linearVelocity, tmp); + } + finally { + stack.vectors.pop(); + } + } + + /** + * Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position. + */ + public void internalApplyImpulse(Vector3f linearComponent, Vector3f angularComponent, float impulseMagnitude) { + linearVelocity.scaleAdd(impulseMagnitude, linearComponent, linearVelocity); + angularVelocity.scaleAdd(impulseMagnitude * angularFactor, angularComponent, angularVelocity); + } + + public void writebackVelocity() { + if (invMass != 0f) { + originalBody.setLinearVelocity(linearVelocity); + originalBody.setAngularVelocity(angularVelocity); + //m_originalBody->setCompanionId(-1); + } + } + + public void readVelocity() { + if (invMass != 0f) { + linearVelocity.set(originalBody.getLinearVelocity()); + angularVelocity.set(originalBody.getAngularVelocity()); + } + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverConstraint.java new file mode 100644 index 0000000..70c37af --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverConstraint.java @@ -0,0 +1,55 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +import javax.vecmath.Vector3f; + +/** + * 1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. + * + * @author jezek2 + */ +public class SolverConstraint { + + public final Vector3f relpos1CrossNormal = new Vector3f(); + public final Vector3f contactNormal = new Vector3f(); + + public final Vector3f relpos2CrossNormal = new Vector3f(); + public final Vector3f angularComponentA = new Vector3f(); + + public final Vector3f angularComponentB = new Vector3f(); + public float appliedVelocityImpulse; + public float appliedImpulse; + public int solverBodyIdA; + public int solverBodyIdB; + + public float friction; + public float restitution; + public float jacDiagABInv; + public float penetration; + + public SolverConstraintType constraintType; + public int frictionIndex; + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverConstraintType.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverConstraintType.java new file mode 100644 index 0000000..eaf6286 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverConstraintType.java @@ -0,0 +1,33 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +/** + * + * @author jezek2 + */ +public enum SolverConstraintType { + SOLVER_CONTACT_1D, + SOLVER_FRICTION_1D +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverMode.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverMode.java new file mode 100644 index 0000000..bd90d9b --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/SolverMode.java @@ -0,0 +1,37 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +/** + * + * @author jezek2 + */ +public class SolverMode { + + public static final int SOLVER_RANDMIZE_ORDER = 1; + public static final int SOLVER_FRICTION_SEPARATE = 2; + public static final int SOLVER_USE_WARMSTARTING = 4; + public static final int SOLVER_CACHE_FRIENDLY = 8; + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/TranslationalLimitMotor.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/TranslationalLimitMotor.java new file mode 100644 index 0000000..d8dafbc --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/TranslationalLimitMotor.java @@ -0,0 +1,155 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +/* +2007-09-09 +btGeneric6DofConstraint Refactored by Francisco Le�n +email: [email protected] +http://gimpact.sf.net +*/ + +package javabullet.dynamics.constraintsolver; + +import javabullet.BulletStack; +import javabullet.dynamics.RigidBody; +import javabullet.linearmath.VectorUtil; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class TranslationalLimitMotor { + + protected final BulletStack stack = BulletStack.get(); + + public final Vector3f lowerLimit = new Vector3f(); //!< the constraint lower limits + public final Vector3f upperLimit = new Vector3f(); //!< the constraint upper limits + public final Vector3f accumulatedImpulse = new Vector3f(); + + public float limitSoftness; //!< Softness for linear limit + public float damping; //!< Damping for linear limit + public float restitution; //! Bounce parameter for linear limit + + public TranslationalLimitMotor() { + lowerLimit.set(0f, 0f, 0f); + upperLimit.set(0f, 0f, 0f); + accumulatedImpulse.set(0f, 0f, 0f); + + limitSoftness = 0.7f; + damping = 1.0f; + restitution = 0.5f; + } + + public TranslationalLimitMotor(TranslationalLimitMotor other) { + lowerLimit.set(other.lowerLimit); + upperLimit.set(other.upperLimit); + accumulatedImpulse.set(other.accumulatedImpulse); + + limitSoftness = other.limitSoftness; + damping = other.damping; + restitution = other.restitution; + } + + /** + * Test limit.<p> + * - free means upper < lower,<br> + * - locked means upper == lower<br> + * - limited means upper > lower<br> + * - limitIndex: first 3 are linear, next 3 are angular + */ + public boolean isLimited(int limitIndex) { + return (VectorUtil.getCoord(upperLimit, limitIndex) >= VectorUtil.getCoord(lowerLimit, limitIndex)); + } + + public float solveLinearAxis(float timeStep, float jacDiagABInv, RigidBody body1, Vector3f pointInA, RigidBody body2, Vector3f pointInB, int limit_index, Vector3f axis_normal_on_a) { + stack.vectors.push(); + try { + Vector3f tmp = stack.vectors.get(); + + // find relative velocity + Vector3f rel_pos1 = stack.vectors.get(); + rel_pos1.sub(pointInA, body1.getCenterOfMassPosition()); + + Vector3f rel_pos2 = stack.vectors.get(); + rel_pos2.sub(pointInB, body2.getCenterOfMassPosition()); + + Vector3f vel1 = stack.vectors.get(body1.getVelocityInLocalPoint(rel_pos1)); + Vector3f vel2 = stack.vectors.get(body2.getVelocityInLocalPoint(rel_pos2)); + Vector3f vel = stack.vectors.get(); + vel.sub(vel1, vel2); + + float rel_vel = axis_normal_on_a.dot(vel); + + // apply displacement correction + + // positional error (zeroth order error) + tmp.sub(pointInA, pointInB); + float depth = -(tmp).dot(axis_normal_on_a); + float lo = -1e30f; + float hi = 1e30f; + + float minLimit = VectorUtil.getCoord(lowerLimit, limit_index); + float maxLimit = VectorUtil.getCoord(upperLimit, limit_index); + + // handle the limits + if (minLimit < maxLimit) { + { + if (depth > maxLimit) { + depth -= maxLimit; + lo = 0f; + + } + else { + if (depth < minLimit) { + depth -= minLimit; + hi = 0f; + } + else { + return 0.0f; + } + } + } + } + + float normalImpulse = limitSoftness * (restitution * depth / timeStep - damping * rel_vel) * jacDiagABInv; + + float oldNormalImpulse = VectorUtil.getCoord(accumulatedImpulse, limit_index); + float sum = oldNormalImpulse + normalImpulse; + VectorUtil.setCoord(accumulatedImpulse, limit_index, sum > hi ? 0f : sum < lo ? 0f : sum); + normalImpulse = VectorUtil.getCoord(accumulatedImpulse, limit_index) - oldNormalImpulse; + + Vector3f impulse_vector = stack.vectors.get(); + impulse_vector.scale(normalImpulse, axis_normal_on_a); + body1.applyImpulse(impulse_vector, rel_pos1); + + tmp.negate(impulse_vector); + body2.applyImpulse(tmp, rel_pos2); + return normalImpulse; + } + finally { + stack.vectors.pop(); + } + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/TypedConstraint.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/TypedConstraint.java new file mode 100644 index 0000000..e68d0a0 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/TypedConstraint.java @@ -0,0 +1,101 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javabullet.dynamics.RigidBody; + +/** + * TypedConstraint is the baseclass for Bullet constraints and vehicles. + * + * @author jezek2 + */ +public abstract class TypedConstraint { + + protected final BulletStack stack = BulletStack.get(); + + private static final RigidBody s_fixed = new RigidBody(0, null, null); + + private int userConstraintType = -1; + private int userConstraintId = -1; + + private TypedConstraintType constraintType; + + protected RigidBody rbA; + protected RigidBody rbB; + protected float appliedImpulse = 0f; + + public TypedConstraint(TypedConstraintType type) { + this(type, s_fixed, s_fixed); + } + + public TypedConstraint(TypedConstraintType type, RigidBody rbA) { + this(type, rbA, s_fixed); + } + + public TypedConstraint(TypedConstraintType type, RigidBody rbA, RigidBody rbB) { + this.constraintType = type; + this.rbA = rbA; + this.rbB = rbB; + s_fixed.setMassProps(0f, BulletGlobals.ZERO_VECTOR3); + } + + public abstract void buildJacobian(); + + public abstract void solveConstraint(float timeStep); + + public RigidBody getRigidBodyA() { + return rbA; + } + + public RigidBody getRigidBodyB() { + return rbB; + } + + public int getUserConstraintType() { + return userConstraintType; + } + + public void setUserConstraintType(int userConstraintType) { + this.userConstraintType = userConstraintType; + } + + public int getUserConstraintId() { + return userConstraintId; + } + + public void setUserConstraintId(int userConstraintId) { + this.userConstraintId = userConstraintId; + } + + public float getAppliedImpulse() { + return appliedImpulse; + } + + public TypedConstraintType getConstraintType() { + return constraintType; + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/TypedConstraintType.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/TypedConstraintType.java new file mode 100644 index 0000000..c2f08c0 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/TypedConstraintType.java @@ -0,0 +1,36 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.constraintsolver; + +/** + * + * @author jezek2 + */ +public enum TypedConstraintType { + POINT2POINT_CONSTRAINT_TYPE, + HINGE_CONSTRAINT_TYPE, + CONETWIST_CONSTRAINT_TYPE, + D6_CONSTRAINT_TYPE, + VEHICLE_CONSTRAINT_TYPE +} diff --git a/src/jbullet/src/javabullet/dynamics/constraintsolver/package-info.java b/src/jbullet/src/javabullet/dynamics/constraintsolver/package-info.java new file mode 100644 index 0000000..08aa9fd --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/constraintsolver/package-info.java @@ -0,0 +1,28 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +/** + * Constraints. + */ +package javabullet.dynamics.constraintsolver; + diff --git a/src/jbullet/src/javabullet/dynamics/package-info.java b/src/jbullet/src/javabullet/dynamics/package-info.java new file mode 100644 index 0000000..394cf74 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/package-info.java @@ -0,0 +1,28 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +/** + * DynamicsWorld and RigidBody. + */ +package javabullet.dynamics; + diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/DefaultVehicleRaycaster.java b/src/jbullet/src/javabullet/dynamics/vehicle/DefaultVehicleRaycaster.java new file mode 100644 index 0000000..c14949e --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/vehicle/DefaultVehicleRaycaster.java @@ -0,0 +1,63 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.vehicle; + +import javabullet.collision.dispatch.CollisionWorld.ClosestRayResultCallback; +import javabullet.dynamics.DynamicsWorld; +import javabullet.dynamics.RigidBody; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class DefaultVehicleRaycaster implements VehicleRaycaster { + + protected DynamicsWorld dynamicsWorld; + + public DefaultVehicleRaycaster(DynamicsWorld world) { + this.dynamicsWorld = world; + } + + public Object castRay(Vector3f from, Vector3f to, VehicleRaycasterResult result) { + //RayResultCallback& resultCallback; + + ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(from, to); + + dynamicsWorld.rayTest(from, to, rayCallback); + + if (rayCallback.hasHit()) { + RigidBody body = RigidBody.upcast(rayCallback.collisionObject); + if (body != null) { + result.hitPointInWorld.set(rayCallback.hitPointWorld); + result.hitNormalInWorld.set(rayCallback.hitNormalWorld); + result.hitNormalInWorld.normalize(); + result.distFraction = rayCallback.closestHitFraction; + return body; + } + } + return null; + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/RaycastVehicle.java b/src/jbullet/src/javabullet/dynamics/vehicle/RaycastVehicle.java new file mode 100644 index 0000000..c13817f --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/vehicle/RaycastVehicle.java @@ -0,0 +1,765 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.vehicle; + +import java.util.ArrayList; +import java.util.List; +import javabullet.dynamics.RigidBody; +import javabullet.dynamics.constraintsolver.ContactConstraint; +import javabullet.dynamics.constraintsolver.TypedConstraint; +import javabullet.dynamics.constraintsolver.TypedConstraintType; +import javabullet.linearmath.MatrixUtil; +import javabullet.linearmath.MiscUtil; +import javabullet.linearmath.QuaternionUtil; +import javabullet.linearmath.Transform; +import javabullet.util.FloatArrayList; +import javax.vecmath.Matrix3f; +import javax.vecmath.Quat4f; +import javax.vecmath.Vector3f; + +/** + * Raycast vehicle, very special constraint that turn a rigidbody into a vehicle. + * + * @author jezek2 + */ +public class RaycastVehicle extends TypedConstraint { + + private static RigidBody s_fixedObject = new RigidBody(0, null, null); + private static final float sideFrictionStiffness2 = 1.0f; + + protected List<Vector3f> forwardWS = new ArrayList<Vector3f>(); + protected List<Vector3f> axle = new ArrayList<Vector3f>(); + protected FloatArrayList forwardImpulse = new FloatArrayList(); + protected FloatArrayList sideImpulse = new FloatArrayList(); + + private float tau; + private float damping; + private VehicleRaycaster vehicleRaycaster; + private float pitchControl = 0f; + private float steeringValue; + private float currentVehicleSpeedKmHour; + + private RigidBody chassisBody; + + private int indexRightAxis = 0; + private int indexUpAxis = 2; + private int indexForwardAxis = 1; + + public List<WheelInfo> wheelInfo = new ArrayList<WheelInfo>(); + + // constructor to create a car from an existing rigidbody + public RaycastVehicle(VehicleTuning tuning, RigidBody chassis, VehicleRaycaster raycaster) { + super(TypedConstraintType.VEHICLE_CONSTRAINT_TYPE); + this.vehicleRaycaster = raycaster; + this.chassisBody = chassis; + defaultInit(tuning); + } + + private void defaultInit(VehicleTuning tuning) { + currentVehicleSpeedKmHour = 0f; + steeringValue = 0f; + } + + /** + * Basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed. + */ + public WheelInfo addWheel(Vector3f connectionPointCS, Vector3f wheelDirectionCS0, Vector3f wheelAxleCS, float suspensionRestLength, float wheelRadius, VehicleTuning tuning, boolean isFrontWheel) { + WheelInfoConstructionInfo ci = new WheelInfoConstructionInfo(); + + ci.chassisConnectionCS.set(connectionPointCS); + ci.wheelDirectionCS.set(wheelDirectionCS0); + ci.wheelAxleCS.set(wheelAxleCS); + ci.suspensionRestLength = suspensionRestLength; + ci.wheelRadius = wheelRadius; + ci.suspensionStiffness = tuning.suspensionStiffness; + ci.wheelsDampingCompression = tuning.suspensionCompression; + ci.wheelsDampingRelaxation = tuning.suspensionDamping; + ci.frictionSlip = tuning.frictionSlip; + ci.bIsFrontWheel = isFrontWheel; + ci.maxSuspensionTravelCm = tuning.maxSuspensionTravelCm; + + wheelInfo.add(new WheelInfo(ci)); + + WheelInfo wheel = wheelInfo.get(getNumWheels() - 1); + + updateWheelTransformsWS(wheel, false); + updateWheelTransform(getNumWheels() - 1, false); + return wheel; + } + + public Transform getWheelTransformWS(int wheelIndex) { + assert (wheelIndex < getNumWheels()); + WheelInfo wheel = wheelInfo.get(wheelIndex); + return wheel.worldTransform; + } + + public void updateWheelTransform(int wheelIndex) { + updateWheelTransform(wheelIndex, true); + } + + public void updateWheelTransform(int wheelIndex, boolean interpolatedTransform) { + stack.vectors.push(); + stack.quats.push(); + stack.matrices.push(); + try { + WheelInfo wheel = wheelInfo.get(wheelIndex); + updateWheelTransformsWS(wheel, interpolatedTransform); + Vector3f up = stack.vectors.get(); + up.negate(wheel.raycastInfo.wheelDirectionWS); + Vector3f right = wheel.raycastInfo.wheelAxleWS; + Vector3f fwd = stack.vectors.get(); + fwd.cross(up, right); + fwd.normalize(); + // up = right.cross(fwd); + // up.normalize(); + + // rotate around steering over de wheelAxleWS + float steering = wheel.steering; + + Quat4f steeringOrn = stack.quats.get(); + QuaternionUtil.setRotation(steeringOrn, up, steering); //wheel.m_steering); + Matrix3f steeringMat = stack.matrices.get(); + MatrixUtil.setRotation(steeringMat, steeringOrn); + + Quat4f rotatingOrn = stack.quats.get(); + QuaternionUtil.setRotation(rotatingOrn, right, -wheel.rotation); + Matrix3f rotatingMat = stack.matrices.get(); + MatrixUtil.setRotation(rotatingMat, rotatingOrn); + + Matrix3f basis2 = stack.matrices.get(); + basis2.setRow(0, right.x, fwd.x, up.x); + basis2.setRow(1, right.y, fwd.y, up.y); + basis2.setRow(2, right.z, fwd.z, up.z); + + Matrix3f wheelBasis = wheel.worldTransform.basis; + wheelBasis.mul(steeringMat, rotatingMat); + wheelBasis.mul(basis2); + + wheel.worldTransform.origin.scaleAdd(wheel.raycastInfo.suspensionLength, wheel.raycastInfo.wheelDirectionWS, wheel.raycastInfo.hardPointWS); + } + finally { + stack.vectors.pop(); + stack.quats.pop(); + stack.matrices.pop(); + } + } + + public void resetSuspension() { + int i; + for (i = 0; i < wheelInfo.size(); i++) { + WheelInfo wheel = wheelInfo.get(i); + wheel.raycastInfo.suspensionLength = wheel.getSuspensionRestLength(); + wheel.suspensionRelativeVelocity = 0f; + + wheel.raycastInfo.contactNormalWS.negate(wheel.raycastInfo.wheelDirectionWS); + //wheel_info.setContactFriction(btScalar(0.0)); + wheel.clippedInvContactDotSuspension = 1f; + } + } + + public void updateWheelTransformsWS(WheelInfo wheel) { + updateWheelTransformsWS(wheel, true); + } + + public void updateWheelTransformsWS(WheelInfo wheel, boolean interpolatedTransform) { + stack.transforms.push(); + try { + wheel.raycastInfo.isInContact = false; + + Transform chassisTrans = stack.transforms.get(getChassisWorldTransform()); + if (interpolatedTransform && (getRigidBody().getMotionState() != null)) { + getRigidBody().getMotionState().getWorldTransform(chassisTrans); + } + + wheel.raycastInfo.hardPointWS.set(wheel.chassisConnectionPointCS); + chassisTrans.transform(wheel.raycastInfo.hardPointWS); + + wheel.raycastInfo.wheelDirectionWS.set(wheel.wheelDirectionCS); + chassisTrans.basis.transform(wheel.raycastInfo.wheelDirectionWS); + + wheel.raycastInfo.wheelAxleWS.set(wheel.wheelAxleCS); + chassisTrans.basis.transform(wheel.raycastInfo.wheelAxleWS); + } + finally { + stack.transforms.pop(); + } + } + + public float rayCast(WheelInfo wheel) { + stack.vectors.push(); + try { + updateWheelTransformsWS(wheel, false); + + float depth = -1f; + + float raylen = wheel.getSuspensionRestLength() + wheel.wheelsRadius; + + Vector3f rayvector = stack.vectors.get(); + rayvector.scale(raylen, wheel.raycastInfo.wheelDirectionWS); + Vector3f source = wheel.raycastInfo.hardPointWS; + wheel.raycastInfo.contactPointWS.add(source, rayvector); + Vector3f target = wheel.raycastInfo.contactPointWS; + + float param = 0f; + + VehicleRaycasterResult rayResults = new VehicleRaycasterResult(); + + assert (vehicleRaycaster != null); + + Object object = vehicleRaycaster.castRay(source, target, rayResults); + + wheel.raycastInfo.groundObject = null; + + if (object != null) { + param = rayResults.distFraction; + depth = raylen * rayResults.distFraction; + wheel.raycastInfo.contactNormalWS.set(rayResults.hitNormalInWorld); + wheel.raycastInfo.isInContact = true; + + wheel.raycastInfo.groundObject = s_fixedObject; // todo for driving on dynamic/movable objects!; + //wheel.m_raycastInfo.m_groundObject = object; + + float hitDistance = param * raylen; + wheel.raycastInfo.suspensionLength = hitDistance - wheel.wheelsRadius; + // clamp on max suspension travel + + float minSuspensionLength = wheel.getSuspensionRestLength() - wheel.maxSuspensionTravelCm * 0.01f; + float maxSuspensionLength = wheel.getSuspensionRestLength() + wheel.maxSuspensionTravelCm * 0.01f; + if (wheel.raycastInfo.suspensionLength < minSuspensionLength) { + wheel.raycastInfo.suspensionLength = minSuspensionLength; + } + if (wheel.raycastInfo.suspensionLength > maxSuspensionLength) { + wheel.raycastInfo.suspensionLength = maxSuspensionLength; + } + + wheel.raycastInfo.contactPointWS.set(rayResults.hitPointInWorld); + + float denominator = wheel.raycastInfo.contactNormalWS.dot(wheel.raycastInfo.wheelDirectionWS); + + Vector3f chassis_velocity_at_contactPoint = stack.vectors.get(); + Vector3f relpos = stack.vectors.get(); + relpos.sub(wheel.raycastInfo.contactPointWS, getRigidBody().getCenterOfMassPosition()); + + chassis_velocity_at_contactPoint.set(getRigidBody().getVelocityInLocalPoint(relpos)); + + float projVel = wheel.raycastInfo.contactNormalWS.dot(chassis_velocity_at_contactPoint); + + if (denominator >= -0.1f) { + wheel.suspensionRelativeVelocity = 0f; + wheel.clippedInvContactDotSuspension = 1f / 0.1f; + } + else { + float inv = -1f / denominator; + wheel.suspensionRelativeVelocity = projVel * inv; + wheel.clippedInvContactDotSuspension = inv; + } + + } + else { + // put wheel info as in rest position + wheel.raycastInfo.suspensionLength = wheel.getSuspensionRestLength(); + wheel.suspensionRelativeVelocity = 0f; + wheel.raycastInfo.contactNormalWS.negate(wheel.raycastInfo.wheelDirectionWS); + wheel.clippedInvContactDotSuspension = 1f; + } + + return depth; + } + finally { + stack.vectors.pop(); + } + } + + public Transform getChassisWorldTransform() { + /* + if (getRigidBody()->getMotionState()) + { + btTransform chassisWorldTrans; + getRigidBody()->getMotionState()->getWorldTransform(chassisWorldTrans); + return chassisWorldTrans; + } + */ + + return getRigidBody().getCenterOfMassTransform(); + } + + public void updateVehicle(float step) { + stack.vectors.push(); + stack.transforms.push(); + try { + for (int i = 0; i < getNumWheels(); i++) { + updateWheelTransform(i, false); + } + + currentVehicleSpeedKmHour = 3.6f * getRigidBody().getLinearVelocity().length(); + + Transform chassisTrans = stack.transforms.get(getChassisWorldTransform()); + + Vector3f forwardW = stack.vectors.get( + chassisTrans.basis.getElement(0, indexForwardAxis), + chassisTrans.basis.getElement(1, indexForwardAxis), + chassisTrans.basis.getElement(2, indexForwardAxis)); + + if (forwardW.dot(getRigidBody().getLinearVelocity()) < 0f) { + currentVehicleSpeedKmHour *= -1f; + } + + // + // simulate suspension + // + + int i = 0; + for (i = 0; i < wheelInfo.size(); i++) { + float depth; + depth = rayCast(wheelInfo.get(i)); + } + + updateSuspension(step); + + for (i = 0; i < wheelInfo.size(); i++) { + // apply suspension force + WheelInfo wheel = wheelInfo.get(i); + + float suspensionForce = wheel.wheelsSuspensionForce; + + float gMaxSuspensionForce = 6000f; + if (suspensionForce > gMaxSuspensionForce) { + suspensionForce = gMaxSuspensionForce; + } + Vector3f impulse = stack.vectors.get(); + impulse.scale(suspensionForce * step, wheel.raycastInfo.contactNormalWS); + Vector3f relpos = stack.vectors.get(); + relpos.sub(wheel.raycastInfo.contactPointWS, getRigidBody().getCenterOfMassPosition()); + + getRigidBody().applyImpulse(impulse, relpos); + } + + updateFriction(step); + + for (i = 0; i < wheelInfo.size(); i++) { + WheelInfo wheel = wheelInfo.get(i); + Vector3f relpos = stack.vectors.get(); + relpos.sub(wheel.raycastInfo.hardPointWS, getRigidBody().getCenterOfMassPosition()); + Vector3f vel = stack.vectors.get(getRigidBody().getVelocityInLocalPoint(relpos)); + + if (wheel.raycastInfo.isInContact) { + Transform chassisWorldTransform = stack.transforms.get(getChassisWorldTransform()); + + Vector3f fwd = stack.vectors.get( + chassisWorldTransform.basis.getElement(0, indexForwardAxis), + chassisWorldTransform.basis.getElement(1, indexForwardAxis), + chassisWorldTransform.basis.getElement(2, indexForwardAxis)); + + float proj = fwd.dot(wheel.raycastInfo.contactNormalWS); + Vector3f tmp = stack.vectors.get(); + tmp.scale(proj, wheel.raycastInfo.contactNormalWS); + fwd.sub(tmp); + + float proj2 = fwd.dot(vel); + + wheel.deltaRotation = (proj2 * step) / (wheel.wheelsRadius); + wheel.rotation += wheel.deltaRotation; + + } + else { + wheel.rotation += wheel.deltaRotation; + } + + wheel.deltaRotation *= 0.99f; // damping of rotation when not in contact + } + } + finally { + stack.vectors.pop(); + stack.transforms.pop(); + } + } + + public void setSteeringValue(float steering, int wheel) { + assert (wheel >= 0 && wheel < getNumWheels()); + + WheelInfo wheel_info = getWheelInfo(wheel); + wheel_info.steering = steering; + } + + public float getSteeringValue(int wheel) { + return getWheelInfo(wheel).steering; + } + + public void applyEngineForce(float force, int wheel) { + assert (wheel >= 0 && wheel < getNumWheels()); + WheelInfo wheel_info = getWheelInfo(wheel); + wheel_info.engineForce = force; + } + + public WheelInfo getWheelInfo(int index) { + assert ((index >= 0) && (index < getNumWheels())); + + return wheelInfo.get(index); + } + + public void setBrake(float brake, int wheelIndex) { + assert ((wheelIndex >= 0) && (wheelIndex < getNumWheels())); + getWheelInfo(wheelIndex).brake = brake; + } + + public void updateSuspension(float deltaTime) { + float chassisMass = 1f / chassisBody.getInvMass(); + + for (int w_it = 0; w_it < getNumWheels(); w_it++) { + WheelInfo wheel_info = wheelInfo.get(w_it); + + if (wheel_info.raycastInfo.isInContact) { + float force; + // Spring + { + float susp_length = wheel_info.getSuspensionRestLength(); + float current_length = wheel_info.raycastInfo.suspensionLength; + + float length_diff = (susp_length - current_length); + + force = wheel_info.suspensionStiffness * length_diff * wheel_info.clippedInvContactDotSuspension; + } + + // Damper + { + float projected_rel_vel = wheel_info.suspensionRelativeVelocity; + { + float susp_damping; + if (projected_rel_vel < 0f) { + susp_damping = wheel_info.wheelsDampingCompression; + } + else { + susp_damping = wheel_info.wheelsDampingRelaxation; + } + force -= susp_damping * projected_rel_vel; + } + } + + // RESULT + wheel_info.wheelsSuspensionForce = force * chassisMass; + if (wheel_info.wheelsSuspensionForce < 0f) { + wheel_info.wheelsSuspensionForce = 0f; + } + } + else { + wheel_info.wheelsSuspensionForce = 0f; + } + } + } + + private float calcRollingFriction(WheelContactPoint contactPoint) { + stack.vectors.push(); + try { + float j1 = 0f; + + Vector3f contactPosWorld = contactPoint.frictionPositionWorld; + + Vector3f rel_pos1 = stack.vectors.get(); + rel_pos1.sub(contactPosWorld, contactPoint.body0.getCenterOfMassPosition()); + Vector3f rel_pos2 = stack.vectors.get(); + rel_pos2.sub(contactPosWorld, contactPoint.body1.getCenterOfMassPosition()); + + float maxImpulse = contactPoint.maxImpulse; + + Vector3f vel1 = stack.vectors.get(contactPoint.body0.getVelocityInLocalPoint(rel_pos1)); + Vector3f vel2 = stack.vectors.get(contactPoint.body1.getVelocityInLocalPoint(rel_pos2)); + Vector3f vel = stack.vectors.get(); + vel.sub(vel1, vel2); + + float vrel = contactPoint.frictionDirectionWorld.dot(vel); + + // calculate j that moves us to zero relative velocity + j1 = -vrel * contactPoint.jacDiagABInv; + j1 = Math.min(j1, maxImpulse); + j1 = Math.max(j1, -maxImpulse); + + return j1; + } + finally { + stack.vectors.pop(); + } + } + + public void updateFriction(float timeStep) { + stack.vectors.push(); + stack.matrices.push(); + try { + // calculate the impulse, so that the wheels don't move sidewards + int numWheel = getNumWheels(); + if (numWheel == 0) { + return; + } + + MiscUtil.resize(forwardWS, numWheel, Vector3f.class); + MiscUtil.resize(axle, numWheel, Vector3f.class); + MiscUtil.resize(forwardImpulse, numWheel, 0f); + MiscUtil.resize(sideImpulse, numWheel, 0f); + + Vector3f tmp = stack.vectors.get(); + + int numWheelsOnGround = 0; + + // collapse all those loops into one! + for (int i = 0; i < getNumWheels(); i++) { + WheelInfo wheel_info = wheelInfo.get(i); + RigidBody groundObject = (RigidBody) wheel_info.raycastInfo.groundObject; + if (groundObject != null) { + numWheelsOnGround++; + } + sideImpulse.set(i, 0f); + forwardImpulse.set(i, 0f); + } + + { + for (int i = 0; i < getNumWheels(); i++) { + + WheelInfo wheel_info = wheelInfo.get(i); + + RigidBody groundObject = (RigidBody) wheel_info.raycastInfo.groundObject; + + if (groundObject != null) { + Transform wheelTrans = getWheelTransformWS(i); + + Matrix3f wheelBasis0 = stack.matrices.get(wheelTrans.basis); + axle.get(i).set( + wheelBasis0.getElement(0, indexRightAxis), + wheelBasis0.getElement(1, indexRightAxis), + wheelBasis0.getElement(2, indexRightAxis)); + + Vector3f surfNormalWS = wheel_info.raycastInfo.contactNormalWS; + float proj = axle.get(i).dot(surfNormalWS); + tmp.scale(proj, surfNormalWS); + axle.get(i).sub(tmp); + axle.get(i).normalize(); + + forwardWS.get(i).cross(surfNormalWS, axle.get(i)); + forwardWS.get(i).normalize(); + + float[] floatPtr = stack.floatArrays.getFixed(1); + ContactConstraint.resolveSingleBilateral(chassisBody, wheel_info.raycastInfo.contactPointWS, + groundObject, wheel_info.raycastInfo.contactPointWS, + 0f, axle.get(i), floatPtr, timeStep); + sideImpulse.set(i, floatPtr[0]); + stack.floatArrays.release(floatPtr); + + sideImpulse.set(i, sideImpulse.get(i) * sideFrictionStiffness2); + } + } + } + + float sideFactor = 1f; + float fwdFactor = 0.5f; + + boolean sliding = false; + { + for (int wheel = 0; wheel < getNumWheels(); wheel++) { + WheelInfo wheel_info = wheelInfo.get(wheel); + RigidBody groundObject = (RigidBody) wheel_info.raycastInfo.groundObject; + + float rollingFriction = 0f; + + if (groundObject != null) { + if (wheel_info.engineForce != 0f) { + rollingFriction = wheel_info.engineForce * timeStep; + } + else { + float defaultRollingFrictionImpulse = 0f; + float maxImpulse = wheel_info.brake != 0f ? wheel_info.brake : defaultRollingFrictionImpulse; + WheelContactPoint contactPt = new WheelContactPoint(chassisBody, groundObject, wheel_info.raycastInfo.contactPointWS, forwardWS.get(wheel), maxImpulse); + rollingFriction = calcRollingFriction(contactPt); + } + } + + // switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break) + + forwardImpulse.set(wheel, 0f); + wheelInfo.get(wheel).skidInfo = 1f; + + if (groundObject != null) { + wheelInfo.get(wheel).skidInfo = 1f; + + float maximp = wheel_info.wheelsSuspensionForce * timeStep * wheel_info.frictionSlip; + float maximpSide = maximp; + + float maximpSquared = maximp * maximpSide; + + forwardImpulse.set(wheel, rollingFriction); //wheelInfo.m_engineForce* timeStep; + + float x = (forwardImpulse.get(wheel)) * fwdFactor; + float y = (sideImpulse.get(wheel)) * sideFactor; + + float impulseSquared = (x * x + y * y); + + if (impulseSquared > maximpSquared) { + sliding = true; + + float factor = maximp / (float) Math.sqrt(impulseSquared); + + wheelInfo.get(wheel).skidInfo *= factor; + } + } + + } + } + + if (sliding) { + for (int wheel = 0; wheel < getNumWheels(); wheel++) { + if (sideImpulse.get(wheel) != 0f) { + if (wheelInfo.get(wheel).skidInfo < 1f) { + forwardImpulse.set(wheel, forwardImpulse.get(wheel) * wheelInfo.get(wheel).skidInfo); + sideImpulse.set(wheel, sideImpulse.get(wheel) * wheelInfo.get(wheel).skidInfo); + } + } + } + } + + // apply the impulses + { + for (int wheel = 0; wheel < getNumWheels(); wheel++) { + WheelInfo wheel_info = wheelInfo.get(wheel); + + Vector3f rel_pos = stack.vectors.get(); + rel_pos.sub(wheel_info.raycastInfo.contactPointWS, chassisBody.getCenterOfMassPosition()); + + if (forwardImpulse.get(wheel) != 0f) { + tmp.scale(forwardImpulse.get(wheel), forwardWS.get(wheel)); + chassisBody.applyImpulse(tmp, rel_pos); + } + if (sideImpulse.get(wheel) != 0f) { + RigidBody groundObject = (RigidBody) wheelInfo.get(wheel).raycastInfo.groundObject; + + Vector3f rel_pos2 = stack.vectors.get(); + rel_pos2.sub(wheel_info.raycastInfo.contactPointWS, groundObject.getCenterOfMassPosition()); + + Vector3f sideImp = stack.vectors.get(); + sideImp.scale(sideImpulse.get(wheel), axle.get(wheel)); + + rel_pos.z *= wheel_info.rollInfluence; + chassisBody.applyImpulse(sideImp, rel_pos); + + // apply friction impulse on the ground + tmp.negate(sideImp); + groundObject.applyImpulse(tmp, rel_pos2); + } + } + } + } + finally { + stack.vectors.pop(); + stack.matrices.pop(); + } + } + + @Override + public void buildJacobian() { + // not yet + } + + @Override + public void solveConstraint(float timeStep) { + // not yet + } + + public int getNumWheels() { + return wheelInfo.size(); + } + + public void setPitchControl(float pitch) { + this.pitchControl = pitch; + } + + public RigidBody getRigidBody() { + return chassisBody; + } + + public int getRightAxis() { + return indexRightAxis; + } + + public int getUpAxis() { + return indexUpAxis; + } + + public int getForwardAxis() { + return indexForwardAxis; + } + + /** + * Worldspace forward vector. + */ + public Vector3f getForwardVector() { + stack.vectors.push(); + try { + Transform chassisTrans = getChassisWorldTransform(); + + Vector3f forwardW = stack.vectors.get( + chassisTrans.basis.getElement(0, indexForwardAxis), + chassisTrans.basis.getElement(1, indexForwardAxis), + chassisTrans.basis.getElement(2, indexForwardAxis)); + + return stack.vectors.returning(forwardW); + } + finally { + stack.vectors.pop(); + } + } + + /** + * Velocity of vehicle (positive if velocity vector has same direction as foward vector). + */ + public float getCurrentSpeedKmHour() { + return currentVehicleSpeedKmHour; + } + + public void setCoordinateSystem(int rightIndex, int upIndex, int forwardIndex) { + this.indexRightAxis = rightIndex; + this.indexUpAxis = upIndex; + this.indexForwardAxis = forwardIndex; + } + + //////////////////////////////////////////////////////////////////////////// + + private static class WheelContactPoint { + public RigidBody body0; + public RigidBody body1; + public final Vector3f frictionPositionWorld = new Vector3f(); + public final Vector3f frictionDirectionWorld = new Vector3f(); + public float jacDiagABInv; + public float maxImpulse; + + public WheelContactPoint(RigidBody body0, RigidBody body1, Vector3f frictionPosWorld, Vector3f frictionDirectionWorld, float maxImpulse) { + this.body0 = body0; + this.body1 = body1; + this.frictionPositionWorld.set(frictionPosWorld); + this.frictionDirectionWorld.set(frictionDirectionWorld); + this.maxImpulse = maxImpulse; + + float denom0 = body0.computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld); + float denom1 = body1.computeImpulseDenominator(frictionPosWorld, frictionDirectionWorld); + float relaxation = 1f; + jacDiagABInv = relaxation / (denom0 + denom1); + } + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/VehicleRaycaster.java b/src/jbullet/src/javabullet/dynamics/vehicle/VehicleRaycaster.java new file mode 100644 index 0000000..5fff289 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/vehicle/VehicleRaycaster.java @@ -0,0 +1,37 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.vehicle; + +import javax.vecmath.Vector3f; + +/** + * VehicleRaycaster is provides interface for between vehicle simulation and raycasting. + * + * @author jezek2 + */ +public interface VehicleRaycaster { + + public Object castRay(Vector3f from, Vector3f to, VehicleRaycasterResult result); + +} diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/VehicleRaycasterResult.java b/src/jbullet/src/javabullet/dynamics/vehicle/VehicleRaycasterResult.java new file mode 100644 index 0000000..b948341 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/vehicle/VehicleRaycasterResult.java @@ -0,0 +1,38 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.vehicle; + +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class VehicleRaycasterResult { + + public final Vector3f hitPointInWorld = new Vector3f(); + public final Vector3f hitNormalInWorld = new Vector3f(); + public float distFraction = -1f; + +} diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/VehicleTuning.java b/src/jbullet/src/javabullet/dynamics/vehicle/VehicleTuning.java new file mode 100644 index 0000000..47a6f29 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/vehicle/VehicleTuning.java @@ -0,0 +1,38 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.vehicle; + +/** + * + * @author jezek2 + */ +public class VehicleTuning { + + public float suspensionStiffness = 5.88f; + public float suspensionCompression = 0.83f; + public float suspensionDamping = 0.88f; + public float maxSuspensionTravelCm = 500f; + public float frictionSlip = 10.5f; + +} diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/WheelInfo.java b/src/jbullet/src/javabullet/dynamics/vehicle/WheelInfo.java new file mode 100644 index 0000000..d8f9643 --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/vehicle/WheelInfo.java @@ -0,0 +1,145 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.vehicle; + +import javabullet.BulletStack; +import javabullet.dynamics.RigidBody; +import javabullet.linearmath.Transform; +import javax.vecmath.Vector3f; + +/** + * WheelInfo contains information per wheel about friction and suspension. + * + * @author jezek2 + */ +public class WheelInfo { + + protected final BulletStack stack = BulletStack.get(); + + public final RaycastInfo raycastInfo = new RaycastInfo(); + + public final Transform worldTransform = new Transform(); + + public final Vector3f chassisConnectionPointCS = new Vector3f(); // const + public final Vector3f wheelDirectionCS = new Vector3f(); // const + public final Vector3f wheelAxleCS = new Vector3f(); // const or modified by steering + public float suspensionRestLength1; // const + public float maxSuspensionTravelCm; + public float wheelsRadius; // const + public float suspensionStiffness; // const + public float wheelsDampingCompression; // const + public float wheelsDampingRelaxation; // const + public float frictionSlip; + public float steering; + public float rotation; + public float deltaRotation; + public float rollInfluence; + + public float engineForce; + + public float brake; + + public boolean bIsFrontWheel; + + public Object clientInfo; // can be used to store pointer to sync transforms... + + public float clippedInvContactDotSuspension; + public float suspensionRelativeVelocity; + // calculated by suspension + public float wheelsSuspensionForce; + public float skidInfo; + + public WheelInfo(WheelInfoConstructionInfo ci) { + suspensionRestLength1 = ci.suspensionRestLength; + maxSuspensionTravelCm = ci.maxSuspensionTravelCm; + + wheelsRadius = ci.wheelRadius; + suspensionStiffness = ci.suspensionStiffness; + wheelsDampingCompression = ci.wheelsDampingCompression; + wheelsDampingRelaxation = ci.wheelsDampingRelaxation; + chassisConnectionPointCS.set(ci.chassisConnectionCS); + wheelDirectionCS.set(ci.wheelDirectionCS); + wheelAxleCS.set(ci.wheelAxleCS); + frictionSlip = ci.frictionSlip; + steering = 0f; + engineForce = 0f; + rotation = 0f; + deltaRotation = 0f; + brake = 0f; + rollInfluence = 0.1f; + bIsFrontWheel = ci.bIsFrontWheel; + } + + public float getSuspensionRestLength() { + return suspensionRestLength1; + } + + public void updateWheel(RigidBody chassis, RaycastInfo raycastInfo) { + stack.vectors.push(); + try { + if (raycastInfo.isInContact) { + float project = raycastInfo.contactNormalWS.dot(raycastInfo.wheelDirectionWS); + Vector3f chassis_velocity_at_contactPoint = stack.vectors.get(); + Vector3f relpos = stack.vectors.get(); + relpos.sub(raycastInfo.contactPointWS, chassis.getCenterOfMassPosition()); + chassis_velocity_at_contactPoint.set(chassis.getVelocityInLocalPoint(relpos)); + float projVel = raycastInfo.contactNormalWS.dot(chassis_velocity_at_contactPoint); + if (project >= -0.1f) { + suspensionRelativeVelocity = 0f; + clippedInvContactDotSuspension = 1f / 0.1f; + } + else { + float inv = -1f / project; + suspensionRelativeVelocity = projVel * inv; + clippedInvContactDotSuspension = inv; + } + } + else { + // Not in contact : position wheel in a nice (rest length) position + raycastInfo.suspensionLength = getSuspensionRestLength(); + suspensionRelativeVelocity = 0f; + raycastInfo.contactNormalWS.negate(raycastInfo.wheelDirectionWS); + clippedInvContactDotSuspension = 1f; + } + } + finally { + stack.vectors.pop(); + } + } + + //////////////////////////////////////////////////////////////////////////// + + public static class RaycastInfo { + // set by raycaster + public final Vector3f contactNormalWS = new Vector3f(); // contactnormal + public final Vector3f contactPointWS = new Vector3f(); // raycast hitpoint + public float suspensionLength; + public final Vector3f hardPointWS = new Vector3f(); // raycast starting point + public final Vector3f wheelDirectionWS = new Vector3f(); // direction in worldspace + public final Vector3f wheelAxleWS = new Vector3f(); // axle in worldspace + public boolean isInContact; + public Object groundObject; // could be general void* ptr + } + +} diff --git a/src/jbullet/src/javabullet/dynamics/vehicle/WheelInfoConstructionInfo.java b/src/jbullet/src/javabullet/dynamics/vehicle/WheelInfoConstructionInfo.java new file mode 100644 index 0000000..2824bdb --- /dev/null +++ b/src/jbullet/src/javabullet/dynamics/vehicle/WheelInfoConstructionInfo.java @@ -0,0 +1,47 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.dynamics.vehicle; + +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class WheelInfoConstructionInfo { + + public final Vector3f chassisConnectionCS = new Vector3f(); + public final Vector3f wheelDirectionCS = new Vector3f(); + public final Vector3f wheelAxleCS = new Vector3f(); + public float suspensionRestLength; + public float maxSuspensionTravelCm; + public float wheelRadius; + + public float suspensionStiffness; + public float wheelsDampingCompression; + public float wheelsDampingRelaxation; + public float frictionSlip; + public boolean bIsFrontWheel; + +} diff --git a/src/jbullet/src/javabullet/linearmath/AabbUtil2.java b/src/jbullet/src/javabullet/linearmath/AabbUtil2.java new file mode 100644 index 0000000..978dcdf --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/AabbUtil2.java @@ -0,0 +1,139 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +import javabullet.BulletStack; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class AabbUtil2 { + + public static int outcode(Vector3f p, Vector3f halfExtent) { + return (p.x < -halfExtent.x ? 0x01 : 0x0) | + (p.x > halfExtent.x ? 0x08 : 0x0) | + (p.y < -halfExtent.y ? 0x02 : 0x0) | + (p.y > halfExtent.y ? 0x10 : 0x0) | + (p.z < -halfExtent.z ? 0x4 : 0x0) | + (p.z > halfExtent.z ? 0x20 : 0x0); + } + + public static boolean rayAabb(Vector3f rayFrom, Vector3f rayTo, Vector3f aabbMin, Vector3f aabbMax, float[] param, Vector3f normal) { + BulletStack stack = BulletStack.get(); + + stack.vectors.push(); + try { + Vector3f aabbHalfExtent = stack.vectors.get(); + Vector3f aabbCenter = stack.vectors.get(); + Vector3f source = stack.vectors.get(); + Vector3f target = stack.vectors.get(); + Vector3f r = stack.vectors.get(); + Vector3f hitNormal = stack.vectors.get(); + + aabbHalfExtent.sub(aabbMax, aabbMin); + aabbHalfExtent.scale(0.5f); + + aabbCenter.add(aabbMax, aabbMin); + aabbCenter.scale(0.5f); + + source.sub(rayFrom, aabbCenter); + target.sub(rayTo, aabbCenter); + + int sourceOutcode = outcode(source, aabbHalfExtent); + int targetOutcode = outcode(target, aabbHalfExtent); + if ((sourceOutcode & targetOutcode) == 0x0) { + float lambda_enter = 0f; + float lambda_exit = param[0]; + r.sub(target, source); + + float normSign = 1f; + hitNormal.set(0f, 0f, 0f); + int bit = 1; + + for (int j = 0; j < 2; j++) { + for (int i = 0; i != 3; ++i) { + if ((sourceOutcode & bit) != 0) { + float lambda = (-VectorUtil.getCoord(source, i) - VectorUtil.getCoord(aabbHalfExtent, i) * normSign) / VectorUtil.getCoord(r, i); + if (lambda_enter <= lambda) { + lambda_enter = lambda; + hitNormal.set(0f, 0f, 0f); + VectorUtil.setCoord(hitNormal, i, normSign); + } + } + else if ((targetOutcode & bit) != 0) { + float lambda = (-VectorUtil.getCoord(source, i) - VectorUtil.getCoord(aabbHalfExtent, i) * normSign) / VectorUtil.getCoord(r, i); + //btSetMin(lambda_exit, lambda); + lambda_exit = Math.min(lambda_exit, lambda); + } + bit <<= 1; + } + normSign = -1f; + } + if (lambda_enter <= lambda_exit) { + param[0] = lambda_enter; + normal.set(hitNormal); + return true; + } + } + return false; + } + finally { + stack.vectors.pop(); + } + } + + /** + * Conservative test for overlap between two aabbs. + */ + public static boolean testAabbAgainstAabb2(Vector3f aabbMin1, Vector3f aabbMax1, Vector3f aabbMin2, Vector3f aabbMax2) { + boolean overlap = true; + overlap = (aabbMin1.x > aabbMax2.x || aabbMax1.x < aabbMin2.x) ? false : overlap; + overlap = (aabbMin1.z > aabbMax2.z || aabbMax1.z < aabbMin2.z) ? false : overlap; + overlap = (aabbMin1.y > aabbMax2.y || aabbMax1.y < aabbMin2.y) ? false : overlap; + return overlap; + } + + /** + * Conservative test for overlap between triangle and aabb. + */ + public static boolean testTriangleAgainstAabb2(Vector3f[] vertices, Vector3f aabbMin, Vector3f aabbMax) { + Vector3f p1 = vertices[0]; + Vector3f p2 = vertices[1]; + Vector3f p3 = vertices[2]; + + if (Math.min(Math.min(p1.x, p2.x), p3.x) > aabbMax.x) return false; + if (Math.max(Math.max(p1.x, p2.x), p3.x) < aabbMin.x) return false; + + if (Math.min(Math.min(p1.z, p2.z), p3.z) > aabbMax.z) return false; + if (Math.max(Math.max(p1.z, p2.z), p3.z) < aabbMin.z) return false; + + if (Math.min(Math.min(p1.y, p2.y), p3.y) > aabbMax.y) return false; + if (Math.max(Math.max(p1.y, p2.y), p3.y) < aabbMin.y) return false; + + return true; + } + +} diff --git a/src/jbullet/src/javabullet/linearmath/Clock.java b/src/jbullet/src/javabullet/linearmath/Clock.java new file mode 100644 index 0000000..14207bb --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/Clock.java @@ -0,0 +1,56 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +/** + * + * @author jezek2 + */ +public class Clock { + + private long mStartTime; + + public Clock() { + reset(); + } + + public void reset() { + mStartTime = System.currentTimeMillis(); + } + + /** + * Returns the time in ms since the last call to reset or since the Clock was created. + */ + public long getTimeMilliseconds() { + return System.currentTimeMillis() - mStartTime; + } + + /** + * Returns the time in us since the last call to reset or since the Clock was created. + */ + public long getTimeMicroseconds() { + return getTimeMilliseconds() * 1000; + } + +} diff --git a/src/jbullet/src/javabullet/linearmath/DebugDrawModes.java b/src/jbullet/src/javabullet/linearmath/DebugDrawModes.java new file mode 100644 index 0000000..e159a3d --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/DebugDrawModes.java @@ -0,0 +1,46 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +/** + * + * @author jezek2 + */ +public class DebugDrawModes { + + public static final int NO_DEBUG = 0; + public static final int DRAW_WIREFRAME = 1; + public static final int DRAW_AABB = 2; + public static final int DRAW_FEATURES_TEXT = 4; + public static final int DRAW_CONTACT_POINTS = 8; + public static final int NO_DEACTIVATION = 16; + public static final int NO_HELP_TEXT = 32; + public static final int DRAW_TEXT = 64; + public static final int PROFILE_TIMINGS = 128; + public static final int ENABLE_SAT_COMPARISON = 256; + public static final int DISABLE_BULLET_LCP = 512; + public static final int ENABLE_CCD = 1024; + public static final int MAX_DEBUG_DRAW_MODE = 1025; + +} diff --git a/src/jbullet/src/javabullet/linearmath/DefaultMotionState.java b/src/jbullet/src/javabullet/linearmath/DefaultMotionState.java new file mode 100644 index 0000000..e3bbe7e --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/DefaultMotionState.java @@ -0,0 +1,65 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +/** + * DefaultMotionState provides a common implementation to synchronize world transforms with offsets. + * + * @author jezek2 + */ +public class DefaultMotionState implements MotionState { + + public final Transform graphicsWorldTrans = new Transform(); + public final Transform centerOfMassOffset = new Transform(); + public final Transform startWorldTrans = new Transform(); + + public DefaultMotionState() { + graphicsWorldTrans.setIdentity(); + centerOfMassOffset.setIdentity(); + startWorldTrans.setIdentity(); + } + + public DefaultMotionState(Transform startTrans) { + this.graphicsWorldTrans.set(startTrans); + centerOfMassOffset.setIdentity(); + this.startWorldTrans.set(startTrans); + } + + public DefaultMotionState(Transform startTrans, Transform centerOfMassOffset) { + this.graphicsWorldTrans.set(startTrans); + this.centerOfMassOffset.set(centerOfMassOffset); + this.startWorldTrans.set(startTrans); + } + + public void getWorldTransform(Transform centerOfMassWorldTrans) { + centerOfMassWorldTrans.inverse(centerOfMassOffset); + centerOfMassWorldTrans.mul(graphicsWorldTrans); + } + + public void setWorldTransform(Transform centerOfMassWorldTrans) { + graphicsWorldTrans.set(centerOfMassWorldTrans); + graphicsWorldTrans.mul(centerOfMassOffset); + } + +} diff --git a/src/jbullet/src/javabullet/linearmath/GeometryUtil.java b/src/jbullet/src/javabullet/linearmath/GeometryUtil.java new file mode 100644 index 0000000..354203c --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/GeometryUtil.java @@ -0,0 +1,185 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +import java.util.List; +import javabullet.BulletStack; +import javax.vecmath.Vector3f; +import javax.vecmath.Vector4f; + +/** + * + * @author jezek2 + */ +public class GeometryUtil { + + public static boolean isPointInsidePlanes(List<Vector4f> planeEquations, Vector3f point, float margin) { + int numbrushes = planeEquations.size(); + for (int i = 0; i < numbrushes; i++) { + Vector4f N1 = planeEquations.get(i); + float dist = VectorUtil.dot3(N1, point) + N1.w - margin; + if (dist > 0f) { + return false; + } + } + return true; + } + + public static boolean areVerticesBehindPlane(Vector4f planeNormal, List<Vector3f> vertices, float margin) { + int numvertices = vertices.size(); + for (int i = 0; i < numvertices; i++) { + Vector3f N1 = vertices.get(i); + float dist = VectorUtil.dot3(planeNormal, N1) + planeNormal.w - margin; + if (dist > 0f) { + return false; + } + } + return true; + } + + private static boolean notExist(Vector4f planeEquation, List<Vector4f> planeEquations) { + int numbrushes = planeEquations.size(); + for (int i = 0; i < numbrushes; i++) { + Vector4f N1 = planeEquations.get(i); + if (VectorUtil.dot3(planeEquation, N1) > 0.999f) { + return false; + } + } + return true; + } + + public static void getPlaneEquationsFromVertices(List<Vector3f> vertices, List<Vector4f> planeEquationsOut) { + BulletStack stack = BulletStack.get(); + + stack.vectors.push(); + stack.vectors4.push(); + try { + Vector4f planeEquation = stack.vectors4.get(); + Vector3f edge0 = stack.vectors.get(), edge1 = stack.vectors.get(); + Vector3f tmp = stack.vectors.get(); + + int numvertices = vertices.size(); + // brute force: + for (int i = 0; i < numvertices; i++) { + Vector3f N1 = vertices.get(i); + + for (int j = i + 1; j < numvertices; j++) { + Vector3f N2 = vertices.get(j); + + for (int k = j + 1; k < numvertices; k++) { + Vector3f N3 = vertices.get(k); + + edge0.sub(N2, N1); + edge1.sub(N3, N1); + float normalSign = 1f; + for (int ww = 0; ww < 2; ww++) { + tmp.cross(edge0, edge1); + planeEquation.x = normalSign * tmp.x; + planeEquation.y = normalSign * tmp.y; + planeEquation.z = normalSign * tmp.z; + + if (VectorUtil.lengthSquared3(planeEquation) > 0.0001f) { + VectorUtil.normalize3(planeEquation); + if (notExist(planeEquation, planeEquationsOut)) { + planeEquation.w = -VectorUtil.dot3(planeEquation, N1); + + // check if inside, and replace supportingVertexOut if needed + if (areVerticesBehindPlane(planeEquation, vertices, 0.01f)) { + planeEquationsOut.add(planeEquation); + } + } + } + normalSign = -1f; + } + } + } + } + } + finally { + stack.vectors.pop(); + stack.vectors4.pop(); + } + } + + public static void getVerticesFromPlaneEquations(List<Vector4f> planeEquations, List<Vector3f> verticesOut) { + BulletStack stack = BulletStack.get(); + + stack.vectors.push(); + try { + Vector3f n2n3 = stack.vectors.get(); + Vector3f n3n1 = stack.vectors.get(); + Vector3f n1n2 = stack.vectors.get(); + Vector3f potentialVertex = stack.vectors.get(); + + int numbrushes = planeEquations.size(); + // brute force: + for (int i = 0; i < numbrushes; i++) { + Vector4f N1 = planeEquations.get(i); + + for (int j = i + 1; j < numbrushes; j++) { + Vector4f N2 = planeEquations.get(j); + + for (int k = j + 1; k < numbrushes; k++) { + Vector4f N3 = planeEquations.get(k); + + VectorUtil.cross3(n2n3, N2, N3); + VectorUtil.cross3(n3n1, N3, N1); + VectorUtil.cross3(n1n2, N1, N2); + + if ((n2n3.lengthSquared() > 0.0001f) && + (n3n1.lengthSquared() > 0.0001f) && + (n1n2.lengthSquared() > 0.0001f)) { + // point P out of 3 plane equations: + + // d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 ) + // P = ------------------------------------------------------------------------- + // N1 . ( N2 * N3 ) + + float quotient = VectorUtil.dot3(N1, n2n3); + if (Math.abs(quotient) > 0.000001f) { + quotient = -1f / quotient; + n2n3.scale(N1.w); + n3n1.scale(N2.w); + n1n2.scale(N3.w); + potentialVertex.set(n2n3); + potentialVertex.add(n3n1); + potentialVertex.add(n1n2); + potentialVertex.scale(quotient); + + // check if inside, and replace supportingVertexOut if needed + if (isPointInsidePlanes(planeEquations, potentialVertex, 0.01f)) { + verticesOut.add(potentialVertex); + } + } + } + } + } + } + } + finally { + stack.vectors.pop(); + } + } + +} diff --git a/src/jbullet/src/javabullet/linearmath/IDebugDraw.java b/src/jbullet/src/javabullet/linearmath/IDebugDraw.java new file mode 100644 index 0000000..407e4c6 --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/IDebugDraw.java @@ -0,0 +1,86 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +import javabullet.BulletStack; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public abstract class IDebugDraw { + + protected final BulletStack stack = BulletStack.get(); + + public abstract void drawLine(Vector3f from, Vector3f to, Vector3f color); + + public abstract void drawContactPoint(Vector3f PointOnB, Vector3f normalOnB, float distance, int lifeTime, Vector3f color); + + public abstract void reportErrorWarning(String warningString); + + public abstract void draw3dText(Vector3f location, String textString); + + public abstract void setDebugMode(int debugMode); + + public abstract int getDebugMode(); + + public void drawAabb(Vector3f from, Vector3f to, Vector3f color) { + stack.vectors.push(); + try { + Vector3f halfExtents = stack.vectors.get(to); + halfExtents.sub(from); + halfExtents.scale(0.5f); + + Vector3f center = stack.vectors.get(to); + center.add(from); + center.scale(0.5f); + + int i, j; + + Vector3f edgecoord = stack.vectors.get(1f, 1f, 1f), pa = stack.vectors.get(), pb = stack.vectors.get(); + for (i = 0; i < 4; i++) { + for (j = 0; j < 3; j++) { + pa.set(edgecoord.x * halfExtents.x, edgecoord.y * halfExtents.y, edgecoord.z * halfExtents.z); + pa.add(center); + + int othercoord = j % 3; + + VectorUtil.mulCoord(edgecoord, othercoord, -1f); + pb.set(edgecoord.x * halfExtents.x, edgecoord.y * halfExtents.y, edgecoord.z * halfExtents.z); + pb.add(center); + + drawLine(pa, pb, color); + } + edgecoord.set(-1f, -1f, -1f); + if (i < 3) { + VectorUtil.mulCoord(edgecoord, i, -1f); + } + } + } + finally { + stack.vectors.pop(); + } + } +} diff --git a/src/jbullet/src/javabullet/linearmath/MatrixUtil.java b/src/jbullet/src/javabullet/linearmath/MatrixUtil.java new file mode 100644 index 0000000..f5c9774 --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/MatrixUtil.java @@ -0,0 +1,208 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +import javabullet.BulletStack; +import javax.vecmath.Matrix3f; +import javax.vecmath.Quat4f; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class MatrixUtil { + + public static void scale(Matrix3f dest, Matrix3f mat, Vector3f s) { + dest.m00 = mat.m00 * s.x; dest.m01 = mat.m01 * s.y; dest.m02 = mat.m02 * s.z; + dest.m10 = mat.m10 * s.x; dest.m11 = mat.m11 * s.y; dest.m12 = mat.m12 * s.z; + dest.m20 = mat.m20 * s.x; dest.m21 = mat.m21 * s.y; dest.m22 = mat.m22 * s.z; + } + + public static void absolute(Matrix3f mat) { + mat.m00 = Math.abs(mat.m00); + mat.m01 = Math.abs(mat.m01); + mat.m02 = Math.abs(mat.m02); + mat.m10 = Math.abs(mat.m10); + mat.m11 = Math.abs(mat.m11); + mat.m12 = Math.abs(mat.m12); + mat.m20 = Math.abs(mat.m20); + mat.m21 = Math.abs(mat.m21); + mat.m22 = Math.abs(mat.m22); + } + + public static void setFromOpenGLSubMatrix(Matrix3f mat, float[] m) { + mat.m00 = m[0]; mat.m01 = m[4]; mat.m02 = m[8]; + mat.m10 = m[1]; mat.m11 = m[5]; mat.m12 = m[9]; + mat.m20 = m[2]; mat.m21 = m[6]; mat.m22 = m[10]; + } + + public static void getOpenGLSubMatrix(Matrix3f mat, float[] m) { + m[0] = mat.m00; + m[1] = mat.m10; + m[2] = mat.m20; + m[3] = 0f; + m[4] = mat.m01; + m[5] = mat.m11; + m[6] = mat.m21; + m[7] = 0f; + m[8] = mat.m02; + m[9] = mat.m12; + m[10] = mat.m22; + m[11] = 0f; + } + + /** + * setEulerZYX + * + * @param euler a const reference to a btVector3 of euler angles + * These angles are used to produce a rotation matrix. The euler + * angles are applied in ZYX order. I.e a vector is first rotated + * about X then Y and then Z + */ + public static void setEulerZYX(Matrix3f mat, float eulerX, float eulerY, float eulerZ) { + float ci = (float) Math.cos(eulerX); + float cj = (float) Math.cos(eulerY); + float ch = (float) Math.cos(eulerZ); + float si = (float) Math.sin(eulerX); + float sj = (float) Math.sin(eulerY); + float sh = (float) Math.sin(eulerZ); + float cc = ci * ch; + float cs = ci * sh; + float sc = si * ch; + float ss = si * sh; + + mat.setRow(0, cj * ch, sj * sc - cs, sj * cc + ss); + mat.setRow(1, cj * sh, sj * ss + cc, sj * cs - sc); + mat.setRow(2, -sj, cj * si, cj * ci); + } + + private static float tdotx(Matrix3f mat, Vector3f vec) { + return mat.m00 * vec.x + mat.m10 * vec.y + mat.m20 * vec.z; + } + + private static float tdoty(Matrix3f mat, Vector3f vec) { + return mat.m01 * vec.x + mat.m11 * vec.y + mat.m21 * vec.z; + } + + private static float tdotz(Matrix3f mat, Vector3f vec) { + return mat.m02 * vec.x + mat.m12 * vec.y + mat.m22 * vec.z; + } + + public static void transposeTransform(Vector3f dest, Vector3f vec, Matrix3f mat) { + float x = tdotx(mat, vec); + float y = tdoty(mat, vec); + float z = tdotz(mat, vec); + dest.x = x; + dest.y = y; + dest.z = z; + } + + public static void setRotation(Matrix3f dest, Quat4f q) { + float d = q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; + assert (d != 0f); + float s = 2f / d; + float xs = q.x * s, ys = q.y * s, zs = q.z * s; + float wx = q.w * xs, wy = q.w * ys, wz = q.w * zs; + float xx = q.x * xs, xy = q.x * ys, xz = q.x * zs; + float yy = q.y * ys, yz = q.y * zs, zz = q.z * zs; + dest.m00 = 1f - (yy + zz); + dest.m01 = xy - wz; + dest.m02 = xz + wy; + dest.m10 = xy + wz; + dest.m11 = 1f - (xx + zz); + dest.m12 = yz - wx; + dest.m20 = xz - wy; + dest.m21 = yz + wx; + dest.m22 = 1f - (xx + yy); + } + + public static void getRotation(Matrix3f mat, Quat4f dest) { + BulletStack stack = BulletStack.get(); + + float trace = mat.m00 + mat.m11 + mat.m22; + float[] temp = stack.floatArrays.getFixed(4); + + if (trace > 0f) { + float s = (float) Math.sqrt(trace + 1f); + temp[3] = (s * 0.5f); + s = 0.5f / s; + + temp[0] = ((mat.m21 - mat.m12) * s); + temp[1] = ((mat.m02 - mat.m20) * s); + temp[2] = ((mat.m10 - mat.m01) * s); + } + else { + int i = mat.m00 < mat.m11 ? (mat.m11 < mat.m22 ? 2 : 1) : (mat.m00 < mat.m22 ? 2 : 0); + int j = (i + 1) % 3; + int k = (i + 2) % 3; + + float s = (float) Math.sqrt(mat.getElement(i, i) - mat.getElement(j, j) - mat.getElement(k, k) + 1f); + temp[i] = s * 0.5f; + s = 0.5f / s; + + temp[3] = (mat.getElement(k, j) - mat.getElement(j, k)) * s; + temp[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s; + temp[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s; + } + dest.set(temp[0], temp[1], temp[2], temp[3]); + + stack.floatArrays.release(temp); + } + + private static float cofac(Matrix3f mat, int r1, int c1, int r2, int c2) { + return mat.getElement(r1, c1) * mat.getElement(r2, c2) - mat.getElement(r1, c2) * mat.getElement(r2, c1); + } + + public static void invert(Matrix3f mat) { + float co_x = cofac(mat, 1, 1, 2, 2); + float co_y = cofac(mat, 1, 2, 2, 0); + float co_z = cofac(mat, 1, 0, 2, 1); + + float det = mat.m00*co_x + mat.m01*co_y + mat.m02*co_z; + assert (det != 0f); + + float s = 1f / det; + float m00 = co_x * s; + float m01 = cofac(mat, 0, 2, 2, 1) * s; + float m02 = cofac(mat, 0, 1, 1, 2) * s; + float m10 = co_y * s; + float m11 = cofac(mat, 0, 0, 2, 2) * s; + float m12 = cofac(mat, 0, 2, 1, 0) * s; + float m20 = co_z * s; + float m21 = cofac(mat, 0, 1, 2, 0) * s; + float m22 = cofac(mat, 0, 0, 1, 1) * s; + + mat.m00 = m00; + mat.m01 = m01; + mat.m02 = m02; + mat.m10 = m10; + mat.m11 = m11; + mat.m12 = m12; + mat.m20 = m20; + mat.m21 = m21; + mat.m22 = m22; + } + +} diff --git a/src/jbullet/src/javabullet/linearmath/MiscUtil.java b/src/jbullet/src/javabullet/linearmath/MiscUtil.java new file mode 100644 index 0000000..e522fb2 --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/MiscUtil.java @@ -0,0 +1,156 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +import java.util.Comparator; +import java.util.List; +import javabullet.util.FloatArrayList; +import javabullet.util.IntArrayList; + +/** + * + * @author jezek2 + */ +public class MiscUtil { + + public static int getListCapacityForHash(List<?> list) { + return getListCapacityForHash(list.size()); + } + + public static int getListCapacityForHash(int size) { + int n = 2; + while (n < size) { + n <<= 1; + } + return n; + } + + public static <T> void ensureIndex(List<T> list, int index, T value) { + while (list.size() <= index) { + list.add(value); + } + } + + public static void resize(IntArrayList list, int size, int value) { + while (list.size() < size) { + list.add(value); + } + + while (list.size() > size) { + list.remove(list.size() - 1); + } + } + + public static void resize(FloatArrayList list, int size, float value) { + while (list.size() < size) { + list.add(value); + } + + while (list.size() > size) { + list.remove(list.size() - 1); + } + } + + public static <T> void resize(List<T> list, int size, Class<T> valueCls) { + try { + while (list.size() < size) { + list.add(valueCls != null? valueCls.newInstance() : null); + } + + while (list.size() > size) { + list.remove(list.size() - 1); + } + } + catch (IllegalAccessException e) { + throw new IllegalStateException(e); + } + catch (InstantiationException e) { + throw new IllegalStateException(e); + } + } + + public static <T> int indexOf(T[] array, T obj) { + for (int i=0; i<array.length; i++) { + if (array[i] == obj) return i; + } + return -1; + } + + public static float GEN_clamped(float a, float lb, float ub) { + return a < lb ? lb : (ub < a ? ub : a); + } + + /** + * Heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/ + */ + private static <T> void downHeap(List<T> pArr, int k, int n, Comparator<T> comparator) { + /* PRE: a[k+1..N] is a heap */ + /* POST: a[k..N] is a heap */ + + T temp = pArr.get(k - 1); + /* k has child(s) */ + while (k <= n / 2) { + int child = 2 * k; + + if ((child < n) && comparator.compare(pArr.get(child - 1), pArr.get(child)) < 0) { + child++; + } + /* pick larger child */ + if (comparator.compare(temp, pArr.get(child - 1)) < 0) { + /* move child up */ + pArr.set(k - 1, pArr.get(child - 1)); + k = child; + } + else { + break; + } + } + pArr.set(k - 1, temp); + } + + public static <T> void heapSort(List<T> list, Comparator<T> comparator) { + /* sort a[0..N-1], N.B. 0 to N-1 */ + int k; + int n = list.size(); + for (k = n / 2; k > 0; k--) { + downHeap(list, k, n, comparator); + } + + /* a[1..N] is now a heap */ + while (n >= 1) { + swap(list, 0, n - 1); /* largest of a[0..n-1] */ + + n = n - 1; + /* restore a[1..i-1] heap */ + downHeap(list, 1, n, comparator); + } + } + + private static <T> void swap(List<T> list, int index0, int index1) { + T temp = list.get(index0); + list.set(index0, list.get(index1)); + list.set(index1, temp); + } + +} diff --git a/src/jbullet/src/javabullet/linearmath/MotionState.java b/src/jbullet/src/javabullet/linearmath/MotionState.java new file mode 100644 index 0000000..964df4a --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/MotionState.java @@ -0,0 +1,41 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +/** + * MotionState allows the dynamics world to synchronize the updated world transforms with graphics. + * For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation). + * + * @author jezek2 + */ +public interface MotionState { + + public void getWorldTransform(Transform worldTrans); + + /** + * Note: Bullet only calls the update of worldtransform for active objects. + */ + public void setWorldTransform(Transform worldTrans); + +} diff --git a/src/jbullet/src/javabullet/linearmath/QuaternionUtil.java b/src/jbullet/src/javabullet/linearmath/QuaternionUtil.java new file mode 100644 index 0000000..65fece4 --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/QuaternionUtil.java @@ -0,0 +1,104 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javax.vecmath.Quat4f; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class QuaternionUtil { + + public static float getAngle(Quat4f q) { + float s = 2f * (float) Math.acos(q.w); + return s; + } + + public static void setRotation(Quat4f q, Vector3f axis, float angle) { + float d = axis.length(); + assert (d != 0f); + float s = (float)Math.sin(angle * 0.5f) / d; + q.set(axis.x * s, axis.y * s, axis.z * s, (float) Math.cos(angle * 0.5f)); + } + + // Game Programming Gems 2.10. make sure v0,v1 are normalized + public static Quat4f shortestArcQuat(Vector3f v0, Vector3f v1) { + BulletStack stack = BulletStack.get(); + + stack.vectors.push(); + stack.quats.push(); + try { + Vector3f c = stack.vectors.get(); + c.cross(v0, v1); + float d = v0.dot(v1); + + if (d < -1.0 + BulletGlobals.FLT_EPSILON) { + // just pick any vector + return stack.quats.returning(stack.quats.get(0.0f, 1.0f, 0.0f, 0.0f)); + } + + float s = (float) Math.sqrt((1.0f + d) * 2.0f); + float rs = 1.0f / s; + + return stack.quats.returning(stack.quats.get(c.x * rs, c.y * rs, c.z * rs, s * 0.5f)); + } + finally { + stack.vectors.pop(); + stack.quats.pop(); + } + } + + public static void mul(Quat4f q, Vector3f w) { + float rx = q.w * w.x + q.y * w.z - q.z * w.y; + float ry = q.w * w.y + q.z * w.x - q.x * w.z; + float rz = q.w * w.z + q.x * w.y - q.y * w.x; + float rw = -q.x * w.x - q.y * w.y - q.z * w.z; + q.set(rx, ry, rz, rw); + } + + public static Vector3f quatRotate(Quat4f rotation, Vector3f v) { + BulletStack stack = BulletStack.get(); + + stack.vectors.push(); + stack.quats.push(); + try { + Quat4f q = stack.quats.get(rotation); + QuaternionUtil.mul(q, v); + + Quat4f tmp = stack.quats.get(); + tmp.inverse(rotation); + q.mul(tmp); + return stack.vectors.returning(stack.vectors.get(q.x, q.y, q.z)); + } + finally { + stack.vectors.pop(); + stack.quats.pop(); + } + } + +} diff --git a/src/jbullet/src/javabullet/linearmath/ScalarUtil.java b/src/jbullet/src/javabullet/linearmath/ScalarUtil.java new file mode 100644 index 0000000..c898ebb --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/ScalarUtil.java @@ -0,0 +1,58 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +import javabullet.BulletGlobals; + +/** + * + * @author jezek2 + */ +public class ScalarUtil { + + public static float fsel(float a, float b, float c) { + return a >= 0 ? b : c; + } + + public static boolean fuzzyZero(float x) { + return Math.abs(x) < BulletGlobals.FLT_EPSILON; + } + + public static float atan2Fast(float y, float x) { + float coeff_1 = BulletGlobals.SIMD_PI / 4.0f; + float coeff_2 = 3.0f * coeff_1; + float abs_y = Math.abs(y); + float angle; + if (x >= 0.0f) { + float r = (x - abs_y) / (x + abs_y); + angle = coeff_1 - coeff_1 * r; + } + else { + float r = (x + abs_y) / (abs_y - x); + angle = coeff_2 - coeff_1 * r; + } + return (y < 0.0f) ? -angle : angle; + } + +} diff --git a/src/jbullet/src/javabullet/linearmath/Transform.java b/src/jbullet/src/javabullet/linearmath/Transform.java new file mode 100644 index 0000000..1b44958 --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/Transform.java @@ -0,0 +1,148 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +import javabullet.BulletStack; +import javax.vecmath.Matrix3f; +import javax.vecmath.Quat4f; +import javax.vecmath.Vector3f; + +/** + * Transform supports rigid transforms (only translation and rotation, no scaling/shear). + * + * @author jezek2 + */ +public class Transform { + + protected BulletStack stack; + + public final Matrix3f basis = new Matrix3f(); + public final Vector3f origin = new Vector3f(); + + public Transform() { + } + + public Transform(Matrix3f mat) { + basis.set(mat); + } + + public Transform(Transform tr) { + set(tr); + } + + public void set(Transform tr) { + basis.set(tr.basis); + origin.set(tr.origin); + } + + public void transform(Vector3f v) { + basis.transform(v); + v.add(origin); + } + + public void setIdentity() { + basis.setIdentity(); + origin.set(0f, 0f, 0f); + } + + public void inverse() { + basis.transpose(); + origin.scale(-1f); + basis.transform(origin); + } + + public void inverse(Transform tr) { + set(tr); + inverse(); + } + + public void mul(Transform tr) { + if (stack == null) stack = BulletStack.get(); + + stack.vectors.push(); + try { + Vector3f vec = stack.vectors.get(tr.origin); + transform(vec); + + basis.mul(tr.basis); + origin.set(vec); + } + finally { + stack.vectors.pop(); + } + } + + public void mul(Transform tr1, Transform tr2) { + set(tr1); + mul(tr2); + } + + public void invXform(Vector3f inVec, Vector3f out) { + if (stack == null) stack = BulletStack.get(); + + stack.matrices.push(); + try { + out.sub(inVec, origin); + + Matrix3f mat = stack.matrices.get(basis); + mat.transpose(); + mat.transform(out); + } + finally { + stack.matrices.pop(); + } + } + + public Quat4f getRotation() { + if (stack == null) stack = BulletStack.get(); + + stack.quats.push(); + try { + Quat4f q = stack.quats.get(); + MatrixUtil.getRotation(basis, q); + return stack.quats.returning(q); + } + finally { + stack.quats.pop(); + } + } + + public void setRotation(Quat4f q) { + MatrixUtil.setRotation(basis, q); + } + + public void setFromOpenGLMatrix(float[] m) { + MatrixUtil.setFromOpenGLSubMatrix(basis, m); + origin.set(m[12], m[13], m[14]); + } + + public void getOpenGLMatrix(float[] m) { + MatrixUtil.getOpenGLSubMatrix(basis, m); + m[12] = origin.x; + m[13] = origin.y; + m[14] = origin.z; + m[15] = 1f; + } + +} diff --git a/src/jbullet/src/javabullet/linearmath/TransformUtil.java b/src/jbullet/src/javabullet/linearmath/TransformUtil.java new file mode 100644 index 0000000..25acf80 --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/TransformUtil.java @@ -0,0 +1,174 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +import javabullet.BulletGlobals; +import javabullet.BulletStack; +import javax.vecmath.Matrix3f; +import javax.vecmath.Quat4f; +import javax.vecmath.Vector3f; + +/** + * + * @author jezek2 + */ +public class TransformUtil { + + public static final float SIMDSQRT12 = 0.7071067811865475244008443621048490f; + public static final float ANGULAR_MOTION_THRESHOLD = 0.5f*BulletGlobals.SIMD_HALF_PI; + + public static float recipSqrt(float x) { + return 1f / (float)Math.sqrt(x); /* reciprocal square root */ + } + + public static void planeSpace1(Vector3f n, Vector3f p, Vector3f q) { + if (Math.abs(n.z) > SIMDSQRT12) { + // choose p in y-z plane + float a = n.y * n.y + n.z * n.z; + float k = recipSqrt(a); + p.set(0, -n.z * k, n.y * k); + // set q = n x p + q.set(a * k, -n.x * p.z, n.x * p.y); + } + else { + // choose p in x-y plane + float a = n.x * n.x + n.y * n.y; + float k = recipSqrt(a); + p.set(-n.y * k, n.x * k, 0); + // set q = n x p + q.set(-n.z * p.y, n.z * p.x, a * k); + } + } + + public static void integrateTransform(Transform curTrans, Vector3f linvel, Vector3f angvel, float timeStep, Transform predictedTransform) { + BulletStack stack = BulletStack.get(); + + stack.vectors.push(); + stack.quats.push(); + try { + predictedTransform.origin.scaleAdd(timeStep, linvel, curTrans.origin); + // //#define QUATERNION_DERIVATIVE + // #ifdef QUATERNION_DERIVATIVE + // btQuaternion predictedOrn = curTrans.getRotation(); + // predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5)); + // predictedOrn.normalize(); + // #else + // exponential map + Vector3f axis = stack.vectors.get(); + float fAngle = angvel.length(); + + // limit the angular motion + if (fAngle * timeStep > ANGULAR_MOTION_THRESHOLD) { + fAngle = ANGULAR_MOTION_THRESHOLD / timeStep; + } + + if (fAngle < 0.001f) { + // use Taylor's expansions of sync function + axis.scale(0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * fAngle * fAngle, angvel); + } + else { + // sync(fAngle) = sin(c*fAngle)/t + axis.scale((float) Math.sin(0.5f * fAngle * timeStep) / fAngle, angvel); + } + Quat4f dorn = stack.quats.get(axis.x, axis.y, axis.z, (float) Math.cos(fAngle * timeStep * 0.5f)); + Quat4f orn0 = stack.quats.get(curTrans.getRotation()); + + Quat4f predictedOrn = stack.quats.get(); + predictedOrn.mul(dorn, orn0); + predictedOrn.normalize(); + // #endif + predictedTransform.setRotation(predictedOrn); + } + finally { + stack.vectors.pop(); + stack.quats.pop(); + } + } + + public static void calculateVelocity(Transform transform0, Transform transform1, float timeStep, Vector3f linVel, Vector3f angVel) { + BulletStack stack = BulletStack.get(); + + stack.vectors.push(); + try { + linVel.sub(transform1.origin, transform0.origin); + linVel.scale(1f / timeStep); + + Vector3f axis = stack.vectors.get(); + float[] angle = new float[1]; + calculateDiffAxisAngle(transform0, transform1, axis, angle); + angVel.scale(angle[0] / timeStep, axis); + } + finally { + stack.vectors.pop(); + } + } + + public static void calculateDiffAxisAngle(Transform transform0, Transform transform1, Vector3f axis, float[] angle) { + BulletStack stack = BulletStack.get(); + + stack.matrices.push(); + stack.quats.push(); + try { + // #ifdef USE_QUATERNION_DIFF + // btQuaternion orn0 = transform0.getRotation(); + // btQuaternion orn1a = transform1.getRotation(); + // btQuaternion orn1 = orn0.farthest(orn1a); + // btQuaternion dorn = orn1 * orn0.inverse(); + // #else + Matrix3f tmp = stack.matrices.get(); + tmp.set(transform0.basis); + MatrixUtil.invert(tmp); + + Matrix3f dmat = stack.matrices.get(); + dmat.mul(transform1.basis, tmp); + + Quat4f dorn = stack.quats.get(); + MatrixUtil.getRotation(dmat, dorn); + // #endif + + // floating point inaccuracy can lead to w component > 1..., which breaks + + dorn.normalize(); + + angle[0] = QuaternionUtil.getAngle(dorn); + axis.set(dorn.x, dorn.y, dorn.z); + // TODO: probably not needed + //axis[3] = btScalar(0.); + + // check for axis length + float len = axis.lengthSquared(); + if (len < BulletGlobals.FLT_EPSILON * BulletGlobals.FLT_EPSILON) { + axis.set(1f, 0f, 0f); + } + else { + axis.scale(1f / (float) Math.sqrt(len)); + } + } + finally { + stack.matrices.pop(); + stack.quats.pop(); + } + } + +} diff --git a/src/jbullet/src/javabullet/linearmath/VectorUtil.java b/src/jbullet/src/javabullet/linearmath/VectorUtil.java new file mode 100644 index 0000000..b0ba6ca --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/VectorUtil.java @@ -0,0 +1,189 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.linearmath; + +import javax.vecmath.Vector3f; +import javax.vecmath.Vector4f; + +/** + * + * @author jezek2 + */ +public class VectorUtil { + + public static int maxAxis(Vector3f v) { + int maxIndex = -1; + float maxVal = -1e30f; + if (v.x > maxVal) { + maxIndex = 0; + maxVal = v.x; + } + if (v.y > maxVal) { + maxIndex = 1; + maxVal = v.y; + } + if (v.z > maxVal) { + maxIndex = 2; + maxVal = v.z; + } + + return maxIndex; + } + + public static int maxAxis4(Vector4f v) { + int maxIndex = -1; + float maxVal = -1e30f; + if (v.x > maxVal) { + maxIndex = 0; + maxVal = v.x; + } + if (v.y > maxVal) { + maxIndex = 1; + maxVal = v.y; + } + if (v.z > maxVal) { + maxIndex = 2; + maxVal = v.z; + } + if (v.w > maxVal) { + maxIndex = 3; + maxVal = v.w; + } + + return maxIndex; + } + + public static int closestAxis4(Vector4f vec) { + Vector4f tmp = new Vector4f(vec); + tmp.absolute(); + return maxAxis4(tmp); + } + + public static float getCoord(Vector3f vec, int num) { + switch (num) { + case 0: return vec.x; + case 1: return vec.y; + case 2: return vec.z; + default: throw new InternalError(); + } + } + + public static void setCoord(Vector3f vec, int num, float value) { + switch (num) { + case 0: vec.x = value; break; + case 1: vec.y = value; break; + case 2: vec.z = value; break; + default: throw new InternalError(); + } + } + + public static void mulCoord(Vector3f vec, int num, float value) { + switch (num) { + case 0: vec.x *= value; break; + case 1: vec.y *= value; break; + case 2: vec.z *= value; break; + default: throw new InternalError(); + } + } + + public static void setInterpolate3(Vector3f dest, Vector3f v0, Vector3f v1, float rt) { + float s = 1f - rt; + dest.x = s * v0.x + rt * v1.x; + dest.y = s * v0.y + rt * v1.y; + dest.z = s * v0.z + rt * v1.z; + // don't do the unused w component + // m_co[3] = s * v0[3] + rt * v1[3]; + } + + public static void add(Vector3f dest, Vector3f v1, Vector3f v2) { + dest.x = v1.x + v2.x; + dest.y = v1.y + v2.y; + dest.z = v1.z + v2.z; + } + + public static void add(Vector3f dest, Vector3f v1, Vector3f v2, Vector3f v3) { + dest.x = v1.x + v2.x + v3.x; + dest.y = v1.y + v2.y + v3.y; + dest.z = v1.z + v2.z + v3.z; + } + + public static void add(Vector3f dest, Vector3f v1, Vector3f v2, Vector3f v3, Vector3f v4) { + dest.x = v1.x + v2.x + v3.x + v4.x; + dest.y = v1.y + v2.y + v3.y + v4.y; + dest.z = v1.z + v2.z + v3.z + v4.z; + } + + public static void mul(Vector3f dest, Vector3f v1, Vector3f v2) { + dest.x = v1.x * v2.x; + dest.y = v1.y * v2.y; + dest.z = v1.z * v2.z; + } + + public static void div(Vector3f dest, Vector3f v1, Vector3f v2) { + dest.x = v1.x / v2.x; + dest.y = v1.y / v2.y; + dest.z = v1.z / v2.z; + } + + public static void setMin(Vector3f a, Vector3f b) { + a.x = Math.min(a.x, b.x); + a.y = Math.min(a.y, b.y); + a.z = Math.min(a.z, b.z); + } + + public static void setMax(Vector3f a, Vector3f b) { + a.x = Math.max(a.x, b.x); + a.y = Math.max(a.y, b.y); + a.z = Math.max(a.z, b.z); + } + + public static float dot3(Vector4f v0, Vector3f v1) { + return (v0.x*v1.x + v0.y*v1.y + v0.z*v1.z); + } + + public static float dot3(Vector4f v0, Vector4f v1) { + return (v0.x*v1.x + v0.y*v1.y + v0.z*v1.z); + } + + public static float lengthSquared3(Vector4f v) { + return (v.x*v.x + v.y*v.y + v.z*v.z); + } + + public static void normalize3(Vector4f v) { + float norm = (float)(1.0/Math.sqrt(v.x*v.x + v.y*v.y + v.z*v.z)); + v.x *= norm; + v.y *= norm; + v.z *= norm; + } + + public static void cross3(Vector3f dest, Vector4f v1, Vector4f v2) { + float x,y; + x = v1.y*v2.z - v1.z*v2.y; + y = v2.x*v1.z - v2.z*v1.x; + dest.z = v1.x*v2.y - v1.y*v2.x; + dest.x = x; + dest.y = y; + } + +} diff --git a/src/jbullet/src/javabullet/linearmath/package-info.java b/src/jbullet/src/javabullet/linearmath/package-info.java new file mode 100644 index 0000000..3c33279 --- /dev/null +++ b/src/jbullet/src/javabullet/linearmath/package-info.java @@ -0,0 +1,28 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +/** + * Vector math library. + */ +package javabullet.linearmath; + diff --git a/src/jbullet/src/javabullet/package-info.java b/src/jbullet/src/javabullet/package-info.java new file mode 100644 index 0000000..d7a4cc1 --- /dev/null +++ b/src/jbullet/src/javabullet/package-info.java @@ -0,0 +1,28 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +/** + * Contains globals, like variables, constants and object pools. + */ +package javabullet; + diff --git a/src/jbullet/src/javabullet/util/FloatArrayList.java b/src/jbullet/src/javabullet/util/FloatArrayList.java new file mode 100644 index 0000000..ab8fe41 --- /dev/null +++ b/src/jbullet/src/javabullet/util/FloatArrayList.java @@ -0,0 +1,71 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.util; + +/** + * + * @author jezek2 + */ +public class FloatArrayList { + + private float[] array = new float[16]; + private int size; + + public void add(float value) { + if (size == array.length) { + expand(); + } + + array[size++] = value; + } + + private void expand() { + float[] newArray = new float[array.length << 1]; + System.arraycopy(array, 0, newArray, 0, array.length); + array = newArray; + } + + public float remove(int index) { + if (index >= size) throw new IndexOutOfBoundsException(); + float old = array[index]; + System.arraycopy(array, index+1, array, index, size - index - 1); + size--; + return old; + } + + public float get(int index) { + if (index >= size) throw new IndexOutOfBoundsException(); + return array[index]; + } + + public void set(int index, float value) { + if (index >= size) throw new IndexOutOfBoundsException(); + array[index] = value; + } + + public int size() { + return size; + } + +} diff --git a/src/jbullet/src/javabullet/util/HashUtil.java b/src/jbullet/src/javabullet/util/HashUtil.java new file mode 100644 index 0000000..10e0008 --- /dev/null +++ b/src/jbullet/src/javabullet/util/HashUtil.java @@ -0,0 +1,126 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.util; + +import gnu.trove.THashMap; +import gnu.trove.TObjectObjectProcedure; +import java.util.HashMap; +import java.util.Iterator; + +/** + * Wrapper for THashMap (from GNU Trove), with fallback to less effective original HashMap. + * + * @author jezek2 + */ +public class HashUtil { + + private HashUtil() {} + + public interface IMap<K,V> { + public V get(K key); + public V put(K key, V value); + public V remove(K key); + public int size(); + public boolean forEachValue(IObjectProcedure<V> proc); + public boolean retainEntries(IObjectProcedure<V> proc); + } + + public interface IObjectProcedure<T> { + public boolean execute(T value); + } + + //////////////////////////////////////////////////////////////////////////// + + private static Class mapCls; + + static { + try { + mapCls = TroveHashMapImpl.class; + } + catch (Throwable t) { + mapCls = JavaHashMapImpl.class; + } + } + + @SuppressWarnings("unchecked") + public static <K,V> IMap<K,V> createMap() { + try { + return (IMap<K,V>)mapCls.newInstance(); + } + catch (IllegalAccessException e) { + throw new IllegalStateException(e); + } + catch (InstantiationException e) { + throw new IllegalStateException(e); + } + } + + //////////////////////////////////////////////////////////////////////////// + + protected static class JavaHashMapImpl<K,V> extends HashMap<K,V> implements IMap<K,V> { + public boolean forEachValue(IObjectProcedure<V> proc) { + for (V value : values()) { + if (!proc.execute(value)) return false; + } + return true; + } + + public boolean retainEntries(IObjectProcedure<V> proc) { + boolean mod = false; + for (Iterator<V> it = values().iterator(); it.hasNext(); ) { + V value = it.next(); + if (!proc.execute(value)) { + it.remove(); + mod = true; + } + } + return mod; + } + } + + //////////////////////////////////////////////////////////////////////////// + + protected static class TroveHashMapImpl<K,V> extends THashMap<K,V> implements IMap<K,V> { + private TroveObjectObjectProcedureWrapper<K,V> valueWrapper = new TroveObjectObjectProcedureWrapper<K,V>(); + + public boolean forEachValue(IObjectProcedure<V> proc) { + valueWrapper.proc = proc; + return forEachEntry(valueWrapper); + } + + public boolean retainEntries(IObjectProcedure<V> proc) { + valueWrapper.proc = proc; + return retainEntries(valueWrapper); + } + } + + protected static class TroveObjectObjectProcedureWrapper<K,V> implements TObjectObjectProcedure<K,V> { + public IObjectProcedure<V> proc; + + public boolean execute(K key, V value) { + return proc.execute(value); + } + } + +} diff --git a/src/jbullet/src/javabullet/util/HashUtil.java-orig b/src/jbullet/src/javabullet/util/HashUtil.java-orig new file mode 100644 index 0000000..10e0008 --- /dev/null +++ b/src/jbullet/src/javabullet/util/HashUtil.java-orig @@ -0,0 +1,126 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.util; + +import gnu.trove.THashMap; +import gnu.trove.TObjectObjectProcedure; +import java.util.HashMap; +import java.util.Iterator; + +/** + * Wrapper for THashMap (from GNU Trove), with fallback to less effective original HashMap. + * + * @author jezek2 + */ +public class HashUtil { + + private HashUtil() {} + + public interface IMap<K,V> { + public V get(K key); + public V put(K key, V value); + public V remove(K key); + public int size(); + public boolean forEachValue(IObjectProcedure<V> proc); + public boolean retainEntries(IObjectProcedure<V> proc); + } + + public interface IObjectProcedure<T> { + public boolean execute(T value); + } + + //////////////////////////////////////////////////////////////////////////// + + private static Class mapCls; + + static { + try { + mapCls = TroveHashMapImpl.class; + } + catch (Throwable t) { + mapCls = JavaHashMapImpl.class; + } + } + + @SuppressWarnings("unchecked") + public static <K,V> IMap<K,V> createMap() { + try { + return (IMap<K,V>)mapCls.newInstance(); + } + catch (IllegalAccessException e) { + throw new IllegalStateException(e); + } + catch (InstantiationException e) { + throw new IllegalStateException(e); + } + } + + //////////////////////////////////////////////////////////////////////////// + + protected static class JavaHashMapImpl<K,V> extends HashMap<K,V> implements IMap<K,V> { + public boolean forEachValue(IObjectProcedure<V> proc) { + for (V value : values()) { + if (!proc.execute(value)) return false; + } + return true; + } + + public boolean retainEntries(IObjectProcedure<V> proc) { + boolean mod = false; + for (Iterator<V> it = values().iterator(); it.hasNext(); ) { + V value = it.next(); + if (!proc.execute(value)) { + it.remove(); + mod = true; + } + } + return mod; + } + } + + //////////////////////////////////////////////////////////////////////////// + + protected static class TroveHashMapImpl<K,V> extends THashMap<K,V> implements IMap<K,V> { + private TroveObjectObjectProcedureWrapper<K,V> valueWrapper = new TroveObjectObjectProcedureWrapper<K,V>(); + + public boolean forEachValue(IObjectProcedure<V> proc) { + valueWrapper.proc = proc; + return forEachEntry(valueWrapper); + } + + public boolean retainEntries(IObjectProcedure<V> proc) { + valueWrapper.proc = proc; + return retainEntries(valueWrapper); + } + } + + protected static class TroveObjectObjectProcedureWrapper<K,V> implements TObjectObjectProcedure<K,V> { + public IObjectProcedure<V> proc; + + public boolean execute(K key, V value) { + return proc.execute(value); + } + } + +} diff --git a/src/jbullet/src/javabullet/util/IntArrayList.java b/src/jbullet/src/javabullet/util/IntArrayList.java new file mode 100644 index 0000000..aeab7ba --- /dev/null +++ b/src/jbullet/src/javabullet/util/IntArrayList.java @@ -0,0 +1,71 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +package javabullet.util; + +/** + * + * @author jezek2 + */ +public class IntArrayList { + + private int[] array = new int[16]; + private int size; + + public void add(int value) { + if (size == array.length) { + expand(); + } + + array[size++] = value; + } + + private void expand() { + int[] newArray = new int[array.length << 1]; + System.arraycopy(array, 0, newArray, 0, array.length); + array = newArray; + } + + public int remove(int index) { + if (index >= size) throw new IndexOutOfBoundsException(); + int old = array[index]; + System.arraycopy(array, index+1, array, index, size - index - 1); + size--; + return old; + } + + public int get(int index) { + if (index >= size) throw new IndexOutOfBoundsException(); + return array[index]; + } + + public void set(int index, int value) { + if (index >= size) throw new IndexOutOfBoundsException(); + array[index] = value; + } + + public int size() { + return size; + } + +} diff --git a/src/jbullet/src/javabullet/util/package-info.java b/src/jbullet/src/javabullet/util/package-info.java new file mode 100644 index 0000000..1e9db35 --- /dev/null +++ b/src/jbullet/src/javabullet/util/package-info.java @@ -0,0 +1,28 @@ +/* + * Java port of Bullet (c) 2008 Martin Dvorak <[email protected]> + * + * Bullet Continuous Collision Detection and Physics Library + * Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ + * + * This software is provided 'as-is', without any express or implied warranty. + * In no event will the authors be held liable for any damages arising from + * the use of this software. + * + * Permission is granted to anyone to use this software for any purpose, + * including commercial applications, and to alter it and redistribute it + * freely, subject to the following restrictions: + * + * 1. The origin of this software must not be misrepresented; you must not + * claim that you wrote the original software. If you use this software + * in a product, an acknowledgment in the product documentation would be + * appreciated but is not required. + * 2. Altered source versions must be plainly marked as such, and must not be + * misrepresented as being the original software. + * 3. This notice may not be removed or altered from any source distribution. + */ + +/** + * Java-specific utility classes. + */ +package javabullet.util; + |