Initial Commit

thirdparty/bullet/CMakeLists.txt

@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 3.0 FATAL_ERROR)
+
+project(bullet-physics)
+
+add_subdirectory(src)

thirdparty/bullet/docs/latex/BulletQuickstart.tex

@@ -0,0 +1,106 @@
+
+% Bullet Physics manual
+% http://bulletphysics.org
+% Written by Erwin Coumans
+
+% Preamble (global definitions of the book/manual)
+% Use openany option for documentclass to avoid empty page after TOC
+
+\documentclass[openany]{book}
+%scrbook is easier on the eyes, and use a bigger font
+%\documentclass[openany,DIV=calc,16pt]{scrbook}
+
+% The english package can help breaking words
+\usepackage[english]{babel}
+
+%The Charter or Utopia font is easier on the eyes for screen
+\renewcommand{\familydefault}{bch}%Charter font
+%\renewcommand{\familydefault}{put} %Utopia font
+
+
+% Titlepic allows us to use a picture on the frontpage
+\usepackage{titlepic}
+\usepackage{graphicx}
+
+\usepackage[a4paper, left=2cm, right=1cm,  top=2cm, bottom=3.5cm]{geometry}
+\usepackage[latin1]{inputenc}
+
+% If using \doublespacing include the setspace package
+% \usepackage{setspace}
+\usepackage{fancyhdr}
+\usepackage{tocloft}
+
+
+% The hyperref package already include package url
+\usepackage[colorlinks=true, linkcolor=blue, urlcolor=blue]{hyperref}
+
+% \setcounter{secnumdepth}{4}
+\usepackage{makeidx}\makeindex
+
+%support for C++ source code snippets
+%you can even import existing C++ code as-is
+%or a range of lines within markers beween rangeprefix/rangesuffix
+\usepackage{listings}
+%\usepackage{color}
+\renewcommand{\lstlistingname}{Source Code}
+\renewcommand{\lstlistlistingname}{Source Code Listings}
+\lstset{
+	tabsize=2,	language=C++, keywordstyle=\color[rgb]{0,0,1},
+	 commentstyle=\color[rgb]{0.133,0.545,0.133},
+     stringstyle=\color[rgb]{0.627,0.126,0.941},
+      breaklines=true,
+     numberstyle=\small,
+	basicstyle=\ttfamily\small,
+	rangeprefix=///-----,	rangesuffix=-----,
+	includerangemarker=false,
+	numbers=left,	stepnumber=1,
+	frame=single,
+}
+
+
+% Set the tolerance so TeX really breaks the line
+% and don't run into the right margin (avoid overfull boxes)
+\tolerance=10000
+
+%end of preamble
+
+\begin{document}
+
+%	\pagestyle{fancy}
+
+%\pagenumbering{}
+%
+	\title{\textbf{Bullet 2.83 Quickstart Guide}}
+	\titlepic{\includegraphics[width=0.4\textwidth]{bullet_logo_2010_9.eps}}
+%
+	\author{Erwin Coumans}
+	\maketitle
+
+
+	\renewcommand{\cftchapdotsep}{\cftdotsep}
+	\tableofcontents
+	\pagenumbering{arabic}
+
+%	\fancyhf{}
+%	\doublespacing
+
+% include all the chapters as separate tex files
+% on Mac OSX it is really nice to use the Texpad application for automatic navigation
+
+
+	\include{intro}
+	\include{building}
+	\include{helloworld}
+	\include{faq}
+	
+	\clearpage
+	\addcontentsline{toc}{chapter}{Source code listings}
+	\lstlistoflistings
+
+	\clearpage
+	\addcontentsline{toc}{chapter}{Index}
+	\printindex
+
+	
+\end{document}
+

thirdparty/bullet/docs/latex/building.tex

@@ -0,0 +1,42 @@
+\chapter{Building the Bullet SDK and demos}
+Windows developers can download the zipped sources of Bullet from \url{http://bullet.googlecode.com}. Mac OS X, Linux and other developers should download the gzipped tar archive.
+Bullet provides several build systems.
+
+\section{Using Premake}
+\index{premake}\href{http://industriousone.com/premake}{Premake} is a meta build system based on the Lua scripting language that can generate project files for Microsoft Visual Studio, Apple Xcode as well as Makefiles for GNU make and other build systems. Bullet comes with Premake executables for Windows, Mac OSX and Linux.
+\subsection{Premake Visual Studio project generation}
+You can double-click on \path{Bullet/build/vs2010.bat} to generate Visual Studio 2010 project files and solution. This batch file calls Premake. Just open \path{Bullet/build/vs2010/0BulletSolution.sln}. Newer versions of Visual Studio should automatically convert from the 2010 solution.
+\subsection{Premake Mac OSX Xcode project generation}
+On Mac OSX it is easiest to open a Terminal window and switch current directory to Bullet/build and use the following command to generate XCode projects:
+ \begin{lstlisting}[caption=Premake for Mac OSX, label=premake_osx]
+cd Bullet/build
+./premake_osx xcode4
+open xcode4/0BulletSolution.xcworkspace
+\end{lstlisting}
+
+\subsection{Premake GNU Makefile generation}
+You can also generate GNU Makefiles for Mac OSX or Linux using premake:
+\begin{lstlisting}[caption=Premake to GNU Makefile, label=premake_make]
+cd Bullet/build
+./premake_osx gmake
+cd gmake
+make config=release64
+\end{lstlisting}
+\section{Using cmake}
+Similar to premake, CMake adds support for many other build environments and platforms, including Microsoft Visual Studio, XCode for Mac OSX, KDevelop for Linux and Unix Makefiles. Download and install \index{CMake}CMake from \url{http://cmake.org} and use the CMake cmake-gui tool. Alternatively use a terminal, change to the Bullet root directory and use for example the following commands:
+
+\begin{lstlisting}[caption=CMake to GNU Makefile, label=cmake_make]
+cmake . 
+make
+\end{lstlisting}
+An example to create an XCode project on Mac OSX using CMake:
+\begin{lstlisting}[caption=CMake to Xcode, label=cmake_xcode]
+mkdir xcode
+cd xcode
+cmake .. -G Xcode
+open BULLET_PHYSICS.xcodeproj
+\end{lstlisting}
+
+
+\section{Executing the Example Browser}
+After building the SDK, there are some binary executables in the bin folder. You can execute the $bin/App_ExampleBrowser_*$ to experiment with the Bullet Physics SDK.

thirdparty/bullet/docs/latex/faq.tex

@@ -0,0 +1,3 @@
+
+\chapter{Frequently asked questions}
+Here is a placeholder for a future FAQ. For more information it is best to visit the Bullet Physics forums and wiki at \url{http://bulletphysics.org}.

thirdparty/bullet/docs/latex/helloworld.tex

@@ -0,0 +1,17 @@
+\chapter{Hello World}
+\section{C++ console program}
+Let's discuss the creation of a basic Bullet simulation from the beginning to the end. For simplicity we print the state of the simulation to console using printf, instead of using 3D graphics to display the objects. The source code of this tutorial is located in \path{examples/HelloWorld/HelloWorld.cpp}. 
+
+It is a good idea to try to compile, link and run this HelloWorld.cpp program first.
+
+As you can see in \ref{helloworld_includefiles} you can include a convenience header file \path{btBulletDynamicsCommon.h}.
+
+While linking to Bullet Physics make sure the link order is: BulletSoftBody, BulletDynamics, BulletCollision and LinearMath.
+
+\lstinputlisting[caption=HelloWorld.cpp include header, label=helloworld_includefiles,linerange=includes_start-includes_end]{../../examples/HelloWorld/HelloWorld.cpp}
+Now we create the dynamics world:
+\lstinputlisting[caption=HelloWorld.cpp initialize world, label=stepsimulation,linerange=initialization_start-initialization_end]{../../examples/HelloWorld/HelloWorld.cpp}
+Once the world is created you can step the simulation as follows:
+\lstinputlisting[caption=HelloWorld.cpp step simulation, label=stepsimulation,linerange=stepsimulation_start-stepsimulation_end]{../../examples/HelloWorld/HelloWorld.cpp}
+At the end of the program you delete all objects in the reverse order of creation. Here is the cleanup listing of our HelloWorld.cpp program.
+\lstinputlisting[caption=HelloWorld.cpp cleanup, label=cleanup,linerange=cleanup_start-cleanup_end]{../../examples/HelloWorld/HelloWorld.cpp}

thirdparty/bullet/docs/latex/intro.tex

@@ -0,0 +1,53 @@
+
+%Use the \path{Demos/MyDemo} instead of \texttt{Demos/MyDemo} otherwise text doesn't wrap
+%and runs into the right margin
+
+\chapter{Introduction to Bullet}
+%\section{blablaIntroduction}
+Bullet Physics is a professional open source collision detection, rigid body and soft body dynamics library. Bullet Physics targets real-time and interactive use in games, visual effects in movies and robotics. The library is free for commercial use under the \index{zlib license} \href{http://opensource.org/licenses/zlib-license.php}{zlib license}.
+
+\section{Main Features}
+
+\begin{itemize}
+	\item Open source C++ code under zlib license and free for any commercial use on all platforms including PLAYSTATION 3, XBox 360, Wii, PC, Linux, Mac OSX, Android and iPhone
+	\item Discrete and continuous collision detection including ray and convex sweep test. Collision shapes include concave and convex meshes and all basic primitives
+	\item Fast and stable rigid body dynamics constraint solver, vehicle dynamics, character controller and slider, hinge, generic 6DOF and cone twist constraint for ragdolls
+	\item Soft Body dynamics for cloth, rope and deformable volumes with two-way interaction with rigid bodies, including constraint support
+	\item Native binary .bullet file format and example importers for URDF, Wavefront obj and Quake bsp files.
+\end{itemize}
+
+\section{Contact and Support}
+\begin{itemize}
+	\item Public forum for support and feedback is available at \url{http://bulletphysics.org}
+\end{itemize}
+
+\section{What's new}
+
+\subsection{New in Bullet 2.83}
+\begin{itemize}
+	\item New ExampleBrowser, replacing the standalone demos. Not all demos have been ported over yet, it will happen in upcoming releases. It is recommended to use an OpenGL 3+ system, although there is limited OpenGL 2 fallback. See \path{examples/ExampleBrowser}.
+	\item Import of Universal Robot Description Files (URDF). See \path{examples/Importers/ImportURDFDemo}. You can use File-Open in the ExampleBrowser.
+	\item Improved support for btMultiBody with multi-degree of freedom mobilizers, thanks to Jakub Stepien. See \path{examples/MultiBody/MultiDofDemo}.
+	\item New btGeneric6DofSpring2Constraint, replacing the old generic 6dof constraint, thanks to Gabor PUHR and Tamas Umenhoffer. See \path{examples/Dof6Spring2Setup}	
+	\item OpenCL demo integrated in the ExampleBrowser. The demo is disabled by default, because it only works on high-end desktop GPU systems with compatible up-to-date OpenCL 1.1+ driver. Use $--enable\_experimental\_opencl$ command-line argument in the ExampleBrowser.
+	
+	
+\end{itemize}
+\subsection{New in Bullet 2.82}
+\begin{itemize}
+	\item Featherstone articulated body algorithm implementation with integration in the Bullet constraint solver. See \path{examples/MultiBodyDemo}
+	\item New MLCP constraint solver interface for higher quality direct solvers. Dantzig (OpenDE), PATH and Projected Gauss Seidel MLCP solvers, with fallback to the original Bullet sequential impulse solver. See \path{src/BulletDynamics/MLCPSolvers}
+	\item New btFixedConstraint as alternative to a btGeneric6DofConstraint with all DOFs locked. See \path{Demos/VoronoiFractureDemo}
+	\item Various bug fixes, related to force feedback and friction. Improved performance between btCompoundShape using the new btCompoundCompoundCollisionAlgorithm. See the commit log at \url{https://code.google.com/p/bullet/source/list}
+\end{itemize}
+\subsection{New in Bullet 2.81}
+\begin{itemize}
+	\item SIMD and Neon optimizations for iOS and Mac OSX, thanks to a contribution from Apple
+	\item Rolling Friction using a constraint, thanks to Erin Catto for the idea. See \path{Demos/RollingFrictionDemo/RollingFrictionDemo.cpp}
+	\item XML serialization. See \path{Bullet/Demos/BulletXmlImportDemo} and \path{Bullet/Demos/SerializeDemo}
+	\item Gear constraint. See \path{Bullet/Demos/ConstraintDemo}.
+	\item Improved continuous collision response, feeding speculative contacts to the constraint solver. See \path{Bullet/Demos/CcdPhysicsDemo}
+	\item Improved premake4 build system including support for Mac OSX, Linux and iOS
+	\item Refactoring of collision detection pipeline using stack allocation instead of modifying the collision object. This will allow better future multithreading optimizations.
+\end{itemize}
+

thirdparty/bullet/docs/latex/titlepic.sty

@@ -0,0 +1,68 @@
+% titlepic.sty is a LaTeX package to show a picture on the cover produced by \maketitle.
+% By Thomas ten Cate <ttencate@gmail.com>. Free software, no warranty of any kind.
+% 
+% Version history:
+% 1.1: now more self-contained, comes with a PDF manual
+% 1.0: first release
+% 
+% -----------------------------------------------------------------------------
+
+% No idea whether it works on older LaTeXes.
+\NeedsTeXFormat{LaTeX2e}
+
+% Package identification and version number.
+\ProvidesPackage{titlepic}[2009/08/03 1.1 Package to display a picture on the title page]
+
+% Declare the options.
+\DeclareOption{tt}{\gdef\@tptopspace{}\gdef\@tpsepspace{\vskip 3em}}
+\DeclareOption{tc}{\gdef\@tptopspace{}\gdef\@tpsepspace{\vfil}}
+\DeclareOption{cc}{\gdef\@tptopspace{\null\vfil}\gdef\@tpsepspace{\vskip 3em}}
+\ExecuteOptions{cc}
+\ProcessOptions
+
+% Define the sole command introduced by this package.
+% Very similar to the definition of \title, etc.
+\def\titlepic#1{\gdef\@titlepic{#1}}
+\def\@titlepic{\@empty} % default: no picture
+
+% If a title page was requested from the document class (article/report/book),
+% override \maketitle to show our picture.
+\if@titlepage
+\renewcommand\maketitle{
+	\begin{titlepage}%
+		\let\footnotesize\small
+		\let\footnoterule\relax
+		\let \footnote \thanks
+		\@tptopspace%
+		\begin{center}%
+			{\LARGE \@title \par}%
+			\vskip 3em%
+			{\large
+				\lineskip .75em%
+				\begin{tabular}[t]{c}%
+				\@author
+				\end{tabular}\par%
+			}%
+			\vskip 1.5em%
+			{\large \@date \par}%       % Set date in \large size.
+		\end{center}\par
+		\@tpsepspace%
+		{\centering\@titlepic\par}
+		\vfil
+		\@thanks
+	\end{titlepage}%
+	\setcounter{footnote}{0}%
+	\global\let\thanks\relax
+	\global\let\maketitle\relax
+	\global\let\@thanks\@empty
+	\global\let\@author\@empty
+	\global\let\@date\@empty
+	\global\let\@title\@empty
+	\global\let\@titlepic\@empty
+	\global\let\title\relax
+	\global\let\author\relax
+	\global\let\date\relax
+	\global\let\and\relax
+	\global\let\titlepic\relax
+}
+\fi

thirdparty/bullet/docs/pybullet_quickstart_guide/PlainPreview/Article.md.html

@@ -0,0 +1,10 @@
+<meta charset="utf-8" lang="en">
+Hello Markdeep!
+
+- Hello
+- lists!
+
+Hello MathJax:
+\[f(0)=\frac{1}{2\cdot\pi\cdot i}\cdot \oint_{|z|=1} \frac{f(z)}{z} \textrm{d}z\]
+
+<script src="MarkdeepUtility.js"></script>

thirdparty/bullet/docs/pybullet_quickstart_guide/PlainPreview/BuildMarkdeepUtility.py

@@ -0,0 +1,44 @@
+import re
+
+if (__name__ == "__main__"):
+  # Assemble the script which embeds the Markdeep page into the preview blog
+  PreviewBlogPage = open("PreviewBlogPage.htm", "rb").read().decode("utf-8")
+  HeadMatch = re.search("<head(.*?)>(.*?)</head>", PreviewBlogPage, re.DOTALL)
+  HeadAttributes = HeadMatch.group(1)
+  FullDocumentHead = HeadMatch.group(2)
+  BodyMatch = re.search("<body(.*?)>(.*?)</body>", PreviewBlogPage, re.DOTALL)
+  BodyAttributes = BodyMatch.group(1)
+  FullPreviewBody = BodyMatch.group(2)
+  ArticleHTMLCodeMacro = "$(ARTICLE_HTML_CODE)"
+  iArticleHTMLCodeMacro = FullPreviewBody.find(ArticleHTMLCodeMacro)
+  DocumentBodyPrefix = FullPreviewBody[0:iArticleHTMLCodeMacro]
+  DocumentBodySuffix = FullPreviewBody[iArticleHTMLCodeMacro + len(ArticleHTMLCodeMacro):]
+  FullPrepareHTMLCode = open("PrepareHTML.js", "rb").read().decode("utf-8")
+  ReplacementList = [("$(FULL_DOCUMENT_HEAD)", FullDocumentHead),
+                     ("$(DOCUMENT_BODY_PREFIX)", DocumentBodyPrefix),
+                     ("$(DOCUMENT_BODY_SUFFIX)", DocumentBodySuffix)]
+  for Macro, Replacement in ReplacementList:
+    FullPrepareHTMLCode = FullPrepareHTMLCode.replace(
+        Macro,
+        Replacement.replace("\r\n", "\\r\\n\\\r\n").replace("'", "\\'"))
+  # Generate code which sets body and head attributes appropriately
+  for Element, AttributeCode in [("head", HeadAttributes), ("body", BodyAttributes)]:
+    FullPrepareHTMLCode += "\r\n// Setting " + Element + " attributes\r\n"
+    for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"", AttributeCode):
+      FullPrepareHTMLCode += "document." + Element + ".setAttribute(\"" + Match.group(
+          1) + "\",\"" + Match.group(2) + "\");\r\n"
+  open("PrepareHTML.full.js", "wb").write(FullPrepareHTMLCode.encode("utf-8"))
+
+  # Concatenate all the scripts together
+  SourceFileList = [
+      "PrepareHTML.full.js", "SetMarkdeepMode.js", "markdeep.min.js", "DisplayMarkdeepOutput.js",
+      "InvokeMathJax.js"
+  ]
+  OutputCode = "\r\n\r\n".join([
+      "// " + SourceFile + "\r\n\r\n" + open(SourceFile, "rb").read().decode("utf-8")
+      for SourceFile in SourceFileList
+  ])
+  OutputFile = open("MarkdeepUtility.js", "wb")
+  OutputFile.write(OutputCode.encode("utf-8"))
+  OutputFile.close()
+  print("Done.")

thirdparty/bullet/docs/pybullet_quickstart_guide/PlainPreview/DisplayMarkdeepOutput.js

@@ -0,0 +1,6 @@
+BodyHTML=document.body.innerHTML;
+BeginTag="<!-- MARKDEEP_BEGIN -->";
+EndTag="<!-- MARKDEEP_END -->";
+BodyHTML=BodyHTML.slice(BodyHTML.indexOf(BeginTag)+BeginTag.length,BodyHTML.lastIndexOf(EndTag));
+document.getElementById("BodyDisplayBox").textContent=BodyHTML;
+document.head.innerHTML=FullDocumentHead;

thirdparty/bullet/docs/pybullet_quickstart_guide/PlainPreview/InvokeMathJax.js

@@ -0,0 +1,4 @@
+var MathjaxScript=document.createElement("script");
+MathjaxScript.type="text/javascript";
+MathjaxScript.src="https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML";
+document.head.appendChild(MathjaxScript);

thirdparty/bullet/docs/pybullet_quickstart_guide/PlainPreview/PrepareHTML.full.js

@@ -0,0 +1,117 @@
+/** Converts <>&" to their HTML escape sequences */
+function escapeHTMLEntities(str) {
+    return String(str).replace(/&/g, '&amp;').replace(/</g, '&lt;').replace(/>/g, '&gt;').replace(/"/g, '&quot;');
+}
+
+
+/** Restores the original source string's '<' and '>' as entered in
+    the document, before the browser processed it as HTML. There is no
+    way in an HTML document to distinguish an entity that was entered
+    as an entity.*/
+function unescapeHTMLEntities(str) {
+    // Process &amp; last so that we don't recursively unescape
+    // escaped escape sequences.
+    return str.
+        replace(/&lt;/g, '<').
+        replace(/&gt;/g, '>').
+        replace(/&quot;/g, '"').
+        replace(/&#39;/g, "'").
+        replace(/&ndash;/g, '--').
+        replace(/&mdash;/g, '---').
+        replace(/&amp;/g, '&');
+}
+
+
+/**
+   \param node  A node from an HTML DOM
+
+   \return A String that is a very good reconstruction of what the
+   original source looked like before the browser tried to correct
+   it to legal HTML.
+ */
+function nodeToMarkdeepSource(node, leaveEscapes) {
+    var source = node.innerHTML;
+
+    // Markdown uses <john@bar.com> e-mail syntax, which HTML parsing
+    // will try to close by inserting the matching close tags at the end of the
+    // document. Remove anything that looks like that and comes *after*
+    // the first fallback style.
+    source = source.replace(/(?:<style class="fallback">[\s\S]*?<\/style>[\s\S]*)<\/\S+@\S+\.\S+?>/gim, '');
+    
+    // Remove artificially inserted close tags
+    source = source.replace(/<\/h?ttps?:.*>/gi, '');
+    
+    // Now try to fix the URLs themselves, which will be 
+    // transformed like this: <http: casual-effects.com="" markdeep="">
+    source = source.replace(/<(https?): (.*?)>/gi, function (match, protocol, list) {
+
+        // Remove any quotes--they wouldn't have been legal in the URL anyway
+        var s = '<' + protocol + '://' + list.replace(/=""\s/g, '/');
+
+        if (s.substring(s.length - 3) === '=""') {
+            s = s.substring(0, s.length - 3);
+        }
+
+        // Remove any lingering quotes (since they
+        // wouldn't have been legal in the URL)
+        s = s.replace(/"/g, '');
+
+        return s + '>';
+    });
+
+    // Remove the "fallback" style tags
+    source = source.replace(/<style class=["']fallback["']>.*?<\/style>/gmi, '');
+
+    source = unescapeHTMLEntities(source);
+
+    return source;
+}
+
+// $ (FULL_DOCUMENT_HEAD) is replaced by the contents of the <head> found in 
+// PreviewBlogPage.htm. This document head will overwrite whatever Markdeep does to 
+// the head at the very end.
+FullDocumentHead='\
+\r\n\
+		<meta http-equiv="content-type" content="text/html; charset=UTF-8">\r\n\
+		<meta charset="UTF-8">\r\n\
+\r\n\
+		<!-- Markdeep styles -->\r\n\
+		<style>body{counter-reset: h1 h2 h3 h4 h5 h6}.md code,pre{font-family:Menlo,Consolas,monospace;font-size:15.018802542857143px;line-height:140%}.md div.title{font-size:26px;font-weight:800;line-height:120%;text-align:center}.md div.afterTitles{height:10px}.md div.subtitle{text-align:center}.md .image{display:inline-block}.md div.imagecaption,.md div.tablecaption,.md div.listingcaption{margin:0.2em 5px 10px 5px;text-align: justify;font-style:italic}.md div.imagecaption{margin-bottom:0}.md img{max-width:100%;page-break-inside:avoid}li{text-align:left};.md div.tilde{margin:20px 0 -10px;text-align:center}.md blockquote.fancyquote{margin:25px 0 25px;text-align:left;line-height:160%}.md blockquote.fancyquote::before{content:"“";color:#DDD;font-family:Times New Roman;font-size:45px;line-height:0;margin-right:6px;vertical-align:-0.3em}.md span.fancyquote{font-size:118%;color:#777;font-style:italic}.md span.fancyquote::after{content:"”";font-style:normal;color:#DDD;font-family:Times New Roman;font-size:45px;line-height:0;margin-left:6px;vertical-align:-0.3em}.md blockquote.fancyquote .author{width:100%;margin-top:10px;display:inline-block;text-align:right}.md small{font-size:60%}.md div.title,contents,.md .tocHeader,h1,h2,h3,h4,h5,h6,.md .shortTOC,.md .mediumTOC,.nonumberh1,.nonumberh2,.nonumberh3,.nonumberh4,.nonumberh5,.nonumberh6{font-family:Verdana,Helvetica,Arial,sans-serif;margin:13.4px 0 13.4px;padding:15px 0 3px;border-top:none;clear:both}.md svg.diagram{display:block;font-family:Menlo,Consolas,monospace;font-size:15.018802542857143px;text-align:center;stroke-linecap:round;stroke-width:2px;page-break-inside:avoid;stroke:#000;fill:#000}.md svg.diagram .opendot{fill:#FFF}.md svg.diagram text{stroke:none}.md h1,.tocHeader,.nonumberh1{border-bottom:3px solid;font-size:20px;font-weight:bold;}h1,.nonumberh1{counter-reset: h2 h3 h4 h5 h6}h2,.nonumberh2{counter-reset: h3 h4 h5 h6;border-bottom:2px solid #999;color:#555;font-size:18px;}h3,h4,h5,h6,.nonumberh3,.nonumberh4,.nonumberh5,.nonumberh6{font-family:Helvetica,Arial,sans-serif;color:#555;font-size:16px;}h3{counter-reset:h4 h5 h6}h4{counter-reset:h5 h6}h5{counter-reset:h6}.md table{border-collapse:collapse;line-height:140%;page-break-inside:avoid}.md table.table{margin:auto}.md table.calendar{width:100%;margin:auto;font-size:11px;font-family:Helvetica,Arial,sans-serif}.md table.calendar th{font-size:16px}.md .today{background:#ECF8FA}.md .calendar .parenthesized{color:#999;font-style:italic}.md div.tablecaption{text-align:center}.md table.table th{color:#FFF;background-color:#AAA;border:1px solid #888;padding:8px 15px 8px 15px}.md table.table td{padding:5px 15px 5px 15px;border:1px solid #888}.md table.table tr:nth-child(even){background:#EEE}.md pre.tilde{border-top: 1px solid #CCC;border-bottom: 1px solid #CCC;padding: 5px 0 5px 20px;margin:0 0 30px 0;background:#FCFCFC;page-break-inside:avoid}.md a:link, .md a:visited{color:#38A;text-decoration:none}.md a:link:hover{text-decoration:underline}.md dt{font-weight:700}dl>.md dd{padding:0 0 18px}.md dl>table{margin:35px 0 30px}.md code{white-space:pre;page-break-inside:avoid}.md .endnote{font-size:13px;line-height:15px;padding-left:10px;text-indent:-10px}.md .bib{padding-left:80px;text-indent:-80px;text-align:left}.markdeepFooter{font-size:9px;text-align:right;padding-top:80px;color:#999}.md .mediumTOC{float:right;font-size:12px;line-height:15px;border-left:1px solid #CCC;padding-left:15px;margin:15px 0px 15px 25px}.md .mediumTOC .level1{font-weight:600}.md .longTOC .level1{font-weight:600;display:block;padding-top:12px;margin:0 0 -20px}.md .shortTOC{text-align:center;font-weight:bold;margin-top:15px;font-size:14px}</style>\r\n\
+\r\n\
+		<!-- hljs styles -->\r\n\
+		<style>.hljs{display:block;overflow-x:auto;padding:0.5em;background:#fff;color:#000;-webkit-text-size-adjust:none}.hljs-comment{color:#006a00}.hljs-keyword{color:#02E}.hljs-literal,.nginx .hljs-title{color:#aa0d91}.method,.hljs-list .hljs-title,.hljs-tag .hljs-title,.setting .hljs-value,.hljs-winutils,.tex .hljs-command,.http .hljs-title,.hljs-request,.hljs-status,.hljs-name{color:#008}.hljs-envvar,.tex .hljs-special{color:#660}.hljs-string{color:#c41a16}.hljs-tag .hljs-value,.hljs-cdata,.hljs-filter .hljs-argument,.hljs-attr_selector,.apache .hljs-cbracket,.hljs-date,.hljs-regexp{color:#080}.hljs-sub .hljs-identifier,.hljs-pi,.hljs-tag,.hljs-tag .hljs-keyword,.hljs-decorator,.ini .hljs-title,.hljs-shebang,.hljs-prompt,.hljs-hexcolor,.hljs-rule .hljs-value,.hljs-symbol,.hljs-symbol .hljs-string,.hljs-number,.css .hljs-function,.hljs-function .hljs-title,.coffeescript .hljs-attribute{color:#A0C}.hljs-function .hljs-title{font-weight:bold;color:#000}.hljs-class .hljs-title,.smalltalk .hljs-class,.hljs-type,.hljs-typename,.hljs-tag .hljs-attribute,.hljs-doctype,.hljs-class .hljs-id,.hljs-built_in,.setting,.hljs-params,.clojure .hljs-attribute{color:#5c2699}.hljs-variable{color:#3f6e74}.css .hljs-tag,.hljs-rule .hljs-property,.hljs-pseudo,.hljs-subst{color:#000}.css .hljs-class,.css .hljs-id{color:#9b703f}.hljs-value .hljs-important{color:#ff7700;font-weight:bold}.hljs-rule .hljs-keyword{color:#c5af75}.hljs-annotation,.apache .hljs-sqbracket,.nginx .hljs-built_in{color:#9b859d}.hljs-preprocessor,.hljs-preprocessor *,.hljs-pragma{color:#643820}.tex .hljs-formula{background-color:#eee;font-style:italic}.diff .hljs-header,.hljs-chunk{color:#808080;font-weight:bold}.diff .hljs-change{background-color:#bccff9}.hljs-addition{background-color:#baeeba}.hljs-deletion{background-color:#ffc8bd}.hljs-comment .hljs-doctag{font-weight:bold}.method .hljs-id{color:#000}</style>\r\n\
+	\
+';
+
+// This code is placed at the beginning of the body before the Markdeep code. 
+// $ (DOCUMENT_BODY_PREFIX) is everything in the body of PreviewBlogPage.htm up to 
+// $ (ARTICLE_HTML_CODE).
+DocumentBodyPrefix='\
+\r\n\
+\
+	<!-- MARKDEEP_BEGIN -->\
+	<pre class="markdeep">\
+';
+// This code is placed at the end of the body after the Markdeep code. 
+// $ (DOCUMENT_BODY_SUFFIX) is everything in the body of PreviewBlogPage.htm after 
+// $ (ARTICLE_HTML_CODE).
+DocumentBodySuffix='\
+	</pre>\
+	<!-- MARKDEEP_END -->\
+	<div>Document &lt;body&gt; code:<br/>\
+	<textarea cols="40" rows="10" id="BodyDisplayBox"></textarea></div>\
+\r\n\
+	\
+';
+
+// Get the full Markdeep code from the .md.html file without the script invocation
+MarkdeepCode=nodeToMarkdeepSource(document.body);
+MarkdeepCode=MarkdeepCode.slice(0,MarkdeepCode.lastIndexOf("<script"));
+// Bring it into a form where it can be pasted into an HTML document
+SanitizedMarkdeepCode=escapeHTMLEntities(MarkdeepCode);
+// Surround it by the prefix and suffix code and set that as body code
+document.body.innerHTML=DocumentBodyPrefix+SanitizedMarkdeepCode+DocumentBodySuffix;
+
+// Setting head attributes
+
+// Setting body attributes

thirdparty/bullet/docs/pybullet_quickstart_guide/PlainPreview/PrepareHTML.js

@@ -0,0 +1,102 @@
+/** Converts <>&" to their HTML escape sequences */
+function escapeHTMLEntities(str) {
+    return String(str).replace(/&/g, '&amp;').replace(/</g, '&lt;').replace(/>/g, '&gt;').replace(/"/g, '&quot;');
+}
+
+
+/** Restores the original source string's '<' and '>' as entered in
+    the document, before the browser processed it as HTML. There is no
+    way in an HTML document to distinguish an entity that was entered
+    as an entity.*/
+function unescapeHTMLEntities(str) {
+    // Process &amp; last so that we don't recursively unescape
+    // escaped escape sequences.
+    return str.
+        replace(/&lt;/g, '<').
+        replace(/&gt;/g, '>').
+        replace(/&quot;/g, '"').
+        replace(/&#39;/g, "'").
+        replace(/&ndash;/g, '--').
+        replace(/&mdash;/g, '---').
+        replace(/&amp;/g, '&');
+}
+
+
+/**
+   \param node  A node from an HTML DOM
+
+   \return A String that is a very good reconstruction of what the
+   original source looked like before the browser tried to correct
+   it to legal HTML.
+ */
+function nodeToMarkdeepSource(node, leaveEscapes) {
+    var source = node.innerHTML;
+
+    // Markdown uses <john@bar.com> e-mail syntax, which HTML parsing
+    // will try to close by inserting the matching close tags at the end of the
+    // document. Remove anything that looks like that and comes *after*
+    // the first fallback style.
+    source = source.replace(/(?:<style class="fallback">[\s\S]*?<\/style>[\s\S]*)<\/\S+@\S+\.\S+?>/gim, '');
+    
+    // Remove artificially inserted close tags
+    source = source.replace(/<\/h?ttps?:.*>/gi, '');
+    
+    // Now try to fix the URLs themselves, which will be 
+    // transformed like this: <http: casual-effects.com="" markdeep="">
+    source = source.replace(/<(https?): (.*?)>/gi, function (match, protocol, list) {
+
+        // Remove any quotes--they wouldn't have been legal in the URL anyway
+        var s = '<' + protocol + '://' + list.replace(/=""\s/g, '/');
+
+        if (s.substring(s.length - 3) === '=""') {
+            s = s.substring(0, s.length - 3);
+        }
+
+        // Remove any lingering quotes (since they
+        // wouldn't have been legal in the URL)
+        s = s.replace(/"/g, '');
+
+        return s + '>';
+    });
+
+    // Remove the "fallback" style tags
+    source = source.replace(/<style class=["']fallback["']>.*?<\/style>/gmi, '');
+
+    source = unescapeHTMLEntities(source);
+
+    return source;
+}
+
+// $ (FULL_DOCUMENT_HEAD) is replaced by the contents of the <head> found in 
+// PreviewBlogPage.htm. This document head will overwrite whatever Markdeep does to 
+// the head at the very end.
+FullDocumentHead='\
+$(FULL_DOCUMENT_HEAD)\
+';
+
+// This code is placed at the beginning of the body before the Markdeep code. 
+// $ (DOCUMENT_BODY_PREFIX) is everything in the body of PreviewBlogPage.htm up to 
+// $ (ARTICLE_HTML_CODE).
+DocumentBodyPrefix='\
+$(DOCUMENT_BODY_PREFIX)\
+	<!-- MARKDEEP_BEGIN -->\
+	<pre class="markdeep">\
+';
+// This code is placed at the end of the body after the Markdeep code. 
+// $ (DOCUMENT_BODY_SUFFIX) is everything in the body of PreviewBlogPage.htm after 
+// $ (ARTICLE_HTML_CODE).
+DocumentBodySuffix='\
+	</pre>\
+	<!-- MARKDEEP_END -->\
+	<div>Document &lt;body&gt; code:<br/>\
+	<textarea cols="40" rows="10" id="BodyDisplayBox"></textarea></div>\
+$(DOCUMENT_BODY_SUFFIX)\
+';
+
+// Get the full Markdeep code from the .md.html file without the script invocation
+MarkdeepCode=nodeToMarkdeepSource(document.body);
+MarkdeepCode=MarkdeepCode.slice(0,MarkdeepCode.lastIndexOf("<script"));
+// Bring it into a form where it can be pasted into an HTML document
+SanitizedMarkdeepCode=escapeHTMLEntities(MarkdeepCode);
+// Surround it by the prefix and suffix code and set that as body code
+document.body.innerHTML=DocumentBodyPrefix+SanitizedMarkdeepCode+DocumentBodySuffix;

thirdparty/bullet/docs/pybullet_quickstart_guide/PlainPreview/PreviewBlogPage.htm

@@ -0,0 +1,17 @@
+<!DOCTYPE html>
+<html lang="en-US">
+	<head>
+		<meta http-equiv="content-type" content="text/html; charset=UTF-8">
+		<meta charset="UTF-8">
+
+		<!-- Markdeep styles -->
+		<style>body{counter-reset: h1 h2 h3 h4 h5 h6}.md code,pre{font-family:Menlo,Consolas,monospace;font-size:15.018802542857143px;line-height:140%}.md div.title{font-size:26px;font-weight:800;line-height:120%;text-align:center}.md div.afterTitles{height:10px}.md div.subtitle{text-align:center}.md .image{display:inline-block}.md div.imagecaption,.md div.tablecaption,.md div.listingcaption{margin:0.2em 5px 10px 5px;text-align: justify;font-style:italic}.md div.imagecaption{margin-bottom:0}.md img{max-width:100%;page-break-inside:avoid}li{text-align:left};.md div.tilde{margin:20px 0 -10px;text-align:center}.md blockquote.fancyquote{margin:25px 0 25px;text-align:left;line-height:160%}.md blockquote.fancyquote::before{content:"“";color:#DDD;font-family:Times New Roman;font-size:45px;line-height:0;margin-right:6px;vertical-align:-0.3em}.md span.fancyquote{font-size:118%;color:#777;font-style:italic}.md span.fancyquote::after{content:"”";font-style:normal;color:#DDD;font-family:Times New Roman;font-size:45px;line-height:0;margin-left:6px;vertical-align:-0.3em}.md blockquote.fancyquote .author{width:100%;margin-top:10px;display:inline-block;text-align:right}.md small{font-size:60%}.md div.title,contents,.md .tocHeader,h1,h2,h3,h4,h5,h6,.md .shortTOC,.md .mediumTOC,.nonumberh1,.nonumberh2,.nonumberh3,.nonumberh4,.nonumberh5,.nonumberh6{font-family:Verdana,Helvetica,Arial,sans-serif;margin:13.4px 0 13.4px;padding:15px 0 3px;border-top:none;clear:both}.md svg.diagram{display:block;font-family:Menlo,Consolas,monospace;font-size:15.018802542857143px;text-align:center;stroke-linecap:round;stroke-width:2px;page-break-inside:avoid;stroke:#000;fill:#000}.md svg.diagram .opendot{fill:#FFF}.md svg.diagram text{stroke:none}.md h1,.tocHeader,.nonumberh1{border-bottom:3px solid;font-size:20px;font-weight:bold;}h1,.nonumberh1{counter-reset: h2 h3 h4 h5 h6}h2,.nonumberh2{counter-reset: h3 h4 h5 h6;border-bottom:2px solid #999;color:#555;font-size:18px;}h3,h4,h5,h6,.nonumberh3,.nonumberh4,.nonumberh5,.nonumberh6{font-family:Helvetica,Arial,sans-serif;color:#555;font-size:16px;}h3{counter-reset:h4 h5 h6}h4{counter-reset:h5 h6}h5{counter-reset:h6}.md table{border-collapse:collapse;line-height:140%;page-break-inside:avoid}.md table.table{margin:auto}.md table.calendar{width:100%;margin:auto;font-size:11px;font-family:Helvetica,Arial,sans-serif}.md table.calendar th{font-size:16px}.md .today{background:#ECF8FA}.md .calendar .parenthesized{color:#999;font-style:italic}.md div.tablecaption{text-align:center}.md table.table th{color:#FFF;background-color:#AAA;border:1px solid #888;padding:8px 15px 8px 15px}.md table.table td{padding:5px 15px 5px 15px;border:1px solid #888}.md table.table tr:nth-child(even){background:#EEE}.md pre.tilde{border-top: 1px solid #CCC;border-bottom: 1px solid #CCC;padding: 5px 0 5px 20px;margin:0 0 30px 0;background:#FCFCFC;page-break-inside:avoid}.md a:link, .md a:visited{color:#38A;text-decoration:none}.md a:link:hover{text-decoration:underline}.md dt{font-weight:700}dl>.md dd{padding:0 0 18px}.md dl>table{margin:35px 0 30px}.md code{white-space:pre;page-break-inside:avoid}.md .endnote{font-size:13px;line-height:15px;padding-left:10px;text-indent:-10px}.md .bib{padding-left:80px;text-indent:-80px;text-align:left}.markdeepFooter{font-size:9px;text-align:right;padding-top:80px;color:#999}.md .mediumTOC{float:right;font-size:12px;line-height:15px;border-left:1px solid #CCC;padding-left:15px;margin:15px 0px 15px 25px}.md .mediumTOC .level1{font-weight:600}.md .longTOC .level1{font-weight:600;display:block;padding-top:12px;margin:0 0 -20px}.md .shortTOC{text-align:center;font-weight:bold;margin-top:15px;font-size:14px}</style>
+
+		<!-- hljs styles -->
+		<style>.hljs{display:block;overflow-x:auto;padding:0.5em;background:#fff;color:#000;-webkit-text-size-adjust:none}.hljs-comment{color:#006a00}.hljs-keyword{color:#02E}.hljs-literal,.nginx .hljs-title{color:#aa0d91}.method,.hljs-list .hljs-title,.hljs-tag .hljs-title,.setting .hljs-value,.hljs-winutils,.tex .hljs-command,.http .hljs-title,.hljs-request,.hljs-status,.hljs-name{color:#008}.hljs-envvar,.tex .hljs-special{color:#660}.hljs-string{color:#c41a16}.hljs-tag .hljs-value,.hljs-cdata,.hljs-filter .hljs-argument,.hljs-attr_selector,.apache .hljs-cbracket,.hljs-date,.hljs-regexp{color:#080}.hljs-sub .hljs-identifier,.hljs-pi,.hljs-tag,.hljs-tag .hljs-keyword,.hljs-decorator,.ini .hljs-title,.hljs-shebang,.hljs-prompt,.hljs-hexcolor,.hljs-rule .hljs-value,.hljs-symbol,.hljs-symbol .hljs-string,.hljs-number,.css .hljs-function,.hljs-function .hljs-title,.coffeescript .hljs-attribute{color:#A0C}.hljs-function .hljs-title{font-weight:bold;color:#000}.hljs-class .hljs-title,.smalltalk .hljs-class,.hljs-type,.hljs-typename,.hljs-tag .hljs-attribute,.hljs-doctype,.hljs-class .hljs-id,.hljs-built_in,.setting,.hljs-params,.clojure .hljs-attribute{color:#5c2699}.hljs-variable{color:#3f6e74}.css .hljs-tag,.hljs-rule .hljs-property,.hljs-pseudo,.hljs-subst{color:#000}.css .hljs-class,.css .hljs-id{color:#9b703f}.hljs-value .hljs-important{color:#ff7700;font-weight:bold}.hljs-rule .hljs-keyword{color:#c5af75}.hljs-annotation,.apache .hljs-sqbracket,.nginx .hljs-built_in{color:#9b859d}.hljs-preprocessor,.hljs-preprocessor *,.hljs-pragma{color:#643820}.tex .hljs-formula{background-color:#eee;font-style:italic}.diff .hljs-header,.hljs-chunk{color:#808080;font-weight:bold}.diff .hljs-change{background-color:#bccff9}.hljs-addition{background-color:#baeeba}.hljs-deletion{background-color:#ffc8bd}.hljs-comment .hljs-doctag{font-weight:bold}.method .hljs-id{color:#000}</style>
+	</head>
+
+	<body>
+$(ARTICLE_HTML_CODE)
+	</body>
+</html>

thirdparty/bullet/docs/pybullet_quickstart_guide/PlainPreview/SetMarkdeepMode.js

@@ -0,0 +1 @@
+window.markdeepOptions={mode:"html"};

thirdparty/bullet/docs/pybullet_quickstart_guide/PlainPreview/ShowHeadAdditions.html

@@ -0,0 +1,29 @@
+<!DOCTYPE html>
+<html lang="en-US">
+	<head>
+		<meta http-equiv="content-type" content="text/html; charset=UTF-8">
+		<meta charset="UTF-8">
+	<body>
+		<script>window.markdeepOptions={mode:"script"};</script>
+		<script src="./markdeep.min.js"></script>
+		<div>Document &lt;head&gt; additions:<br/>
+		<textarea cols="80" rows="40" id="HeadDisplayBox"></textarea></div>
+		<script>
+			// Get the complete Markdeep and hljs stylesheet
+			FullStylesheet=window.markdeep.stylesheet();
+			// It should consist of three <style> tags which we pick apart
+			StyleTagList=FullStylesheet.split("<style>");
+			// The second one defines section number, which we do not want, so we 
+			// just drop it. The other two are Markdeep and hljs styles.
+			MarkdeepStylesheet="<style>"+StyleTagList[1];
+			HLJSStylesheet="<style>"+StyleTagList[3];
+			// Now lets show the user what we found
+			document.getElementById("HeadDisplayBox").textContent=
+				 "<!-- Markdeep styles -->\r\n"
+				+MarkdeepStylesheet
+				+"\r\n\r\n<!-- hljs styles -->\r\n"
+				+HLJSStylesheet
+				+"\r\n\r\n<!-- MathJax invocation -->\r\n<script type=\"text/javascript\" async=\"\" src=\"https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML\">\r\n";
+		</script>
+	</body>
+</html>

thirdparty/bullet/docs/pybullet_quickstart_guide/PyBulletQuickstartGuide.md.html

@@ -0,0 +1,466 @@
+<meta charset="utf-8"><!-- -*- markdown -*- -->
+
+
+            **PyBullet Quickstart Guide**
+   [Erwin Coumans](http://twitter.com/erwincoumans), [Google Brain Robotics](https://research.google.com/teams/brain/robotics), <erwincoumans@google.com>
+   [Yunfei Bai](http://yunfei-bai.com), [X](https://x.company), <yunfeibai@x.team>
+
+
+
+Introduction
+======================================================================================
+
+pybullet is an easy to use Python module for physics simulation for robotics, games, visual effects and machine learning. With pybullet you can load articulated bodies from URDF, SDF, MJCF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics, collision detection and ray intersection queries. The Bullet Physics SDK includes pybullet robotic examples such as a simulated Minitaur quadruped, humanoids running using TensorFlow inference and KUKA arms grasping objects.
+
+![<small>[CoRL VR BotLab demo](https://github.com/erwincoumans/pybullet_robots/tree/master/corl_demo)
+  </small>](images/CoRL_VR_demo.png width="80%" border="2")
+
+Aside from physics simulation, there are bindings to rendering, with a CPU renderer (TinyRenderer) and OpenGL visualization and support for Virtual Reality headsets such as HTC Vive and Oculus Rift. pybullet also has functionality to perform collision detection queries (closest points, overlapping pairs, ray intersection test etc) and to add debug rendering (debug lines and text). pybullet has cross-platform built-in client-server support for shared memory, UDP and TCP networking. So you can run pybullet on Linux connecting to a Windows VR server.
+
+pybullet wraps the new Bullet C-API, which is designed to be independent from the underlying physics engine and render engine, so we can easily migrate to newer versions of Bullet, or use a different physics engine or render engine. By default, pybullet uses the Bullet 2.x API on the CPU. We will expose Bullet 3.x running on GPU using OpenCL as well. There is also a C++ API similar to pybullet, see b3RobotSimulatorClientAPI.
+
+pybullet can be easily used with TensorFlow and frameworks such as OpenAI Gym. Researchers from Google Brain [1,2], X, Stanford AI Lab and OpenAI use pybullet/Bullet C-API.
+
+The installation of pybullet is as simple as (sudo) pip install pybullet (Python 2.x), pip3 install pybullet. This will expose the pybullet module as well as pybullet_envs Gym environments.
+
+Hello pybullet World
+
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ python
+import pybullet as p
+import pybullet_data
+physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
+p.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF
+p.setGravity(0,0,-10)
+planeId = p.loadURDF("plane.urdf")
+cubeStartPos = [0,0,1]
+cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
+boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
+p.stepSimulation()
+cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
+print(cubePos,cubeOrn)
+p.disconnect()
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+connect, disconnect
+--------------------------------------------------------------------------------------------
+After importing the pybullet module, the first thing to do is 'connecting' to the physics simulation. pybullet is designed around a client-server driven API, with a client sending commands and a physics server returning the status. pybullet has some built-in physics servers: DIRECT and GUI. Both GUI and DIRECT connections will execute the physics simulation and rendering in the same process as pybullet.
+
+Note that in DIRECT mode you cannot access the OpenGL and VR hardware features, as described in the "Virtual Reality" and "Debug GUI, Lines, Text, Parameters" chapters. DIRECT mode does allow rendering of images using the built-in software renderer through the 'getCameraImage' API. This can be useful for running simulations in the cloud on servers without GPU.
+
+You can provide your own data files, or you can use the pybullet_data package that ships with pybullet. For this, import pybullet_data and register the directory using pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()).
+
+getConnectionInfo
+--------------------------------------------------------------------------------------------
+Given a physicsClientId will return the list [isConnected, connectionMethod]
+
+connect using DIRECT, GUI
+--------------------------------------------------------------------------------------------
+
+The DIRECT connection sends the commands directly to the physics engine, without using any transport layer and no graphics visualization window, and directly returns the status after executing the command.
+
+The GUI connection will create a new graphical user interface (GUI) with 3D OpenGL rendering, within the same process space as pybullet. On Linux and Windows this GUI runs in a separate thread, while on OSX it runs in the same thread due to operating system limitations. On Mac OSX you may see a spinning wheel in the OpenGL Window, until you run a 'stepSimulation' or other pybullet command.
+The commands and status messages are sent between pybullet client and the GUI physics simulation server using an ordinary memory buffer.
+
+It is also possible to connect to a physics server in a different process on the same machine or on a remote machine using SHARED_MEMORY, UDP or TCP networking. See the section about Shared Memory, UDP and TCP for details.
+
+Unlike almost all other methods, this method doesn't parse keyword arguments, due to backward compatibility.
+
+The connect input arguments are:
+
+
+| field    | name                            | type                                          | description                                                                                                                                                                                                                                                                                                                                                                            |
+|----------|---------------------------------|-----------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| required | connection mode                 | integer: DIRECT, GUI, SHARED_MEMORY, UDP, TCP | DIRECT mode create a new physics engine and directly communicates with it. GUI will create a physics engine with graphical GUI frontend and communicates with it. SHARED_MEMORY will connect to an existing physics engine process on the same machine, and communicates with it over shared memory. TCP or UDP will connect to an existing physics server over TCP or UDP networking. |
+| optional | key                             | int                                           | in SHARED_MEMORY mode, optional shared memory key. When starting ExampleBrowser or SharedMemoryPhysics_* you can use optional command-line --shared_memory_key to set the key. This allows to run multiple servers on the same machine.                                                                                                                                                |
+| optional | UdpNetworkAddress (UDP and TCP) | string                                        | IP address or host name, for example "127.0.0.1" or "localhost" or "mymachine.domain.com"                                                                                                                                                                                                                                                                                              |
+| optional | UdpNetworkPort (UDP and TCP)    | integer                                       | UDP port number. Default UDP port is 1234, default TCP port is 6667 (matching the defaults in the server)                                                                                                                                                                                                                                                                              |
+| optional | options                         | string                                        | command-line option passed into the GUI server. At the moment, only the --opengl2 flag is enabled: by default, Bullet uses OpenGL3, but some environments such as virtual machines or remote desktop clients only support OpenGL2. Only one command-line argument can be passed on at the moment.                                                                                      |
+
+connect returns a physics client id or -1 if not connected. The physics client Id is an optional argument to most of the other pybullet commands. If you don't provide it, it will assume physics client id = 0. You can connect to multiple different physics servers, except for GUI.
+
+For example:
+
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ python
+pybullet.connect(pybullet.DIRECT)
+pybullet.connect(pybullet.GUI, options="--opengl2")
+pybullet.connect(pybullet.SHARED_MEMORY,1234)
+pybullet.connect(pybullet.UDP,"192.168.0.1")
+pybullet.connect(pybullet.UDP,"localhost", 1234)
+pybullet.connect(pybullet.TCP,"localhost", 6667)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+connect using Shared Memory
+--------------------------------------------------------------------------------------------
+There are a few physics servers that allow shared memory connection: the App_SharedMemoryPhysics, App_SharedMemoryPhysics_GUI and the Bullet Example Browser has one example under Experimental/Physics Server that allows shared memory connection. This will let you execute the physics simulation and rendering in a separate process.
+
+You can also connect over shared memory to the App_SharedMemoryPhysics_VR, the Virtual Reality application with support for head-mounted display and 6-dof tracked controllers such as HTC Vive and Oculus Rift with Touch controllers. Since the Valve OpenVR SDK only works properly under Windows, the App_SharedMemoryPhysics_VR can only be build under Windows using premake (preferably) or cmake.
+
+connect using UDP or TCP networking
+--------------------------------------------------------------------------------------------
+For UDP networking, there is a App_PhysicsServerUDP that listens to a certain UDP port. It uses the open source enet library for reliable UDP networking. This allows you to execute the physics simulation and rendering on a separate machine. For TCP pybullet uses the clsocket library. This can be useful when using SSH tunneling from a machine behind a firewall to a robot simulation. For example you can run a control stack or machine learning using pybullet on Linux, while running the physics server on Windows in Virtual Reality using HTC Vive or Rift.
+
+One more UDP application is the App_PhysicsServerSharedMemoryBridgeUDP application that acts as a bridge to an existing physics server: you can connect over UDP to this bridge, and the bridge connects to a physics server using shared memory: the bridge passes messages between client and server. In a similar way there is a TCP version (replace UDP by TCP).
+
+Note: at the moment, both client and server need to be either 32bit or 64bit builds!
+
+disconnect
+--------------------------------------------------------------------------------------------
+You can disconnect from a physics server, using the physics client Id returned by the connect call (if non-negative). A 'DIRECT' or 'GUI' physics server will shutdown. A separate (out-of-process) physics server will keep on running. See also 'resetSimulation' to remove all items.
+
+Parameters of disconnect:
+
+| field    | name            | type | description                                                         |
+|----------|-----------------|------|---------------------------------------------------------------------|
+| optional | physicsClientId | int  | if you connect to multiple physics servers, you can pick which one. |
+
+setGravity
+--------------------------------------------------------------------------------------------
+By default, there is no gravitational force enabled. setGravity lets you set the default gravity force for all objects.
+The setGravity input parameters are: (no return value)
+
+| field    | name            | type  | description                                                         |
+|----------|-----------------|-------|---------------------------------------------------------------------|
+| required | gravityX        | float | gravity force along the X world axis                                |
+| required | gravityY        | float | gravity force along the Y world axis                                |
+| required | gravityZ        | float | gravity force along the Z world axis                                |
+| required | physicsClientId | int   | if you connect to multiple physics servers, you can pick which one. |
+
+
+loadURDF
+--------------------------------------------------------------------------------------------
+The loadURDF will send a command to the physics server to load a physics model from a Universal Robot Description File (URDF). The URDF file is used by the ROS project (Robot Operating System) to describe robots and other objects, it was created by the WillowGarage and the Open Source Robotics Foundation (OSRF). Many robots have public URDF files, you can find a description and tutorial here: http://wiki.ros.org/urdf/Tutorials
+
+Important note: most joints (slider, revolute, continuous) have motors enabled by default that prevent free motion. This is similar to a robot joint with a very high-friction harmonic drive. You should set the joint motor control mode and target settings using pybullet.setJointMotorControl2. See the setJointMotorControl2 API for more information.
+
+Warning: by default, pybullet will cache some files to speed up loading. You can disable file caching using setPhysicsEngineParameter(enableFileCaching=0).
+
+The loadURDF arguments are:
+
+
+| option   | name                  | type   | description                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     |
+|----------|-----------------------|--------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| required | fileName              | string | a relative or absolute path to the URDF file on the file system of the physics server.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          |
+| optional | basePosition          | vec3   | create the base of the object at the specified position in world space coordinates [X,Y,Z]                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      |
+| optional | baseOrientation       | vec4   | create the base of the object at the specified orientation as world space quaternion [X,Y,Z,W]                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  |
+| optional | useMaximalCoordinates | int    | Experimental. By default, the joints in the URDF file are created using the reduced coordinate method: the joints are simulated using the Featherstone Articulated Body algorithm (btMultiBody in Bullet 2.x). The useMaximalCoordinates option will create a 6 degree of freedom rigid body for each link, and constraints between those rigid bodies are used to model joints.                                                                                                                                                                                                                                                                                                |
+| optional | useFixedBase          | int    | force the base of the loaded object to be static                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                |
+| optional | flags                 | int    | URDF_USE_INERTIA_FROM_FILE: by default, Bullet recomputed the inertia tensor based on mass and volume of the collision shape. If you can provide more accurate inertia tensor, use this flag.URDF_USE_SELF_COLLISION: by default, Bullet disables self-collision. This flag let's you enable it. You can customize the self-collision behavior using the following flags: URDF_USE_SELF_COLLISION_EXCLUDE_PARENT will discard self-collision between links that are directly connected (parent and child).URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS will discard self-collisions between a child link and any of its ancestors (parents, parents of parents, up to the base). |
+| optional | globalScaling         | float  | globalScaling will apply a scale factor to the URDF model.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      |
+| optional | physicsClientId       | int    | if you are connected to multiple servers, you can pick one.                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     |
+loadURDF returns a body unique id, a non-negative integer value. If the URDF file cannot be loaded, this integer will be negative and not a valid body unique id.
+
+loadBullet, loadSDF, loadMJCF
+--------------------------------------------------------------------------------------------
+You can also load objects from other file formats, such as .bullet, .sdf and .mjcf. Those file formats support multiple objects, so the return value is a list of object unique ids. The SDF format is explained in detail at http://sdformat.org. The loadSDF command only extracts some essential parts of the SDF related to the robot models and geometry, and ignores many elements related to cameras, lights and so on. The loadMJCF command performs basic import of MuJoCo MJCF xml files, used in OpenAI Gym. See also the Important note under loadURDF related to default joint motor settings, and make sure to use setJointMotorControl2.
+
+
+| option   | name                  | type   | description                                                                                                                   |
+|----------|-----------------------|--------|-------------------------------------------------------------------------------------------------------------------------------|
+| required | fileName              | string | a relative or absolute path to the URDF file on the file system of the physics server.                                        |
+| optional | useMaximalCoordinates | int    | Experimental. See loadURDF for more details.                                                                                  |
+| optional | globalScaling         | float  | every object will be scaled using this scale factor (including links, link frames, joint attachments and linear joint limits) |
+| optional | physicsClientId       | int    | if you are connected to multiple servers, you can pick one.                                                                   |
+
+loadBullet, loadSDF and loadMJCF will return an array of object unique ids:
+
+| name            | type        | description                                                    |
+|-----------------|-------------|----------------------------------------------------------------|
+| objectUniqueIds | list of int | the list includes the object unique id for each object loaded. |
+
+
+createCollisionShape/VisualShape
+--------------------------------------------------------------------------------------------
+Although the recommended and easiest way to create stuff in the world is using the loading functions (loadURDF/SDF/MJCF/Bullet), you can also create collision and visual shapes programmatically and use them to create a multi body using createMultiBody. See the createMultiBodyLinks.py and createVisualShape.py example in the Bullet Physics SDK.
+
+The input parameters for createCollisionShape are
+
+| option   | name            | type                  | description                                                                                                                                                                            |
+|----------|-----------------|-----------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| required | shapeType       | int                   | GEOM_SPHERE, GEOM_BOX, GEOM_CAPSULE, GEOM_CYLINDER, GEOM_PLANE, GEOM_MESH                                                                                                              |
+| optional | radius          | float                 | default 0.5: GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER                                                                                                                                  |
+| optional | halfExtents     | vec3 list of 3 floats | default [1,1,1]: for GEOM_BOX                                                                                                                                                          |
+| optional | height          | float                 | default: 1: for GEOM_CAPSULE, GEOM_CYLINDER                                                                                                                                            |
+| optional | fileName        | string                | Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each object (marked as 'o') in the .obj file.                                                      |
+| optional | meshScale       | vec3 list of 3 floats | default: [1,1,1], for GEOM_MESH                                                                                                                                                        |
+| optional | planeNormal     | vec3 list of 3 floats | default: [0,0,1] for GEOM_PLANE                                                                                                                                                        |
+| optional | flags           | int                   | GEOM_FORCE_CONCAVE_TRIMESH: for GEOM_MESH, this will create a concave static triangle mesh. This should not be used with dynamic / moving objects, only for static (mass = 0) terrain. |
+| optional | physicsClientId | int                   | If you are connected to multiple servers, you can pick one.                                                                                                                            |
+
+The return value is a non-negative int unique id for the collision shape or -1 if the call failed.
+
+createVisualShape
+--------------------------------------------------------------------------------------------
+You can create a visual shape in a similar way to creating a collision shape, with some additional arguments to control the visual appearance, such as diffuse and specular color. When you use the GEOM_MESH type, you can point to a Wavefront OBJ file, and the visual shape will parse some parameters from the material file (.mtl) and load a texture. Note that large textures (above 1024x1024 pixels) can slow down the loading and run-time performance.
+
+The input parameters are
+
+| option   | name                   | type                   | description                                                                                                                       |
+|----------|------------------------|------------------------|-----------------------------------------------------------------------------------------------------------------------------------|
+| required | shapeType              | int                    | GEOM_SPHERE, GEOM_BOX, GEOM_CAPSULE, GEOM_CYLINDER, GEOM_PLANE, GEOM_MESH                                                         |
+| optional | radius                 | float                  | default 0.5: GEOM_SPHERE, GEOM_CAPSULE, GEOM_CYLINDER                                                                             |
+| optional | halfExtents            | vec3 list of 3 floats  | default [1,1,1]: for GEOM_BOX                                                                                                     |
+| optional | height                 | float                  | default: 1: for GEOM_CAPSULE, GEOM_CYLINDER                                                                                       |
+| optional | fileName               | string                 | Filename for GEOM_MESH, currently only Wavefront .obj. Will create convex hulls for each object (marked as 'o') in the .obj file. |
+| optional | meshScale              | vec3 list of 3 floats  | default: [1,1,1], for GEOM_MESH                                                                                                   |
+| optional | planeNormal            | vec3 list of 3 floats  | default: [0,0,1] for GEOM_PLANE                                                                                                   |
+| optional | flags                  | int                    | unused at the moment                                                                                                              |
+| optional | rgbaColor              | vec4, list of 4 floats | color components for red, green, blue and alpha, each in range [0..1].                                                            |
+| optional | specularColor          | vec3, list of 3 floats | specular reflection color, red, green, blue components in range [0..1]                                                            |
+| optional | visualFramePosition    | vec3, list of 3 floats | translational offset of the visual shape with respect to the link frame                                                           |
+| optional | visualFrameOrientation | vec4, list of 4 floats | rotational offset (quaternion x,y,z,w) of the visual shape with respect to the link frame                                         |
+| optional | physicsClientId        | int                    | If you are connected to multiple servers, you can pick one.                                                                       |
+
+The return value is a non-negative int unique id for the visual shape or -1 if the call failed.
+
+createMultiBody
+--------------------------------------------------------------------------------------------
+Although the easiest way to create stuff in the world is using the loading functions (loadURDF/SDF/MJCF/Bullet), you can create a multi body using createMultiBody. 
+See the createMultiBodyLinks.py example in the Bullet Physics SDK. The parameters of createMultiBody are very similar to URDF and SDF parameters.
+
+You can create a multi body with only a single base without joints/child links or you can create a multi body with joints/child links. If you provide links, make sure the size of every list is the same (len(linkMasses) == len(linkCollisionShapeIndices) etc). The input parameters for createMultiBody are:
+
+| option   | name                          | type                   | description                                                                                                               |
+|----------|-------------------------------|------------------------|---------------------------------------------------------------------------------------------------------------------------|
+| optional | baseMass                      | float                  | mass of the base, in kg (if using SI units)                                                                               |
+| optional | baseCollisionShapeIndex       | int                    | unique id from createCollisionShape or -1. You can re-use the collision shape for multiple multibodies (instancing)       |
+| optional | baseVisualShapeIndex          | int                    | unique id from createVisualShape or -1. You can reuse the visual shape (instancing)                                       |
+| optional | basePosition                  | vec3, list of 3 floats | Cartesian world position of the base                                                                                      |
+| optional | baseOrientation               | vec4, list of 4 floats | Orientation of base as quaternion [x,y,z,w]                                                                               |
+| optional | baseInertialFramePosition     | vec3, list of 3 floats | Local position of inertial frame                                                                                          |
+| optional | baseInertialFrameOrientation  | vec4, list of 4 floats | Local orientation of inertial frame, [x,y,z,w]                                                                            |
+| optional | linkMasses                    | list of float          | List of the mass values, one for each link.                                                                               |
+| optional | linkCollisionShapeIndices     | list of int            | List of the unique id, one for each link.                                                                                 |
+| optional | linkVisualShapeIndices        | list of int            | list of the visual shape unique id for each link                                                                          |
+| optional | linkPositions                 | list of vec3           | list of local link positions, with respect to parent                                                                      |
+| optional | linkOrientations              | list of vec4           | list of local link orientations, w.r.t. parent                                                                            |
+| optional | linkInertialFramePositions    | list of vec3           | list of local inertial frame pos. in link frame                                                                           |
+| optional | linkInertialFrameOrientations | list of vec4           | list of local inertial frame orn. in link frame                                                                           |
+| optional | linkParentIndices             | list of int            | Link index of the parent link or 0 for the base.                                                                          |
+| optional | linkJointTypes                | list of int            | list of joint types, one for each link. Only JOINT_REVOLUTE, JOINT_PRISMATIC, and JOINT_FIXED is supported at the moment. |
+| optional | linkJointAxis                 | list of vec3           | Joint axis in local frame                                                                                                 |
+| optional | useMaximalCoordinates         | int                    | experimental, best to leave it 0/false.                                                                                   |
+| optional | physicsClientId               | int                    | If you are connected to multiple servers, you can pick one.                                                               |
+
+
+The return value of createMultiBody is a non-negative unique id or -1 for failure. Example:
+
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ python
+cuid = pybullet.createCollisionShape(pybullet.GEOM_BOX, halfExtents = [1, 1, 1])
+mass= 0 #static box
+pybullet.createMultiBody(mass,cuid)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+See also createMultiBodyLinks.py, createObstacleCourse.py and createVisualShape.py in the Bullet/examples/pybullet/examples folder.
+
+saveWorld
+--------------------------------------------------------------------------------------------
+You can create a snapshot of the current world as a pybullet Python file, stored on the server. saveWorld can be useful as a basic editing feature, setting up the robot, joint angles, object positions and environment for example in VR. Later you can just load the pybullet Python file to re-create the world. Note that not all settings are stored in the world file at the moment.
+The input arguments are:
+
+| option   | name           | type   | description                                                |
+|----------|----------------|--------|------------------------------------------------------------|
+| required | fileName       | string | filename of the pybullet file.                             |
+| optional | clientServerId | int    | if you are connected to multiple servers, you can pick one |
+
+stepSimulation
+--------------------------------------------------------------------------------------------
+stepSimulation will perform all the actions in a single forward dynamics simulation step such as collision detection, constraint solving and integration. 
+
+stepSimulation input arguments are optional:
+
+| option   | name            | type | description                                                 |
+|----------|-----------------|------|-------------------------------------------------------------|
+| optional | physicsClientId | int  | if you are connected to multiple servers, you can pick one. |
+
+
+stepSimulation has no return values.
+
+See also setRealTimeSimulation to automatically let the physics server run forward dynamics simulation based on its real-time clock.
+
+setRealTimeSimulation
+--------------------------------------------------------------------------------------------
+
+By default, the physics server will not step the simulation, unless you explicitly send a 'stepSimulation' command. This way you can maintain control determinism of the simulation. It is possible to run the simulation in real-time by letting the physics server automatically step the simulation according to its real-time-clock (RTC) using the setRealTimeSimulation command. If you enable the real-time simulation, you don't need to call 'stepSimulation'.
+
+Note that setRealTimeSimulation has no effect in DIRECT mode: in DIRECT mode the physics server and client happen in the same thread and you trigger every command. In GUI mode and in Virtual Reality mode, and TCP/UDP mode, the physics server runs in a separate thread from the client (pybullet), and setRealTimeSimulation allows the physicsserver  thread to add additional calls to stepSimulation.
+
+
+The input parameters are:
+
+| option   | name                     | type | description                                                 |
+|----------|--------------------------|------|-------------------------------------------------------------|
+| required | enableRealTimeSimulation | int  | 0 to disable real-time simulation, 1 to enable              |
+| optional | physicsClientId          | int  | if you are connected to multiple servers, you can pick one. |
+
+
+getBasePositionAndOrientation
+--------------------------------------------------------------------------------------------
+getBasePositionAndOrientation reports the current position and orientation of the base (or root link) of the body in Cartesian world coordinates. The orientation is a quaternion in [x,y,z,w] format.
+
+The getBasePositionAndOrientation input parameters are:
+
+| option   | name                     | type | description                                                 |
+|----------|--------------------------|------|-------------------------------------------------------------|
+| required | objectUniqueId           | int  | object unique id, as returned from loadURDF.                |
+| optional | physicsClientId          | int  | if you are connected to multiple servers, you can pick one. |
+
+
+getBasePositionAndOrientation returns the position list of 3 floats and orientation as list of 4 floats in [x,y,z,w] order. Use getEulerFromQuaternion to convert the quaternion to Euler if needed.
+
+See also resetBasePositionAndOrientation to reset the position and orientation of the object.
+
+This completes the first pybullet script. Bullet ships with several URDF files in the Bullet/data folder.
+
+resetBasePositionAndOrientation
+--------------------------------------------------------------------------------------------
+You can reset the position and orientation of the base (root) of each object. It is best only to do this at the start, and not during a running simulation, since the command will override the effect of all physics simulation. The linear and angular velocity is set to zero. You can use resetBaseVelocity to reset to a non-zero linear and/or angular velocity.
+
+The input arguments to resetBasePositionAndOrientation are:
+
+| required | objectUniqueId  | int  | object unique id, as returned from loadURDF.                                                  |
+|----------|-----------------|------|-----------------------------------------------------------------------------------------------|
+| required | posObj          | vec3 | reset the base of the object at the specified position in world space coordinates [X,Y,Z]     |
+| required | ornObj          | vec4 | reset the base of the object at the specified orientation as world space quaternion [X,Y,Z,W] |
+| optional | physicsClientId | int  | if you are connected to multiple servers, you can pick one.                                   |
+
+There are no return arguments.
+
+Transforms: Position and Orientation
+--------------------------------------------------------------------------------------------
+The position of objects can be expressed in Cartesian world space coordinates [x,y,z]. The orientation (or rotation) of objects can be expressed using quaternions [x,y,z,w], euler angles [yaw, pitch, roll] or 3x3 matrices. pybullet provides a few helper functions to convert between quaternions, euler angles and 3x3 matrices. In additions there are some functions to multiply and invert transforms.
+
+getQuaternionFromEuler and getEulerFromQuaternion
+--------------------------------------------------------------------------------------------
+The pybullet API uses quaternions to represent orientations. Since quaternions are not very intuitive for people, there are two APIs to convert between quaternions and Euler angles.
+The getQuaternionFromEuler input arguments are:
+
+
+| option   | name            | type                   | description                                                                                                                                 |
+|----------|-----------------|------------------------|---------------------------------------------------------------------------------------------------------------------------------------------|
+| required | eulerAngle      | vec3: list of 3 floats | The X,Y,Z Euler angles are in radians, accumulating 3 rotations expressing the roll around the X, pitch around Y and yaw around the Z axis. |
+| optional | physicsClientId | int                    | unused, added for API consistency.                                                                                                          |
+
+
+getQuaternionFromEuler returns a quaternion, vec4 list of 4 floating point values [X,Y,Z,W].
+
+getEulerFromQuaternion
+--------------------------------------------------------------------------------------------
+The getEulerFromQuaternion input arguments are:
+
+| option   | name            | type                   | description                        |
+|----------|-----------------|------------------------|------------------------------------|
+| required | quaternion      | vec4: list of 4 floats | The quaternion format is [x,y,z,w] |
+| optional | physicsClientId | int                    | unused, added for API consistency. |
+
+getEulerFromQuaternion returns alist of 3 floating point values, a vec3.
+
+getMatrixFromQuaternion 
+--------------------------------------------------------------------------------------------
+getMatrixFromQuaternion is a utility API to create a 3x3 matrix from a quaternion. The input is a quaternion and output a list of 9 floats, representing the matrix.
+
+multiplyTransforms, invertTransform
+--------------------------------------------------------------------------------------------
+pybullet provides a few helper functions to multiply and inverse transforms. This can be helpful to transform coordinates from one to the other coordinate system.
+
+The input parameters of multiplyTransforms are:
+
+| option   | name            | type                   | description                        |
+|----------|-----------------|------------------------|------------------------------------|
+| required | positionA       | vec3, list of 3 floats | position A                         |
+| required | orientationA    | vec4, list of 4 floats | quaternion [x,y,z,w]               |
+| required | positionB       | vec3, list of 3 floats | position B                         |
+| required | orientationB    | vec4, list of 4 floats | quaternion [x,y,z,w]               |
+| optional | physicsClientId | int                    | unused, added for API consistency. |
+
+The return value is a list of position (vec3) and orientation (vec4, quaternion x,y,x,w).
+
+The input and output parameters of invertTransform are:
+
+| option   | name        | type                   | description          |
+|----------|-------------|------------------------|----------------------|
+| required | position    | vec3, list of 3 floats |                      |
+| required | orientation | vec4, list of 4 floats | quaternion [x,y,z,w] |
+
+
+The output of invertTransform is a position (vec3) and orientation (vec4, quaternion x,y,x,w).
+getAPIVersion
+You can query for the API version in a year-month-0-day format. You can only connect between physics client/server of the same API version, with the same number of bits (32-bit / 64bit). There is a optional unused argument physicsClientId, added for API consistency.
+
+| option   | name            | type | description                        |
+|----------|-----------------|------|------------------------------------|
+| optional | physicsClientId | int  | unused, added for API consistency. |
+
+Controlling a robot
+======================================================================================
+
+In the Introduction we already showed how to initialize pybullet and load some objects. If you replace the file name in the loadURDF command with "r2d2.urdf" you can simulate a R2D2 robot from the ROS tutorial. Let's control this R2D2 robot to move, look around and control the gripper. For this we need to know how to access its joint motors.
+
+Base, Joints, Links
+--------------------------------------------------------------------------------------------
+
+![<small>Base, Joints and Links</small>](images/joints_and_links.png width="80%" border="0")
+
+A simulated robot as described in a URDF file has a base, and optionally links connected by joints. Each joint connects one parent link to a child link. At the root of the hierarchy there is a single root parent that we call base. The base can be either fully fixed, 0 degrees of freedom, or fully free, with 6 degrees of freedom. Since each link is connected to a parent with a single joint, the number of joints is equal to the number of links. Regular links have link indices in the range [0..getNumJoints()] Since the base is not a regular 'link', we use the convention of -1 as its link index. We use the convention that joint frames are expressed relative to the parents center of mass inertial frame, which is aligned with the principle axis of inertia.
+
+getNumJoints, getJointInfo
+--------------------------------------------------------------------------------------------
+After you load a robot you can query the number of joints using the getNumJoints API. For the r2d2.urdf this should return 15.
+
+getNumJoints input parameters:
+
+| option   | name            | type | description                                                 |
+|----------|-----------------|------|-------------------------------------------------------------|
+| required | bodyUniqueId    | int  | the body unique id, as returned by loadURDF etc.            |
+| optional | physicsClientId | int  | if you are connected to multiple servers, you can pick one. |
+
+getNumJoints returns an integer value representing the number of joints.
+
+getJointInfo
+--------------------------------------------------------------------------------------------
+For each joint we can query some information, such as its name and type.
+
+getJointInfo input parameters:
+
+| option   | name            | type | description                                                 |
+|----------|-----------------|------|-------------------------------------------------------------|
+| required | bodyUniqueId    | int  | the body unique id, as returned by loadURDF etc.            |
+| required | jointIndex      | int  | an index in the range [0 .. getNumJoints(bodyUniqueId))     |
+| optional | physicsClientId | int  | if you are connected to multiple servers, you can pick one. |
+
+getJointInfo returns a list of information:
+
+| name             | type   | description                                                                                                                                                                                                                   |
+|------------------|--------|-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| jointIndex       | int    | the same joint index as the input parameter                                                                                                                                                                                   |
+| jointName        | string | the name of the joint, as specified in the URDF (or SDF etc) file                                                                                                                                                             |
+| jointType        | int    | "type of the joint, this also implies the number of position and velocity variables. JOINT_REVOLUTE, JOINT_PRISMATIC, JOINT_SPHERICAL, JOINT_PLANAR, JOINT_FIXED. See the section on Base, Joint and Links for more details." |
+| qIndex           | int    | the first position index in the positional state variables for this body                                                                                                                                                      |
+| uIndex           | int    | the first velocity index in the velocity state variables for this body                                                                                                                                                        |
+| flags            | int    | reserved                                                                                                                                                                                                                      |
+| jointDamping     | float  | the joint damping value, as specified in the URDF file                                                                                                                                                                        |
+| jointFriction    | float  | the joint friction value, as specified in the URDF file                                                                                                                                                                       |
+| jointLowerLimit  | float  | Positional lower limit for slider and revolute (hinge) joints.                                                                                                                                                                |
+| jointUpperLimit  | float  | Positional upper limit for slider and revolute joints. Values ignored in case upper limit smaller than lower limit..                                                                                                                       |
+| jointMaxForce    | float  | Maximum force specified in URDF (possibly other file formats) Note that this value is not automatically used. You can use maxForce in 'setJointMotorControl2'.                                                                |
+| jointMaxVelocity | float  | Maximum velocity specified in URDF. Note that the maximum velocity is not used in actual motor control commands at the moment.                                                                                                |
+| linkName         | string | the name of the link, as specified in the URDF (or SDF etc.) file                                                                                                                                                             |
+
+setJointMotorControl2/Array
+--------------------------------------------------------------------------------------------
+Note: setJointMotorControl is obsolete and replaced by setJointMotorControl2 API. (Or even better use setJointMotorControlArray).
+
+We can control a robot by setting a desired control mode for one or more joint motors. During the stepSimulation the physics engine will simulate the motors to reach the given target value that can be reached within the maximum motor forces and other constraints. Each revolute joint and prismatic joint is motorized by default. There are 3 different motor control modes: position control, velocity control and torque control.
+
+You can effectively disable the motor by using a force of 0. You need to disable motor in order to use direct torque control. For example:
+
+
+
+
+<!--Hello MathJax:
+\[f(0)=\frac{1}{2\cdot\pi\cdot i}\cdot \oint_{|z|=1} \frac{f(z)}{z} \textrm{d}z\]
+-->
+
+
+<script window.markdeepOptions={};
+window.markdeepOptions.tocStyle="long"; src="cs371-common.js"></script>
+

thirdparty/bullet/docs/pybullet_quickstart_guide/WordpressPreview/Article.md.html

@@ -0,0 +1,10 @@
+<meta charset="utf-8" lang="en">
+Hello Markdeep!
+
+- Hello
+- lists!
+
+Hello MathJax:
+\[f(0)=\frac{1}{2\cdot\pi\cdot i}\cdot \oint_{|z|=1} \frac{f(z)}{z} \textrm{d}z\]
+
+<script src="MarkdeepUtility.js"></script>

thirdparty/bullet/docs/pybullet_quickstart_guide/WordpressPreview/BuildMarkdeepUtility.py

@@ -0,0 +1,44 @@
+import re
+
+if (__name__ == "__main__"):
+  # Assemble the script which embeds the Markdeep page into the preview blog
+  PreviewBlogPage = open("PreviewBlogPage.htm", "rb").read().decode("utf-8")
+  HeadMatch = re.search("<head(.*?)>(.*?)</head>", PreviewBlogPage, re.DOTALL)
+  HeadAttributes = HeadMatch.group(1)
+  FullDocumentHead = HeadMatch.group(2)
+  BodyMatch = re.search("<body(.*?)>(.*?)</body>", PreviewBlogPage, re.DOTALL)
+  BodyAttributes = BodyMatch.group(1)
+  FullPreviewBody = BodyMatch.group(2)
+  ArticleHTMLCodeMacro = "$(ARTICLE_HTML_CODE)"
+  iArticleHTMLCodeMacro = FullPreviewBody.find(ArticleHTMLCodeMacro)
+  DocumentBodyPrefix = FullPreviewBody[0:iArticleHTMLCodeMacro]
+  DocumentBodySuffix = FullPreviewBody[iArticleHTMLCodeMacro + len(ArticleHTMLCodeMacro):]
+  FullPrepareHTMLCode = open("PrepareHTML.js", "rb").read().decode("utf-8")
+  ReplacementList = [("$(FULL_DOCUMENT_HEAD)", FullDocumentHead),
+                     ("$(DOCUMENT_BODY_PREFIX)", DocumentBodyPrefix),
+                     ("$(DOCUMENT_BODY_SUFFIX)", DocumentBodySuffix)]
+  for Macro, Replacement in ReplacementList:
+    FullPrepareHTMLCode = FullPrepareHTMLCode.replace(
+        Macro,
+        Replacement.replace("\r\n", "\\r\\n\\\r\n").replace("'", "\\'"))
+  # Generate code which sets body and head attributes appropriately
+  for Element, AttributeCode in [("head", HeadAttributes), ("body", BodyAttributes)]:
+    FullPrepareHTMLCode += "\r\n// Setting " + Element + " attributes\r\n"
+    for Match in re.finditer("(\\w+)=\\\"(.*?)\\\"", AttributeCode):
+      FullPrepareHTMLCode += "document." + Element + ".setAttribute(\"" + Match.group(
+          1) + "\",\"" + Match.group(2) + "\");\r\n"
+  open("PrepareHTML.full.js", "wb").write(FullPrepareHTMLCode.encode("utf-8"))
+
+  # Concatenate all the scripts together
+  SourceFileList = [
+      "PrepareHTML.full.js", "SetMarkdeepMode.js", "markdeep.min.js", "DisplayMarkdeepOutput.js",
+      "InvokeMathJax.js"
+  ]
+  OutputCode = "\r\n\r\n".join([
+      "// " + SourceFile + "\r\n\r\n" + open(SourceFile, "rb").read().decode("utf-8")
+      for SourceFile in SourceFileList
+  ])
+  OutputFile = open("MarkdeepUtility.js", "wb")
+  OutputFile.write(OutputCode.encode("utf-8"))
+  OutputFile.close()
+  print("Done.")

thirdparty/bullet/docs/pybullet_quickstart_guide/WordpressPreview/DisplayMarkdeepOutput.js

@@ -0,0 +1,6 @@
+BodyHTML=document.body.innerHTML;
+BeginTag="<!-- MARKDEEP_BEGIN -->";
+EndTag="<!-- MARKDEEP_END -->";
+BodyHTML=BodyHTML.slice(BodyHTML.indexOf(BeginTag)+BeginTag.length,BodyHTML.lastIndexOf(EndTag));
+document.getElementById("BodyDisplayBox").textContent=BodyHTML;
+document.head.innerHTML=FullDocumentHead;

thirdparty/bullet/docs/pybullet_quickstart_guide/WordpressPreview/InvokeMathJax.js

@@ -0,0 +1,4 @@
+var MathjaxScript=document.createElement("script");
+MathjaxScript.type="text/javascript";
+MathjaxScript.src="https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML";
+document.head.appendChild(MathjaxScript);

thirdparty/bullet/docs/pybullet_quickstart_guide/WordpressPreview/PrepareHTML.js

@@ -0,0 +1,102 @@
+/** Converts <>&" to their HTML escape sequences */
+function escapeHTMLEntities(str) {
+    return String(str).replace(/&/g, '&amp;').replace(/</g, '&lt;').replace(/>/g, '&gt;').replace(/"/g, '&quot;');
+}
+
+
+/** Restores the original source string's '<' and '>' as entered in
+    the document, before the browser processed it as HTML. There is no
+    way in an HTML document to distinguish an entity that was entered
+    as an entity.*/
+function unescapeHTMLEntities(str) {
+    // Process &amp; last so that we don't recursively unescape
+    // escaped escape sequences.
+    return str.
+        replace(/&lt;/g, '<').
+        replace(/&gt;/g, '>').
+        replace(/&quot;/g, '"').
+        replace(/&#39;/g, "'").
+        replace(/&ndash;/g, '--').
+        replace(/&mdash;/g, '---').
+        replace(/&amp;/g, '&');
+}
+
+
+/**
+   \param node  A node from an HTML DOM
+
+   \return A String that is a very good reconstruction of what the
+   original source looked like before the browser tried to correct
+   it to legal HTML.
+ */
+function nodeToMarkdeepSource(node, leaveEscapes) {
+    var source = node.innerHTML;
+
+    // Markdown uses <john@bar.com> e-mail syntax, which HTML parsing
+    // will try to close by inserting the matching close tags at the end of the
+    // document. Remove anything that looks like that and comes *after*
+    // the first fallback style.
+    source = source.replace(/(?:<style class="fallback">[\s\S]*?<\/style>[\s\S]*)<\/\S+@\S+\.\S+?>/gim, '');
+    
+    // Remove artificially inserted close tags
+    source = source.replace(/<\/h?ttps?:.*>/gi, '');
+    
+    // Now try to fix the URLs themselves, which will be 
+    // transformed like this: <http: casual-effects.com="" markdeep="">
+    source = source.replace(/<(https?): (.*?)>/gi, function (match, protocol, list) {
+
+        // Remove any quotes--they wouldn't have been legal in the URL anyway
+        var s = '<' + protocol + '://' + list.replace(/=""\s/g, '/');
+
+        if (s.substring(s.length - 3) === '=""') {
+            s = s.substring(0, s.length - 3);
+        }
+
+        // Remove any lingering quotes (since they
+        // wouldn't have been legal in the URL)
+        s = s.replace(/"/g, '');
+
+        return s + '>';
+    });
+
+    // Remove the "fallback" style tags
+    source = source.replace(/<style class=["']fallback["']>.*?<\/style>/gmi, '');
+
+    source = unescapeHTMLEntities(source);
+
+    return source;
+}
+
+// $ (FULL_DOCUMENT_HEAD) is replaced by the contents of the <head> found in 
+// PreviewBlogPage.htm. This document head will overwrite whatever Markdeep does to 
+// the head at the very end.
+FullDocumentHead='\
+$(FULL_DOCUMENT_HEAD)\
+';
+
+// This code is placed at the beginning of the body before the Markdeep code. 
+// $ (DOCUMENT_BODY_PREFIX) is everything in the body of PreviewBlogPage.htm up to 
+// $ (ARTICLE_HTML_CODE).
+DocumentBodyPrefix='\
+$(DOCUMENT_BODY_PREFIX)\
+	<!-- MARKDEEP_BEGIN -->\
+	<pre class="markdeep">\
+';
+// This code is placed at the end of the body after the Markdeep code. 
+// $ (DOCUMENT_BODY_SUFFIX) is everything in the body of PreviewBlogPage.htm after 
+// $ (ARTICLE_HTML_CODE).
+DocumentBodySuffix='\
+	</pre>\
+	<!-- MARKDEEP_END -->\
+	<div>Document &lt;body&gt; code:<br/>\
+	<textarea cols="40" rows="10" id="BodyDisplayBox"></textarea></div>\
+$(DOCUMENT_BODY_SUFFIX)\
+';
+
+// Get the full Markdeep code from the .md.html file without the script invocation
+MarkdeepCode=nodeToMarkdeepSource(document.body);
+MarkdeepCode=MarkdeepCode.slice(0,MarkdeepCode.lastIndexOf("<script"));
+// Bring it into a form where it can be pasted into an HTML document
+SanitizedMarkdeepCode=escapeHTMLEntities(MarkdeepCode);
+// Surround it by the prefix and suffix code and set that as body code
+document.body.innerHTML=DocumentBodyPrefix+SanitizedMarkdeepCode+DocumentBodySuffix;

thirdparty/bullet/docs/pybullet_quickstart_guide/WordpressPreview/SetMarkdeepMode.js

@@ -0,0 +1 @@
+window.markdeepOptions={mode:"html"};

thirdparty/bullet/docs/pybullet_quickstart_guide/WordpressPreview/ShowHeadAdditions.html

@@ -0,0 +1,29 @@
+<!DOCTYPE html>
+<html lang="en-US">
+	<head>
+		<meta http-equiv="content-type" content="text/html; charset=UTF-8">
+		<meta charset="UTF-8">
+	<body>
+		<script>window.markdeepOptions={mode:"script"};</script>
+		<script src="./markdeep.min.js"></script>
+		<div>Document &lt;head&gt; additions:<br/>
+		<textarea cols="80" rows="40" id="HeadDisplayBox"></textarea></div>
+		<script>
+			// Get the complete Markdeep and hljs stylesheet
+			FullStylesheet=window.markdeep.stylesheet();
+			// It should consist of three <style> tags which we pick apart
+			StyleTagList=FullStylesheet.split("<style>");
+			// The second one defines section number, which we do not want, so we 
+			// just drop it. The other two are Markdeep and hljs styles.
+			MarkdeepStylesheet="<style>"+StyleTagList[1];
+			HLJSStylesheet="<style>"+StyleTagList[3];
+			// Now lets show the user what we found
+			document.getElementById("HeadDisplayBox").textContent=
+				 "<!-- Markdeep styles -->\r\n"
+				+MarkdeepStylesheet
+				+"\r\n\r\n<!-- hljs styles -->\r\n"
+				+HLJSStylesheet
+				+"\r\n\r\n<!-- MathJax invocation -->\r\n<script type=\"text/javascript\" async=\"\" src=\"https://cdn.mathjax.org/mathjax/latest/MathJax.js?config=TeX-AMS-MML_HTMLorMML\">\r\n";
+		</script>
+	</body>
+</html>

thirdparty/bullet/docs/pybullet_quickstart_guide/cs371-common.js

@@ -0,0 +1,24 @@
+
+// Common code for styling all CS371 web pages
+
+if (location.href.indexOf('?') == -1) {
+    // Force a reload by making a bogus query.
+    var i = ((location.href.indexOf('#') + 1) || (location.href.length + 1)) - 1;
+    location = location.href.substring(0, i) + '?' + location.href.substring(i);
+}
+
+markdeepOptions = {tocStyle: 'long'};
+
+document.write("<link href='https://fonts.googleapis.com/css?family=Roboto:400,300,100,100italic,300italic,400italic,700' rel='stylesheet' type='text/css'>" +
+               "<link rel='stylesheet' type='text/css' href='cs371.css'>");
+
+// Conceal list items that use a '+', except when viewed locally
+if (/^file:\/{3}/i.test(window.location.href)) {
+    document.write('<style>li.plus { color: #D88; }</style>');
+} else {
+    document.write('<style>li.plus { display: none; }</style>');
+}
+
+document.write('<!-- Markdeep: --><style class="fallback">body{visibility:hidden;white-space:pre;font-family:monospace}</style><script src="markdeep.js"></script><script>window.alreadyProcessedMarkdeep||(document.body.style.visibility="visible")</script>');
+
+

thirdparty/bullet/docs/pybullet_quickstart_guide/cs371.css

@@ -0,0 +1,21 @@
+.md table  {
+  font-size: 6px;
+}
+
+body {
+    font-family: "Roboto", sans-serif;
+    background: #FFFDF9;
+}
+
+body, table {
+    font-weight: 300;
+    color: #555;
+}
+
+
+em.asterisk { 
+    font-style: normal; 
+    font-weight: 500;
+   /* color: #333;  */
+}
+

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3BroadphaseCallback.h

@@ -0,0 +1,38 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_BROADPHASE_CALLBACK_H
+#define B3_BROADPHASE_CALLBACK_H
+
+#include "Bullet3Common/b3Vector3.h"
+struct b3BroadphaseProxy;
+
+struct b3BroadphaseAabbCallback
+{
+	virtual ~b3BroadphaseAabbCallback() {}
+	virtual bool process(const b3BroadphaseProxy* proxy) = 0;
+};
+
+struct b3BroadphaseRayCallback : public b3BroadphaseAabbCallback
+{
+	///added some cached data to accelerate ray-AABB tests
+	b3Vector3 m_rayDirectionInverse;
+	unsigned int m_signs[3];
+	b3Scalar m_lambda_max;
+
+	virtual ~b3BroadphaseRayCallback() {}
+};
+
+#endif  //B3_BROADPHASE_CALLBACK_H

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.cpp

@@ -0,0 +1,808 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///b3DynamicBvhBroadphase implementation by Nathanael Presson
+
+#include "b3DynamicBvhBroadphase.h"
+#include "b3OverlappingPair.h"
+
+//
+// Profiling
+//
+
+#if B3_DBVT_BP_PROFILE || B3_DBVT_BP_ENABLE_BENCHMARK
+#include <stdio.h>
+#endif
+
+#if B3_DBVT_BP_PROFILE
+struct b3ProfileScope
+{
+	__forceinline b3ProfileScope(b3Clock& clock, unsigned long& value) : m_clock(&clock), m_value(&value), m_base(clock.getTimeMicroseconds())
+	{
+	}
+	__forceinline ~b3ProfileScope()
+	{
+		(*m_value) += m_clock->getTimeMicroseconds() - m_base;
+	}
+	b3Clock* m_clock;
+	unsigned long* m_value;
+	unsigned long m_base;
+};
+#define b3SPC(_value_) b3ProfileScope spc_scope(m_clock, _value_)
+#else
+#define b3SPC(_value_)
+#endif
+
+//
+// Helpers
+//
+
+//
+template <typename T>
+static inline void b3ListAppend(T* item, T*& list)
+{
+	item->links[0] = 0;
+	item->links[1] = list;
+	if (list) list->links[0] = item;
+	list = item;
+}
+
+//
+template <typename T>
+static inline void b3ListRemove(T* item, T*& list)
+{
+	if (item->links[0])
+		item->links[0]->links[1] = item->links[1];
+	else
+		list = item->links[1];
+	if (item->links[1]) item->links[1]->links[0] = item->links[0];
+}
+
+//
+template <typename T>
+static inline int b3ListCount(T* root)
+{
+	int n = 0;
+	while (root)
+	{
+		++n;
+		root = root->links[1];
+	}
+	return (n);
+}
+
+//
+template <typename T>
+static inline void b3Clear(T& value)
+{
+	static const struct ZeroDummy : T
+	{
+	} zerodummy;
+	value = zerodummy;
+}
+
+//
+// Colliders
+//
+
+/* Tree collider	*/
+struct b3DbvtTreeCollider : b3DynamicBvh::ICollide
+{
+	b3DynamicBvhBroadphase* pbp;
+	b3DbvtProxy* proxy;
+	b3DbvtTreeCollider(b3DynamicBvhBroadphase* p) : pbp(p) {}
+	void Process(const b3DbvtNode* na, const b3DbvtNode* nb)
+	{
+		if (na != nb)
+		{
+			b3DbvtProxy* pa = (b3DbvtProxy*)na->data;
+			b3DbvtProxy* pb = (b3DbvtProxy*)nb->data;
+#if B3_DBVT_BP_SORTPAIRS
+			if (pa->m_uniqueId > pb->m_uniqueId)
+				b3Swap(pa, pb);
+#endif
+			pbp->m_paircache->addOverlappingPair(pa->getUid(), pb->getUid());
+			++pbp->m_newpairs;
+		}
+	}
+	void Process(const b3DbvtNode* n)
+	{
+		Process(n, proxy->leaf);
+	}
+};
+
+//
+// b3DynamicBvhBroadphase
+//
+
+//
+b3DynamicBvhBroadphase::b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache)
+{
+	m_deferedcollide = false;
+	m_needcleanup = true;
+	m_releasepaircache = (paircache != 0) ? false : true;
+	m_prediction = 0;
+	m_stageCurrent = 0;
+	m_fixedleft = 0;
+	m_fupdates = 1;
+	m_dupdates = 0;
+	m_cupdates = 10;
+	m_newpairs = 1;
+	m_updates_call = 0;
+	m_updates_done = 0;
+	m_updates_ratio = 0;
+	m_paircache = paircache ? paircache : new (b3AlignedAlloc(sizeof(b3HashedOverlappingPairCache), 16)) b3HashedOverlappingPairCache();
+
+	m_pid = 0;
+	m_cid = 0;
+	for (int i = 0; i <= STAGECOUNT; ++i)
+	{
+		m_stageRoots[i] = 0;
+	}
+#if B3_DBVT_BP_PROFILE
+	b3Clear(m_profiling);
+#endif
+	m_proxies.resize(proxyCapacity);
+}
+
+//
+b3DynamicBvhBroadphase::~b3DynamicBvhBroadphase()
+{
+	if (m_releasepaircache)
+	{
+		m_paircache->~b3OverlappingPairCache();
+		b3AlignedFree(m_paircache);
+	}
+}
+
+//
+b3BroadphaseProxy* b3DynamicBvhBroadphase::createProxy(const b3Vector3& aabbMin,
+													   const b3Vector3& aabbMax,
+													   int objectId,
+													   void* userPtr,
+													   int collisionFilterGroup,
+													   int collisionFilterMask)
+{
+	b3DbvtProxy* mem = &m_proxies[objectId];
+	b3DbvtProxy* proxy = new (mem) b3DbvtProxy(aabbMin, aabbMax, userPtr,
+											   collisionFilterGroup,
+											   collisionFilterMask);
+
+	b3DbvtAabbMm aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
+
+	//bproxy->aabb			=	b3DbvtVolume::FromMM(aabbMin,aabbMax);
+	proxy->stage = m_stageCurrent;
+	proxy->m_uniqueId = objectId;
+	proxy->leaf = m_sets[0].insert(aabb, proxy);
+	b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
+	if (!m_deferedcollide)
+	{
+		b3DbvtTreeCollider collider(this);
+		collider.proxy = proxy;
+		m_sets[0].collideTV(m_sets[0].m_root, aabb, collider);
+		m_sets[1].collideTV(m_sets[1].m_root, aabb, collider);
+	}
+	return (proxy);
+}
+
+//
+void b3DynamicBvhBroadphase::destroyProxy(b3BroadphaseProxy* absproxy,
+										  b3Dispatcher* dispatcher)
+{
+	b3DbvtProxy* proxy = (b3DbvtProxy*)absproxy;
+	if (proxy->stage == STAGECOUNT)
+		m_sets[1].remove(proxy->leaf);
+	else
+		m_sets[0].remove(proxy->leaf);
+	b3ListRemove(proxy, m_stageRoots[proxy->stage]);
+	m_paircache->removeOverlappingPairsContainingProxy(proxy->getUid(), dispatcher);
+
+	m_needcleanup = true;
+}
+
+void b3DynamicBvhBroadphase::getAabb(int objectId, b3Vector3& aabbMin, b3Vector3& aabbMax) const
+{
+	const b3DbvtProxy* proxy = &m_proxies[objectId];
+	aabbMin = proxy->m_aabbMin;
+	aabbMax = proxy->m_aabbMax;
+}
+/*
+void	b3DynamicBvhBroadphase::getAabb(b3BroadphaseProxy* absproxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const
+{
+	b3DbvtProxy*						proxy=(b3DbvtProxy*)absproxy;
+	aabbMin = proxy->m_aabbMin;
+	aabbMax = proxy->m_aabbMax;
+}
+*/
+
+struct BroadphaseRayTester : b3DynamicBvh::ICollide
+{
+	b3BroadphaseRayCallback& m_rayCallback;
+	BroadphaseRayTester(b3BroadphaseRayCallback& orgCallback)
+		: m_rayCallback(orgCallback)
+	{
+	}
+	void Process(const b3DbvtNode* leaf)
+	{
+		b3DbvtProxy* proxy = (b3DbvtProxy*)leaf->data;
+		m_rayCallback.process(proxy);
+	}
+};
+
+void b3DynamicBvhBroadphase::rayTest(const b3Vector3& rayFrom, const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin, const b3Vector3& aabbMax)
+{
+	BroadphaseRayTester callback(rayCallback);
+
+	m_sets[0].rayTestInternal(m_sets[0].m_root,
+							  rayFrom,
+							  rayTo,
+							  rayCallback.m_rayDirectionInverse,
+							  rayCallback.m_signs,
+							  rayCallback.m_lambda_max,
+							  aabbMin,
+							  aabbMax,
+							  callback);
+
+	m_sets[1].rayTestInternal(m_sets[1].m_root,
+							  rayFrom,
+							  rayTo,
+							  rayCallback.m_rayDirectionInverse,
+							  rayCallback.m_signs,
+							  rayCallback.m_lambda_max,
+							  aabbMin,
+							  aabbMax,
+							  callback);
+}
+
+struct BroadphaseAabbTester : b3DynamicBvh::ICollide
+{
+	b3BroadphaseAabbCallback& m_aabbCallback;
+	BroadphaseAabbTester(b3BroadphaseAabbCallback& orgCallback)
+		: m_aabbCallback(orgCallback)
+	{
+	}
+	void Process(const b3DbvtNode* leaf)
+	{
+		b3DbvtProxy* proxy = (b3DbvtProxy*)leaf->data;
+		m_aabbCallback.process(proxy);
+	}
+};
+
+void b3DynamicBvhBroadphase::aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& aabbCallback)
+{
+	BroadphaseAabbTester callback(aabbCallback);
+
+	const B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds = b3DbvtVolume::FromMM(aabbMin, aabbMax);
+	//process all children, that overlap with  the given AABB bounds
+	m_sets[0].collideTV(m_sets[0].m_root, bounds, callback);
+	m_sets[1].collideTV(m_sets[1].m_root, bounds, callback);
+}
+
+//
+void b3DynamicBvhBroadphase::setAabb(int objectId,
+									 const b3Vector3& aabbMin,
+									 const b3Vector3& aabbMax,
+									 b3Dispatcher* /*dispatcher*/)
+{
+	b3DbvtProxy* proxy = &m_proxies[objectId];
+	//	b3DbvtProxy*						proxy=(b3DbvtProxy*)absproxy;
+	B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
+	aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
+#if B3_DBVT_BP_PREVENTFALSEUPDATE
+	if (b3NotEqual(aabb, proxy->leaf->volume))
+#endif
+	{
+		bool docollide = false;
+		if (proxy->stage == STAGECOUNT)
+		{ /* fixed -> dynamic set	*/
+			m_sets[1].remove(proxy->leaf);
+			proxy->leaf = m_sets[0].insert(aabb, proxy);
+			docollide = true;
+		}
+		else
+		{ /* dynamic set				*/
+			++m_updates_call;
+			if (b3Intersect(proxy->leaf->volume, aabb))
+			{ /* Moving				*/
+
+				const b3Vector3 delta = aabbMin - proxy->m_aabbMin;
+				b3Vector3 velocity(((proxy->m_aabbMax - proxy->m_aabbMin) / 2) * m_prediction);
+				if (delta[0] < 0) velocity[0] = -velocity[0];
+				if (delta[1] < 0) velocity[1] = -velocity[1];
+				if (delta[2] < 0) velocity[2] = -velocity[2];
+				if (
+#ifdef B3_DBVT_BP_MARGIN
+					m_sets[0].update(proxy->leaf, aabb, velocity, B3_DBVT_BP_MARGIN)
+#else
+					m_sets[0].update(proxy->leaf, aabb, velocity)
+#endif
+				)
+				{
+					++m_updates_done;
+					docollide = true;
+				}
+			}
+			else
+			{ /* Teleporting			*/
+				m_sets[0].update(proxy->leaf, aabb);
+				++m_updates_done;
+				docollide = true;
+			}
+		}
+		b3ListRemove(proxy, m_stageRoots[proxy->stage]);
+		proxy->m_aabbMin = aabbMin;
+		proxy->m_aabbMax = aabbMax;
+		proxy->stage = m_stageCurrent;
+		b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
+		if (docollide)
+		{
+			m_needcleanup = true;
+			if (!m_deferedcollide)
+			{
+				b3DbvtTreeCollider collider(this);
+				m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
+				m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
+			}
+		}
+	}
+}
+
+//
+void b3DynamicBvhBroadphase::setAabbForceUpdate(b3BroadphaseProxy* absproxy,
+												const b3Vector3& aabbMin,
+												const b3Vector3& aabbMax,
+												b3Dispatcher* /*dispatcher*/)
+{
+	b3DbvtProxy* proxy = (b3DbvtProxy*)absproxy;
+	B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
+	aabb = b3DbvtVolume::FromMM(aabbMin, aabbMax);
+	bool docollide = false;
+	if (proxy->stage == STAGECOUNT)
+	{ /* fixed -> dynamic set	*/
+		m_sets[1].remove(proxy->leaf);
+		proxy->leaf = m_sets[0].insert(aabb, proxy);
+		docollide = true;
+	}
+	else
+	{ /* dynamic set				*/
+		++m_updates_call;
+		/* Teleporting			*/
+		m_sets[0].update(proxy->leaf, aabb);
+		++m_updates_done;
+		docollide = true;
+	}
+	b3ListRemove(proxy, m_stageRoots[proxy->stage]);
+	proxy->m_aabbMin = aabbMin;
+	proxy->m_aabbMax = aabbMax;
+	proxy->stage = m_stageCurrent;
+	b3ListAppend(proxy, m_stageRoots[m_stageCurrent]);
+	if (docollide)
+	{
+		m_needcleanup = true;
+		if (!m_deferedcollide)
+		{
+			b3DbvtTreeCollider collider(this);
+			m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
+			m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
+		}
+	}
+}
+
+//
+void b3DynamicBvhBroadphase::calculateOverlappingPairs(b3Dispatcher* dispatcher)
+{
+	collide(dispatcher);
+#if B3_DBVT_BP_PROFILE
+	if (0 == (m_pid % B3_DBVT_BP_PROFILING_RATE))
+	{
+		printf("fixed(%u) dynamics(%u) pairs(%u)\r\n", m_sets[1].m_leaves, m_sets[0].m_leaves, m_paircache->getNumOverlappingPairs());
+		unsigned int total = m_profiling.m_total;
+		if (total <= 0) total = 1;
+		printf("ddcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_ddcollide * 100) / total, m_profiling.m_ddcollide / B3_DBVT_BP_PROFILING_RATE);
+		printf("fdcollide: %u%% (%uus)\r\n", (50 + m_profiling.m_fdcollide * 100) / total, m_profiling.m_fdcollide / B3_DBVT_BP_PROFILING_RATE);
+		printf("cleanup:   %u%% (%uus)\r\n", (50 + m_profiling.m_cleanup * 100) / total, m_profiling.m_cleanup / B3_DBVT_BP_PROFILING_RATE);
+		printf("total:     %uus\r\n", total / B3_DBVT_BP_PROFILING_RATE);
+		const unsigned long sum = m_profiling.m_ddcollide +
+								  m_profiling.m_fdcollide +
+								  m_profiling.m_cleanup;
+		printf("leaked: %u%% (%uus)\r\n", 100 - ((50 + sum * 100) / total), (total - sum) / B3_DBVT_BP_PROFILING_RATE);
+		printf("job counts: %u%%\r\n", (m_profiling.m_jobcount * 100) / ((m_sets[0].m_leaves + m_sets[1].m_leaves) * B3_DBVT_BP_PROFILING_RATE));
+		b3Clear(m_profiling);
+		m_clock.reset();
+	}
+#endif
+
+	performDeferredRemoval(dispatcher);
+}
+
+void b3DynamicBvhBroadphase::performDeferredRemoval(b3Dispatcher* dispatcher)
+{
+	if (m_paircache->hasDeferredRemoval())
+	{
+		b3BroadphasePairArray& overlappingPairArray = m_paircache->getOverlappingPairArray();
+
+		//perform a sort, to find duplicates and to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
+
+		int invalidPair = 0;
+
+		int i;
+
+		b3BroadphasePair previousPair = b3MakeBroadphasePair(-1, -1);
+
+		for (i = 0; i < overlappingPairArray.size(); i++)
+		{
+			b3BroadphasePair& pair = overlappingPairArray[i];
+
+			bool isDuplicate = (pair == previousPair);
+
+			previousPair = pair;
+
+			bool needsRemoval = false;
+
+			if (!isDuplicate)
+			{
+				//important to perform AABB check that is consistent with the broadphase
+				b3DbvtProxy* pa = &m_proxies[pair.x];
+				b3DbvtProxy* pb = &m_proxies[pair.y];
+				bool hasOverlap = b3Intersect(pa->leaf->volume, pb->leaf->volume);
+
+				if (hasOverlap)
+				{
+					needsRemoval = false;
+				}
+				else
+				{
+					needsRemoval = true;
+				}
+			}
+			else
+			{
+				//remove duplicate
+				needsRemoval = true;
+				//should have no algorithm
+			}
+
+			if (needsRemoval)
+			{
+				m_paircache->cleanOverlappingPair(pair, dispatcher);
+
+				pair.x = -1;
+				pair.y = -1;
+				invalidPair++;
+			}
+		}
+
+		//perform a sort, to sort 'invalid' pairs to the end
+		overlappingPairArray.quickSort(b3BroadphasePairSortPredicate());
+		overlappingPairArray.resize(overlappingPairArray.size() - invalidPair);
+	}
+}
+
+//
+void b3DynamicBvhBroadphase::collide(b3Dispatcher* dispatcher)
+{
+	/*printf("---------------------------------------------------------\n");
+	printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
+	printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
+	printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
+	{
+		int i;
+		for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
+		{
+			printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
+				getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
+		}
+		printf("\n");
+	}
+*/
+
+	b3SPC(m_profiling.m_total);
+	/* optimize				*/
+	m_sets[0].optimizeIncremental(1 + (m_sets[0].m_leaves * m_dupdates) / 100);
+	if (m_fixedleft)
+	{
+		const int count = 1 + (m_sets[1].m_leaves * m_fupdates) / 100;
+		m_sets[1].optimizeIncremental(1 + (m_sets[1].m_leaves * m_fupdates) / 100);
+		m_fixedleft = b3Max<int>(0, m_fixedleft - count);
+	}
+	/* dynamic -> fixed set	*/
+	m_stageCurrent = (m_stageCurrent + 1) % STAGECOUNT;
+	b3DbvtProxy* current = m_stageRoots[m_stageCurrent];
+	if (current)
+	{
+		b3DbvtTreeCollider collider(this);
+		do
+		{
+			b3DbvtProxy* next = current->links[1];
+			b3ListRemove(current, m_stageRoots[current->stage]);
+			b3ListAppend(current, m_stageRoots[STAGECOUNT]);
+#if B3_DBVT_BP_ACCURATESLEEPING
+			m_paircache->removeOverlappingPairsContainingProxy(current, dispatcher);
+			collider.proxy = current;
+			b3DynamicBvh::collideTV(m_sets[0].m_root, current->aabb, collider);
+			b3DynamicBvh::collideTV(m_sets[1].m_root, current->aabb, collider);
+#endif
+			m_sets[0].remove(current->leaf);
+			B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
+			curAabb = b3DbvtVolume::FromMM(current->m_aabbMin, current->m_aabbMax);
+			current->leaf = m_sets[1].insert(curAabb, current);
+			current->stage = STAGECOUNT;
+			current = next;
+		} while (current);
+		m_fixedleft = m_sets[1].m_leaves;
+		m_needcleanup = true;
+	}
+	/* collide dynamics		*/
+	{
+		b3DbvtTreeCollider collider(this);
+		if (m_deferedcollide)
+		{
+			b3SPC(m_profiling.m_fdcollide);
+			m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[1].m_root, collider);
+		}
+		if (m_deferedcollide)
+		{
+			b3SPC(m_profiling.m_ddcollide);
+			m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[0].m_root, collider);
+		}
+	}
+	/* clean up				*/
+	if (m_needcleanup)
+	{
+		b3SPC(m_profiling.m_cleanup);
+		b3BroadphasePairArray& pairs = m_paircache->getOverlappingPairArray();
+		if (pairs.size() > 0)
+		{
+			int ni = b3Min(pairs.size(), b3Max<int>(m_newpairs, (pairs.size() * m_cupdates) / 100));
+			for (int i = 0; i < ni; ++i)
+			{
+				b3BroadphasePair& p = pairs[(m_cid + i) % pairs.size()];
+				b3DbvtProxy* pa = &m_proxies[p.x];
+				b3DbvtProxy* pb = &m_proxies[p.y];
+				if (!b3Intersect(pa->leaf->volume, pb->leaf->volume))
+				{
+#if B3_DBVT_BP_SORTPAIRS
+					if (pa->m_uniqueId > pb->m_uniqueId)
+						b3Swap(pa, pb);
+#endif
+					m_paircache->removeOverlappingPair(pa->getUid(), pb->getUid(), dispatcher);
+					--ni;
+					--i;
+				}
+			}
+			if (pairs.size() > 0)
+				m_cid = (m_cid + ni) % pairs.size();
+			else
+				m_cid = 0;
+		}
+	}
+	++m_pid;
+	m_newpairs = 1;
+	m_needcleanup = false;
+	if (m_updates_call > 0)
+	{
+		m_updates_ratio = m_updates_done / (b3Scalar)m_updates_call;
+	}
+	else
+	{
+		m_updates_ratio = 0;
+	}
+	m_updates_done /= 2;
+	m_updates_call /= 2;
+}
+
+//
+void b3DynamicBvhBroadphase::optimize()
+{
+	m_sets[0].optimizeTopDown();
+	m_sets[1].optimizeTopDown();
+}
+
+//
+b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache()
+{
+	return (m_paircache);
+}
+
+//
+const b3OverlappingPairCache* b3DynamicBvhBroadphase::getOverlappingPairCache() const
+{
+	return (m_paircache);
+}
+
+//
+void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin, b3Vector3& aabbMax) const
+{
+	B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume)
+	bounds;
+
+	if (!m_sets[0].empty())
+		if (!m_sets[1].empty())
+			b3Merge(m_sets[0].m_root->volume,
+					m_sets[1].m_root->volume, bounds);
+		else
+			bounds = m_sets[0].m_root->volume;
+	else if (!m_sets[1].empty())
+		bounds = m_sets[1].m_root->volume;
+	else
+		bounds = b3DbvtVolume::FromCR(b3MakeVector3(0, 0, 0), 0);
+	aabbMin = bounds.Mins();
+	aabbMax = bounds.Maxs();
+}
+
+void b3DynamicBvhBroadphase::resetPool(b3Dispatcher* dispatcher)
+{
+	int totalObjects = m_sets[0].m_leaves + m_sets[1].m_leaves;
+	if (!totalObjects)
+	{
+		//reset internal dynamic tree data structures
+		m_sets[0].clear();
+		m_sets[1].clear();
+
+		m_deferedcollide = false;
+		m_needcleanup = true;
+		m_stageCurrent = 0;
+		m_fixedleft = 0;
+		m_fupdates = 1;
+		m_dupdates = 0;
+		m_cupdates = 10;
+		m_newpairs = 1;
+		m_updates_call = 0;
+		m_updates_done = 0;
+		m_updates_ratio = 0;
+
+		m_pid = 0;
+		m_cid = 0;
+		for (int i = 0; i <= STAGECOUNT; ++i)
+		{
+			m_stageRoots[i] = 0;
+		}
+	}
+}
+
+//
+void b3DynamicBvhBroadphase::printStats()
+{
+}
+
+//
+#if B3_DBVT_BP_ENABLE_BENCHMARK
+
+struct b3BroadphaseBenchmark
+{
+	struct Experiment
+	{
+		const char* name;
+		int object_count;
+		int update_count;
+		int spawn_count;
+		int iterations;
+		b3Scalar speed;
+		b3Scalar amplitude;
+	};
+	struct Object
+	{
+		b3Vector3 center;
+		b3Vector3 extents;
+		b3BroadphaseProxy* proxy;
+		b3Scalar time;
+		void update(b3Scalar speed, b3Scalar amplitude, b3BroadphaseInterface* pbi)
+		{
+			time += speed;
+			center[0] = b3Cos(time * (b3Scalar)2.17) * amplitude +
+						b3Sin(time) * amplitude / 2;
+			center[1] = b3Cos(time * (b3Scalar)1.38) * amplitude +
+						b3Sin(time) * amplitude;
+			center[2] = b3Sin(time * (b3Scalar)0.777) * amplitude;
+			pbi->setAabb(proxy, center - extents, center + extents, 0);
+		}
+	};
+	static int UnsignedRand(int range = RAND_MAX - 1) { return (rand() % (range + 1)); }
+	static b3Scalar UnitRand() { return (UnsignedRand(16384) / (b3Scalar)16384); }
+	static void OutputTime(const char* name, b3Clock& c, unsigned count = 0)
+	{
+		const unsigned long us = c.getTimeMicroseconds();
+		const unsigned long ms = (us + 500) / 1000;
+		const b3Scalar sec = us / (b3Scalar)(1000 * 1000);
+		if (count > 0)
+			printf("%s : %u us (%u ms), %.2f/s\r\n", name, us, ms, count / sec);
+		else
+			printf("%s : %u us (%u ms)\r\n", name, us, ms);
+	}
+};
+
+void b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface* pbi)
+{
+	static const b3BroadphaseBenchmark::Experiment experiments[] =
+		{
+			{"1024o.10%", 1024, 10, 0, 8192, (b3Scalar)0.005, (b3Scalar)100},
+			/*{"4096o.10%",4096,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},
+		{"8192o.10%",8192,10,0,8192,(b3Scalar)0.005,(b3Scalar)100},*/
+		};
+	static const int nexperiments = sizeof(experiments) / sizeof(experiments[0]);
+	b3AlignedObjectArray<b3BroadphaseBenchmark::Object*> objects;
+	b3Clock wallclock;
+	/* Begin			*/
+	for (int iexp = 0; iexp < nexperiments; ++iexp)
+	{
+		const b3BroadphaseBenchmark::Experiment& experiment = experiments[iexp];
+		const int object_count = experiment.object_count;
+		const int update_count = (object_count * experiment.update_count) / 100;
+		const int spawn_count = (object_count * experiment.spawn_count) / 100;
+		const b3Scalar speed = experiment.speed;
+		const b3Scalar amplitude = experiment.amplitude;
+		printf("Experiment #%u '%s':\r\n", iexp, experiment.name);
+		printf("\tObjects: %u\r\n", object_count);
+		printf("\tUpdate: %u\r\n", update_count);
+		printf("\tSpawn: %u\r\n", spawn_count);
+		printf("\tSpeed: %f\r\n", speed);
+		printf("\tAmplitude: %f\r\n", amplitude);
+		srand(180673);
+		/* Create objects	*/
+		wallclock.reset();
+		objects.reserve(object_count);
+		for (int i = 0; i < object_count; ++i)
+		{
+			b3BroadphaseBenchmark::Object* po = new b3BroadphaseBenchmark::Object();
+			po->center[0] = b3BroadphaseBenchmark::UnitRand() * 50;
+			po->center[1] = b3BroadphaseBenchmark::UnitRand() * 50;
+			po->center[2] = b3BroadphaseBenchmark::UnitRand() * 50;
+			po->extents[0] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
+			po->extents[1] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
+			po->extents[2] = b3BroadphaseBenchmark::UnitRand() * 2 + 2;
+			po->time = b3BroadphaseBenchmark::UnitRand() * 2000;
+			po->proxy = pbi->createProxy(po->center - po->extents, po->center + po->extents, 0, po, 1, 1, 0, 0);
+			objects.push_back(po);
+		}
+		b3BroadphaseBenchmark::OutputTime("\tInitialization", wallclock);
+		/* First update		*/
+		wallclock.reset();
+		for (int i = 0; i < objects.size(); ++i)
+		{
+			objects[i]->update(speed, amplitude, pbi);
+		}
+		b3BroadphaseBenchmark::OutputTime("\tFirst update", wallclock);
+		/* Updates			*/
+		wallclock.reset();
+		for (int i = 0; i < experiment.iterations; ++i)
+		{
+			for (int j = 0; j < update_count; ++j)
+			{
+				objects[j]->update(speed, amplitude, pbi);
+			}
+			pbi->calculateOverlappingPairs(0);
+		}
+		b3BroadphaseBenchmark::OutputTime("\tUpdate", wallclock, experiment.iterations);
+		/* Clean up			*/
+		wallclock.reset();
+		for (int i = 0; i < objects.size(); ++i)
+		{
+			pbi->destroyProxy(objects[i]->proxy, 0);
+			delete objects[i];
+		}
+		objects.resize(0);
+		b3BroadphaseBenchmark::OutputTime("\tRelease", wallclock);
+	}
+}
+#else
+/*void							b3DynamicBvhBroadphase::benchmark(b3BroadphaseInterface*)
+{}
+*/
+#endif
+
+#if B3_DBVT_BP_PROFILE
+#undef b3SPC
+#endif

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3DynamicBvhBroadphase.h

@@ -0,0 +1,197 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+///b3DynamicBvhBroadphase implementation by Nathanael Presson
+#ifndef B3_DBVT_BROADPHASE_H
+#define B3_DBVT_BROADPHASE_H
+
+#include "Bullet3Collision/BroadPhaseCollision/b3DynamicBvh.h"
+#include "Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+#include "b3BroadphaseCallback.h"
+
+//
+// Compile time config
+//
+
+#define B3_DBVT_BP_PROFILE 0
+//#define B3_DBVT_BP_SORTPAIRS				1
+#define B3_DBVT_BP_PREVENTFALSEUPDATE 0
+#define B3_DBVT_BP_ACCURATESLEEPING 0
+#define B3_DBVT_BP_ENABLE_BENCHMARK 0
+#define B3_DBVT_BP_MARGIN (b3Scalar)0.05
+
+#if B3_DBVT_BP_PROFILE
+#define B3_DBVT_BP_PROFILING_RATE 256
+
+#endif
+
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3BroadphaseProxy
+{
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	///optional filtering to cull potential collisions
+	enum CollisionFilterGroups
+	{
+		DefaultFilter = 1,
+		StaticFilter = 2,
+		KinematicFilter = 4,
+		DebrisFilter = 8,
+		SensorTrigger = 16,
+		CharacterFilter = 32,
+		AllFilter = -1  //all bits sets: DefaultFilter | StaticFilter | KinematicFilter | DebrisFilter | SensorTrigger
+	};
+
+	//Usually the client b3CollisionObject or Rigidbody class
+	void* m_clientObject;
+	int m_collisionFilterGroup;
+	int m_collisionFilterMask;
+	int m_uniqueId;  //m_uniqueId is introduced for paircache. could get rid of this, by calculating the address offset etc.
+
+	b3Vector3 m_aabbMin;
+	b3Vector3 m_aabbMax;
+
+	B3_FORCE_INLINE int getUid() const
+	{
+		return m_uniqueId;
+	}
+
+	//used for memory pools
+	b3BroadphaseProxy() : m_clientObject(0)
+	{
+	}
+
+	b3BroadphaseProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask)
+		: m_clientObject(userPtr),
+		  m_collisionFilterGroup(collisionFilterGroup),
+		  m_collisionFilterMask(collisionFilterMask),
+		  m_aabbMin(aabbMin),
+		  m_aabbMax(aabbMax)
+	{
+	}
+};
+
+//
+// b3DbvtProxy
+//
+struct b3DbvtProxy : b3BroadphaseProxy
+{
+	/* Fields		*/
+	//b3DbvtAabbMm	aabb;
+	b3DbvtNode* leaf;
+	b3DbvtProxy* links[2];
+	int stage;
+	/* ctor			*/
+
+	explicit b3DbvtProxy() {}
+	b3DbvtProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, void* userPtr, int collisionFilterGroup, int collisionFilterMask) : b3BroadphaseProxy(aabbMin, aabbMax, userPtr, collisionFilterGroup, collisionFilterMask)
+	{
+		links[0] = links[1] = 0;
+	}
+};
+
+typedef b3AlignedObjectArray<b3DbvtProxy*> b3DbvtProxyArray;
+
+///The b3DynamicBvhBroadphase implements a broadphase using two dynamic AABB bounding volume hierarchies/trees (see b3DynamicBvh).
+///One tree is used for static/non-moving objects, and another tree is used for dynamic objects. Objects can move from one tree to the other.
+///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases b3AxisSweep3 and b332BitAxisSweep3.
+struct b3DynamicBvhBroadphase
+{
+	/* Config		*/
+	enum
+	{
+		DYNAMIC_SET = 0, /* Dynamic set index	*/
+		FIXED_SET = 1,   /* Fixed set index		*/
+		STAGECOUNT = 2   /* Number of stages		*/
+	};
+	/* Fields		*/
+	b3DynamicBvh m_sets[2];                     // Dbvt sets
+	b3DbvtProxy* m_stageRoots[STAGECOUNT + 1];  // Stages list
+
+	b3AlignedObjectArray<b3DbvtProxy> m_proxies;
+	b3OverlappingPairCache* m_paircache;  // Pair cache
+	b3Scalar m_prediction;                // Velocity prediction
+	int m_stageCurrent;                   // Current stage
+	int m_fupdates;                       // % of fixed updates per frame
+	int m_dupdates;                       // % of dynamic updates per frame
+	int m_cupdates;                       // % of cleanup updates per frame
+	int m_newpairs;                       // Number of pairs created
+	int m_fixedleft;                      // Fixed optimization left
+	unsigned m_updates_call;              // Number of updates call
+	unsigned m_updates_done;              // Number of updates done
+	b3Scalar m_updates_ratio;             // m_updates_done/m_updates_call
+	int m_pid;                            // Parse id
+	int m_cid;                            // Cleanup index
+	bool m_releasepaircache;              // Release pair cache on delete
+	bool m_deferedcollide;                // Defere dynamic/static collision to collide call
+	bool m_needcleanup;                   // Need to run cleanup?
+#if B3_DBVT_BP_PROFILE
+	b3Clock m_clock;
+	struct
+	{
+		unsigned long m_total;
+		unsigned long m_ddcollide;
+		unsigned long m_fdcollide;
+		unsigned long m_cleanup;
+		unsigned long m_jobcount;
+	} m_profiling;
+#endif
+	/* Methods		*/
+	b3DynamicBvhBroadphase(int proxyCapacity, b3OverlappingPairCache* paircache = 0);
+	virtual ~b3DynamicBvhBroadphase();
+	void collide(b3Dispatcher* dispatcher);
+	void optimize();
+
+	/* b3BroadphaseInterface Implementation	*/
+	b3BroadphaseProxy* createProxy(const b3Vector3& aabbMin, const b3Vector3& aabbMax, int objectIndex, void* userPtr, int collisionFilterGroup, int collisionFilterMask);
+	virtual void destroyProxy(b3BroadphaseProxy* proxy, b3Dispatcher* dispatcher);
+	virtual void setAabb(int objectId, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* dispatcher);
+	virtual void rayTest(const b3Vector3& rayFrom, const b3Vector3& rayTo, b3BroadphaseRayCallback& rayCallback, const b3Vector3& aabbMin = b3MakeVector3(0, 0, 0), const b3Vector3& aabbMax = b3MakeVector3(0, 0, 0));
+	virtual void aabbTest(const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3BroadphaseAabbCallback& callback);
+
+	//virtual void					getAabb(b3BroadphaseProxy* proxy,b3Vector3& aabbMin, b3Vector3& aabbMax ) const;
+	virtual void getAabb(int objectId, b3Vector3& aabbMin, b3Vector3& aabbMax) const;
+	virtual void calculateOverlappingPairs(b3Dispatcher* dispatcher = 0);
+	virtual b3OverlappingPairCache* getOverlappingPairCache();
+	virtual const b3OverlappingPairCache* getOverlappingPairCache() const;
+	virtual void getBroadphaseAabb(b3Vector3& aabbMin, b3Vector3& aabbMax) const;
+	virtual void printStats();
+
+	///reset broadphase internal structures, to ensure determinism/reproducability
+	virtual void resetPool(b3Dispatcher* dispatcher);
+
+	void performDeferredRemoval(b3Dispatcher* dispatcher);
+
+	void setVelocityPrediction(b3Scalar prediction)
+	{
+		m_prediction = prediction;
+	}
+	b3Scalar getVelocityPrediction() const
+	{
+		return m_prediction;
+	}
+
+	///this setAabbForceUpdate is similar to setAabb but always forces the aabb update.
+	///it is not part of the b3BroadphaseInterface but specific to b3DynamicBvhBroadphase.
+	///it bypasses certain optimizations that prevent aabb updates (when the aabb shrinks), see
+	///http://code.google.com/p/bullet/issues/detail?id=223
+	void setAabbForceUpdate(b3BroadphaseProxy* absproxy, const b3Vector3& aabbMin, const b3Vector3& aabbMax, b3Dispatcher* /*dispatcher*/);
+
+	//static void						benchmark(b3BroadphaseInterface*);
+};
+
+#endif

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPair.h

@@ -0,0 +1,70 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_OVERLAPPING_PAIR_H
+#define B3_OVERLAPPING_PAIR_H
+
+#include "Bullet3Common/shared/b3Int4.h"
+
+#define B3_NEW_PAIR_MARKER -1
+#define B3_REMOVED_PAIR_MARKER -2
+
+typedef b3Int4 b3BroadphasePair;
+
+inline b3Int4 b3MakeBroadphasePair(int xx, int yy)
+{
+	b3Int4 pair;
+
+	if (xx < yy)
+	{
+		pair.x = xx;
+		pair.y = yy;
+	}
+	else
+	{
+		pair.x = yy;
+		pair.y = xx;
+	}
+	pair.z = B3_NEW_PAIR_MARKER;
+	pair.w = B3_NEW_PAIR_MARKER;
+	return pair;
+}
+
+/*struct b3BroadphasePair : public b3Int4
+{
+	explicit b3BroadphasePair(){}
+	
+};
+*/
+
+class b3BroadphasePairSortPredicate
+{
+public:
+	bool operator()(const b3BroadphasePair& a, const b3BroadphasePair& b) const
+	{
+		const int uidA0 = a.x;
+		const int uidB0 = b.x;
+		const int uidA1 = a.y;
+		const int uidB1 = b.y;
+		return uidA0 > uidB0 || (uidA0 == uidB0 && uidA1 > uidB1);
+	}
+};
+
+B3_FORCE_INLINE bool operator==(const b3BroadphasePair& a, const b3BroadphasePair& b)
+{
+	return (a.x == b.x) && (a.y == b.y);
+}
+
+#endif  //B3_OVERLAPPING_PAIR_H

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.cpp

@@ -0,0 +1,559 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b3OverlappingPairCache.h"
+
+//#include "b3Dispatcher.h"
+//#include "b3CollisionAlgorithm.h"
+#include "Bullet3Geometry/b3AabbUtil.h"
+
+#include <stdio.h>
+
+int b3g_overlappingPairs = 0;
+int b3g_removePairs = 0;
+int b3g_addedPairs = 0;
+int b3g_findPairs = 0;
+
+b3HashedOverlappingPairCache::b3HashedOverlappingPairCache() : m_overlapFilterCallback(0)
+//,	m_blockedForChanges(false)
+{
+	int initialAllocatedSize = 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+	growTables();
+}
+
+b3HashedOverlappingPairCache::~b3HashedOverlappingPairCache()
+{
+}
+
+void b3HashedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher)
+{
+	/*	if (pair.m_algorithm)
+	{
+		{
+			pair.m_algorithm->~b3CollisionAlgorithm();
+			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
+			pair.m_algorithm=0;
+		}
+	}
+	*/
+}
+
+void b3HashedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
+{
+	class CleanPairCallback : public b3OverlapCallback
+	{
+		int m_cleanProxy;
+		b3OverlappingPairCache* m_pairCache;
+		b3Dispatcher* m_dispatcher;
+
+	public:
+		CleanPairCallback(int cleanProxy, b3OverlappingPairCache* pairCache, b3Dispatcher* dispatcher)
+			: m_cleanProxy(cleanProxy),
+			  m_pairCache(pairCache),
+			  m_dispatcher(dispatcher)
+		{
+		}
+		virtual bool processOverlap(b3BroadphasePair& pair)
+		{
+			if ((pair.x == m_cleanProxy) ||
+				(pair.y == m_cleanProxy))
+			{
+				m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
+			}
+			return false;
+		}
+	};
+
+	CleanPairCallback cleanPairs(proxy, this, dispatcher);
+
+	processAllOverlappingPairs(&cleanPairs, dispatcher);
+}
+
+void b3HashedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
+{
+	class RemovePairCallback : public b3OverlapCallback
+	{
+		int m_obsoleteProxy;
+
+	public:
+		RemovePairCallback(int obsoleteProxy)
+			: m_obsoleteProxy(obsoleteProxy)
+		{
+		}
+		virtual bool processOverlap(b3BroadphasePair& pair)
+		{
+			return ((pair.x == m_obsoleteProxy) ||
+					(pair.y == m_obsoleteProxy));
+		}
+	};
+
+	RemovePairCallback removeCallback(proxy);
+
+	processAllOverlappingPairs(&removeCallback, dispatcher);
+}
+
+b3BroadphasePair* b3HashedOverlappingPairCache::findPair(int proxy0, int proxy1)
+{
+	b3g_findPairs++;
+	if (proxy0 > proxy1)
+		b3Swap(proxy0, proxy1);
+	int proxyId1 = proxy0;
+	int proxyId2 = proxy1;
+
+	/*if (proxyId1 > proxyId2) 
+		b3Swap(proxyId1, proxyId2);*/
+
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
+
+	if (hash >= m_hashTable.size())
+	{
+		return NULL;
+	}
+
+	int index = m_hashTable[hash];
+	while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+	{
+		index = m_next[index];
+	}
+
+	if (index == B3_NULL_PAIR)
+	{
+		return NULL;
+	}
+
+	b3Assert(index < m_overlappingPairArray.size());
+
+	return &m_overlappingPairArray[index];
+}
+
+//#include <stdio.h>
+
+void b3HashedOverlappingPairCache::growTables()
+{
+	int newCapacity = m_overlappingPairArray.capacity();
+
+	if (m_hashTable.size() < newCapacity)
+	{
+		//grow hashtable and next table
+		int curHashtableSize = m_hashTable.size();
+
+		m_hashTable.resize(newCapacity);
+		m_next.resize(newCapacity);
+
+		int i;
+
+		for (i = 0; i < newCapacity; ++i)
+		{
+			m_hashTable[i] = B3_NULL_PAIR;
+		}
+		for (i = 0; i < newCapacity; ++i)
+		{
+			m_next[i] = B3_NULL_PAIR;
+		}
+
+		for (i = 0; i < curHashtableSize; i++)
+		{
+			const b3BroadphasePair& pair = m_overlappingPairArray[i];
+			int proxyId1 = pair.x;
+			int proxyId2 = pair.y;
+			/*if (proxyId1 > proxyId2) 
+				b3Swap(proxyId1, proxyId2);*/
+			int hashValue = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
+			m_next[i] = m_hashTable[hashValue];
+			m_hashTable[hashValue] = i;
+		}
+	}
+}
+
+b3BroadphasePair* b3HashedOverlappingPairCache::internalAddPair(int proxy0, int proxy1)
+{
+	if (proxy0 > proxy1)
+		b3Swap(proxy0, proxy1);
+	int proxyId1 = proxy0;
+	int proxyId2 = proxy1;
+
+	/*if (proxyId1 > proxyId2) 
+		b3Swap(proxyId1, proxyId2);*/
+
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));  // New hash value with new mask
+
+	b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
+	if (pair != NULL)
+	{
+		return pair;
+	}
+	/*for(int i=0;i<m_overlappingPairArray.size();++i)
+		{
+		if(	(m_overlappingPairArray[i].m_pProxy0==proxy0)&&
+			(m_overlappingPairArray[i].m_pProxy1==proxy1))
+			{
+			printf("Adding duplicated %u<>%u\r\n",proxyId1,proxyId2);
+			internalFindPair(proxy0, proxy1, hash);
+			}
+		}*/
+	int count = m_overlappingPairArray.size();
+	int oldCapacity = m_overlappingPairArray.capacity();
+	pair = &m_overlappingPairArray.expandNonInitializing();
+
+	//this is where we add an actual pair, so also call the 'ghost'
+	//	if (m_ghostPairCallback)
+	//		m_ghostPairCallback->addOverlappingPair(proxy0,proxy1);
+
+	int newCapacity = m_overlappingPairArray.capacity();
+
+	if (oldCapacity < newCapacity)
+	{
+		growTables();
+		//hash with new capacity
+		hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
+	}
+
+	*pair = b3MakeBroadphasePair(proxy0, proxy1);
+
+	//	pair->m_pProxy0 = proxy0;
+	//	pair->m_pProxy1 = proxy1;
+	//pair->m_algorithm = 0;
+	//pair->m_internalTmpValue = 0;
+
+	m_next[count] = m_hashTable[hash];
+	m_hashTable[hash] = count;
+
+	return pair;
+}
+
+void* b3HashedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
+{
+	b3g_removePairs++;
+	if (proxy0 > proxy1)
+		b3Swap(proxy0, proxy1);
+	int proxyId1 = proxy0;
+	int proxyId2 = proxy1;
+
+	/*if (proxyId1 > proxyId2) 
+		b3Swap(proxyId1, proxyId2);*/
+
+	int hash = static_cast<int>(getHash(static_cast<unsigned int>(proxyId1), static_cast<unsigned int>(proxyId2)) & (m_overlappingPairArray.capacity() - 1));
+
+	b3BroadphasePair* pair = internalFindPair(proxy0, proxy1, hash);
+	if (pair == NULL)
+	{
+		return 0;
+	}
+
+	cleanOverlappingPair(*pair, dispatcher);
+
+	int pairIndex = int(pair - &m_overlappingPairArray[0]);
+	b3Assert(pairIndex < m_overlappingPairArray.size());
+
+	// Remove the pair from the hash table.
+	int index = m_hashTable[hash];
+	b3Assert(index != B3_NULL_PAIR);
+
+	int previous = B3_NULL_PAIR;
+	while (index != pairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != B3_NULL_PAIR)
+	{
+		b3Assert(m_next[previous] == pairIndex);
+		m_next[previous] = m_next[pairIndex];
+	}
+	else
+	{
+		m_hashTable[hash] = m_next[pairIndex];
+	}
+
+	// We now move the last pair into spot of the
+	// pair being removed. We need to fix the hash
+	// table indices to support the move.
+
+	int lastPairIndex = m_overlappingPairArray.size() - 1;
+
+	//if (m_ghostPairCallback)
+	//	m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
+
+	// If the removed pair is the last pair, we are done.
+	if (lastPairIndex == pairIndex)
+	{
+		m_overlappingPairArray.pop_back();
+		return 0;
+	}
+
+	// Remove the last pair from the hash table.
+	const b3BroadphasePair* last = &m_overlappingPairArray[lastPairIndex];
+	/* missing swap here too, Nat. */
+	int lastHash = static_cast<int>(getHash(static_cast<unsigned int>(last->x), static_cast<unsigned int>(last->y)) & (m_overlappingPairArray.capacity() - 1));
+
+	index = m_hashTable[lastHash];
+	b3Assert(index != B3_NULL_PAIR);
+
+	previous = B3_NULL_PAIR;
+	while (index != lastPairIndex)
+	{
+		previous = index;
+		index = m_next[index];
+	}
+
+	if (previous != B3_NULL_PAIR)
+	{
+		b3Assert(m_next[previous] == lastPairIndex);
+		m_next[previous] = m_next[lastPairIndex];
+	}
+	else
+	{
+		m_hashTable[lastHash] = m_next[lastPairIndex];
+	}
+
+	// Copy the last pair into the remove pair's spot.
+	m_overlappingPairArray[pairIndex] = m_overlappingPairArray[lastPairIndex];
+
+	// Insert the last pair into the hash table
+	m_next[pairIndex] = m_hashTable[lastHash];
+	m_hashTable[lastHash] = pairIndex;
+
+	m_overlappingPairArray.pop_back();
+
+	return 0;
+}
+//#include <stdio.h>
+
+void b3HashedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
+{
+	int i;
+
+	//	printf("m_overlappingPairArray.size()=%d\n",m_overlappingPairArray.size());
+	for (i = 0; i < m_overlappingPairArray.size();)
+	{
+		b3BroadphasePair* pair = &m_overlappingPairArray[i];
+		if (callback->processOverlap(*pair))
+		{
+			removeOverlappingPair(pair->x, pair->y, dispatcher);
+
+			b3g_overlappingPairs--;
+		}
+		else
+		{
+			i++;
+		}
+	}
+}
+
+void b3HashedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
+{
+	///need to keep hashmap in sync with pair address, so rebuild all
+	b3BroadphasePairArray tmpPairs;
+	int i;
+	for (i = 0; i < m_overlappingPairArray.size(); i++)
+	{
+		tmpPairs.push_back(m_overlappingPairArray[i]);
+	}
+
+	for (i = 0; i < tmpPairs.size(); i++)
+	{
+		removeOverlappingPair(tmpPairs[i].x, tmpPairs[i].y, dispatcher);
+	}
+
+	for (i = 0; i < m_next.size(); i++)
+	{
+		m_next[i] = B3_NULL_PAIR;
+	}
+
+	tmpPairs.quickSort(b3BroadphasePairSortPredicate());
+
+	for (i = 0; i < tmpPairs.size(); i++)
+	{
+		addOverlappingPair(tmpPairs[i].x, tmpPairs[i].y);
+	}
+}
+
+void* b3SortedOverlappingPairCache::removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher)
+{
+	if (!hasDeferredRemoval())
+	{
+		b3BroadphasePair findPair = b3MakeBroadphasePair(proxy0, proxy1);
+
+		int findIndex = m_overlappingPairArray.findLinearSearch(findPair);
+		if (findIndex < m_overlappingPairArray.size())
+		{
+			b3g_overlappingPairs--;
+			b3BroadphasePair& pair = m_overlappingPairArray[findIndex];
+
+			cleanOverlappingPair(pair, dispatcher);
+			//if (m_ghostPairCallback)
+			//	m_ghostPairCallback->removeOverlappingPair(proxy0, proxy1,dispatcher);
+
+			m_overlappingPairArray.swap(findIndex, m_overlappingPairArray.capacity() - 1);
+			m_overlappingPairArray.pop_back();
+			return 0;
+		}
+	}
+
+	return 0;
+}
+
+b3BroadphasePair* b3SortedOverlappingPairCache::addOverlappingPair(int proxy0, int proxy1)
+{
+	//don't add overlap with own
+	b3Assert(proxy0 != proxy1);
+
+	if (!needsBroadphaseCollision(proxy0, proxy1))
+		return 0;
+
+	b3BroadphasePair* pair = &m_overlappingPairArray.expandNonInitializing();
+	*pair = b3MakeBroadphasePair(proxy0, proxy1);
+
+	b3g_overlappingPairs++;
+	b3g_addedPairs++;
+
+	//	if (m_ghostPairCallback)
+	//		m_ghostPairCallback->addOverlappingPair(proxy0, proxy1);
+	return pair;
+}
+
+///this findPair becomes really slow. Either sort the list to speedup the query, or
+///use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
+///we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
+///Also we can use a 2D bitmap, which can be useful for a future GPU implementation
+b3BroadphasePair* b3SortedOverlappingPairCache::findPair(int proxy0, int proxy1)
+{
+	if (!needsBroadphaseCollision(proxy0, proxy1))
+		return 0;
+
+	b3BroadphasePair tmpPair = b3MakeBroadphasePair(proxy0, proxy1);
+	int findIndex = m_overlappingPairArray.findLinearSearch(tmpPair);
+
+	if (findIndex < m_overlappingPairArray.size())
+	{
+		//b3Assert(it != m_overlappingPairSet.end());
+		b3BroadphasePair* pair = &m_overlappingPairArray[findIndex];
+		return pair;
+	}
+	return 0;
+}
+
+//#include <stdio.h>
+
+void b3SortedOverlappingPairCache::processAllOverlappingPairs(b3OverlapCallback* callback, b3Dispatcher* dispatcher)
+{
+	int i;
+
+	for (i = 0; i < m_overlappingPairArray.size();)
+	{
+		b3BroadphasePair* pair = &m_overlappingPairArray[i];
+		if (callback->processOverlap(*pair))
+		{
+			cleanOverlappingPair(*pair, dispatcher);
+			pair->x = -1;
+			pair->y = -1;
+			m_overlappingPairArray.swap(i, m_overlappingPairArray.size() - 1);
+			m_overlappingPairArray.pop_back();
+			b3g_overlappingPairs--;
+		}
+		else
+		{
+			i++;
+		}
+	}
+}
+
+b3SortedOverlappingPairCache::b3SortedOverlappingPairCache() : m_blockedForChanges(false),
+															   m_hasDeferredRemoval(true),
+															   m_overlapFilterCallback(0)
+
+{
+	int initialAllocatedSize = 2;
+	m_overlappingPairArray.reserve(initialAllocatedSize);
+}
+
+b3SortedOverlappingPairCache::~b3SortedOverlappingPairCache()
+{
+}
+
+void b3SortedOverlappingPairCache::cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher)
+{
+	/*	if (pair.m_algorithm)
+	{
+		{
+			pair.m_algorithm->~b3CollisionAlgorithm();
+			dispatcher->freeCollisionAlgorithm(pair.m_algorithm);
+			pair.m_algorithm=0;
+			b3g_removePairs--;
+		}
+	}
+	*/
+}
+
+void b3SortedOverlappingPairCache::cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher)
+{
+	class CleanPairCallback : public b3OverlapCallback
+	{
+		int m_cleanProxy;
+		b3OverlappingPairCache* m_pairCache;
+		b3Dispatcher* m_dispatcher;
+
+	public:
+		CleanPairCallback(int cleanProxy, b3OverlappingPairCache* pairCache, b3Dispatcher* dispatcher)
+			: m_cleanProxy(cleanProxy),
+			  m_pairCache(pairCache),
+			  m_dispatcher(dispatcher)
+		{
+		}
+		virtual bool processOverlap(b3BroadphasePair& pair)
+		{
+			if ((pair.x == m_cleanProxy) ||
+				(pair.y == m_cleanProxy))
+			{
+				m_pairCache->cleanOverlappingPair(pair, m_dispatcher);
+			}
+			return false;
+		}
+	};
+
+	CleanPairCallback cleanPairs(proxy, this, dispatcher);
+
+	processAllOverlappingPairs(&cleanPairs, dispatcher);
+}
+
+void b3SortedOverlappingPairCache::removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher)
+{
+	class RemovePairCallback : public b3OverlapCallback
+	{
+		int m_obsoleteProxy;
+
+	public:
+		RemovePairCallback(int obsoleteProxy)
+			: m_obsoleteProxy(obsoleteProxy)
+		{
+		}
+		virtual bool processOverlap(b3BroadphasePair& pair)
+		{
+			return ((pair.x == m_obsoleteProxy) ||
+					(pair.y == m_obsoleteProxy));
+		}
+	};
+
+	RemovePairCallback removeCallback(proxy);
+
+	processAllOverlappingPairs(&removeCallback, dispatcher);
+}
+
+void b3SortedOverlappingPairCache::sortOverlappingPairs(b3Dispatcher* dispatcher)
+{
+	//should already be sorted
+}

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/b3OverlappingPairCache.h

@@ -0,0 +1,427 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_OVERLAPPING_PAIR_CACHE_H
+#define B3_OVERLAPPING_PAIR_CACHE_H
+
+#include "Bullet3Common/shared/b3Int2.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+class b3Dispatcher;
+#include "b3OverlappingPair.h"
+
+typedef b3AlignedObjectArray<b3BroadphasePair> b3BroadphasePairArray;
+
+struct b3OverlapCallback
+{
+	virtual ~b3OverlapCallback()
+	{
+	}
+	//return true for deletion of the pair
+	virtual bool processOverlap(b3BroadphasePair& pair) = 0;
+};
+
+struct b3OverlapFilterCallback
+{
+	virtual ~b3OverlapFilterCallback()
+	{
+	}
+	// return true when pairs need collision
+	virtual bool needBroadphaseCollision(int proxy0, int proxy1) const = 0;
+};
+
+extern int b3g_removePairs;
+extern int b3g_addedPairs;
+extern int b3g_findPairs;
+
+const int B3_NULL_PAIR = 0xffffffff;
+
+///The b3OverlappingPairCache provides an interface for overlapping pair management (add, remove, storage), used by the b3BroadphaseInterface broadphases.
+///The b3HashedOverlappingPairCache and b3SortedOverlappingPairCache classes are two implementations.
+class b3OverlappingPairCache
+{
+public:
+	virtual ~b3OverlappingPairCache() {}  // this is needed so we can get to the derived class destructor
+
+	virtual b3BroadphasePair* getOverlappingPairArrayPtr() = 0;
+
+	virtual const b3BroadphasePair* getOverlappingPairArrayPtr() const = 0;
+
+	virtual b3BroadphasePairArray& getOverlappingPairArray() = 0;
+
+	virtual void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher) = 0;
+
+	virtual int getNumOverlappingPairs() const = 0;
+
+	virtual void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher) = 0;
+
+	virtual void setOverlapFilterCallback(b3OverlapFilterCallback* callback) = 0;
+
+	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher) = 0;
+
+	virtual b3BroadphasePair* findPair(int proxy0, int proxy1) = 0;
+
+	virtual bool hasDeferredRemoval() = 0;
+
+	//virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)=0;
+
+	virtual b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1) = 0;
+	virtual void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher) = 0;
+	virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/, b3Dispatcher* /*dispatcher*/) = 0;
+
+	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher) = 0;
+};
+
+/// Hash-space based Pair Cache, thanks to Erin Catto, Box2D, http://www.box2d.org, and Pierre Terdiman, Codercorner, http://codercorner.com
+class b3HashedOverlappingPairCache : public b3OverlappingPairCache
+{
+	b3BroadphasePairArray m_overlappingPairArray;
+	b3OverlapFilterCallback* m_overlapFilterCallback;
+	//	bool		m_blockedForChanges;
+
+public:
+	b3HashedOverlappingPairCache();
+	virtual ~b3HashedOverlappingPairCache();
+
+	virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
+
+	virtual void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher);
+
+	B3_FORCE_INLINE bool needsBroadphaseCollision(int proxy0, int proxy1) const
+	{
+		if (m_overlapFilterCallback)
+			return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
+
+		bool collides = true;  //(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+		//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+
+		return collides;
+	}
+
+	// Add a pair and return the new pair. If the pair already exists,
+	// no new pair is created and the old one is returned.
+	virtual b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1)
+	{
+		b3g_addedPairs++;
+
+		if (!needsBroadphaseCollision(proxy0, proxy1))
+			return 0;
+
+		return internalAddPair(proxy0, proxy1);
+	}
+
+	void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
+
+	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher);
+
+	virtual b3BroadphasePair* getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	const b3BroadphasePair* getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	b3BroadphasePairArray& getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+
+	const b3BroadphasePairArray& getOverlappingPairArray() const
+	{
+		return m_overlappingPairArray;
+	}
+
+	void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher);
+
+	b3BroadphasePair* findPair(int proxy0, int proxy1);
+
+	int GetCount() const { return m_overlappingPairArray.size(); }
+	//	b3BroadphasePair* GetPairs() { return m_pairs; }
+
+	b3OverlapFilterCallback* getOverlapFilterCallback()
+	{
+		return m_overlapFilterCallback;
+	}
+
+	void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
+	{
+		m_overlapFilterCallback = callback;
+	}
+
+	int getNumOverlappingPairs() const
+	{
+		return m_overlappingPairArray.size();
+	}
+
+private:
+	b3BroadphasePair* internalAddPair(int proxy0, int proxy1);
+
+	void growTables();
+
+	B3_FORCE_INLINE bool equalsPair(const b3BroadphasePair& pair, int proxyId1, int proxyId2)
+	{
+		return pair.x == proxyId1 && pair.y == proxyId2;
+	}
+
+	/*
+	// Thomas Wang's hash, see: http://www.concentric.net/~Ttwang/tech/inthash.htm
+	// This assumes proxyId1 and proxyId2 are 16-bit.
+	B3_FORCE_INLINE int getHash(int proxyId1, int proxyId2)
+	{
+		int key = (proxyId2 << 16) | proxyId1;
+		key = ~key + (key << 15);
+		key = key ^ (key >> 12);
+		key = key + (key << 2);
+		key = key ^ (key >> 4);
+		key = key * 2057;
+		key = key ^ (key >> 16);
+		return key;
+	}
+	*/
+
+	B3_FORCE_INLINE unsigned int getHash(unsigned int proxyId1, unsigned int proxyId2)
+	{
+		int key = static_cast<int>(((unsigned int)proxyId1) | (((unsigned int)proxyId2) << 16));
+		// Thomas Wang's hash
+
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
+		return static_cast<unsigned int>(key);
+	}
+
+	B3_FORCE_INLINE b3BroadphasePair* internalFindPair(int proxy0, int proxy1, int hash)
+	{
+		int proxyId1 = proxy0;
+		int proxyId2 = proxy1;
+#if 0  // wrong, 'equalsPair' use unsorted uids, copy-past devil striked again. Nat.
+		if (proxyId1 > proxyId2) 
+			b3Swap(proxyId1, proxyId2);
+#endif
+
+		int index = m_hashTable[hash];
+
+		while (index != B3_NULL_PAIR && equalsPair(m_overlappingPairArray[index], proxyId1, proxyId2) == false)
+		{
+			index = m_next[index];
+		}
+
+		if (index == B3_NULL_PAIR)
+		{
+			return NULL;
+		}
+
+		b3Assert(index < m_overlappingPairArray.size());
+
+		return &m_overlappingPairArray[index];
+	}
+
+	virtual bool hasDeferredRemoval()
+	{
+		return false;
+	}
+
+	/*	virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
+	{
+		m_ghostPairCallback = ghostPairCallback;
+	}
+	*/
+
+	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
+
+protected:
+	b3AlignedObjectArray<int> m_hashTable;
+	b3AlignedObjectArray<int> m_next;
+	//	b3OverlappingPairCallback*	m_ghostPairCallback;
+};
+
+///b3SortedOverlappingPairCache maintains the objects with overlapping AABB
+///Typically managed by the Broadphase, Axis3Sweep or b3SimpleBroadphase
+class b3SortedOverlappingPairCache : public b3OverlappingPairCache
+{
+protected:
+	//avoid brute-force finding all the time
+	b3BroadphasePairArray m_overlappingPairArray;
+
+	//during the dispatch, check that user doesn't destroy/create proxy
+	bool m_blockedForChanges;
+
+	///by default, do the removal during the pair traversal
+	bool m_hasDeferredRemoval;
+
+	//if set, use the callback instead of the built in filter in needBroadphaseCollision
+	b3OverlapFilterCallback* m_overlapFilterCallback;
+
+	//		b3OverlappingPairCallback*	m_ghostPairCallback;
+
+public:
+	b3SortedOverlappingPairCache();
+	virtual ~b3SortedOverlappingPairCache();
+
+	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* dispatcher);
+
+	void* removeOverlappingPair(int proxy0, int proxy1, b3Dispatcher* dispatcher);
+
+	void cleanOverlappingPair(b3BroadphasePair& pair, b3Dispatcher* dispatcher);
+
+	b3BroadphasePair* addOverlappingPair(int proxy0, int proxy1);
+
+	b3BroadphasePair* findPair(int proxy0, int proxy1);
+
+	void cleanProxyFromPairs(int proxy, b3Dispatcher* dispatcher);
+
+	virtual void removeOverlappingPairsContainingProxy(int proxy, b3Dispatcher* dispatcher);
+
+	inline bool needsBroadphaseCollision(int proxy0, int proxy1) const
+	{
+		if (m_overlapFilterCallback)
+			return m_overlapFilterCallback->needBroadphaseCollision(proxy0, proxy1);
+
+		bool collides = true;  //(proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
+		//collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
+
+		return collides;
+	}
+
+	b3BroadphasePairArray& getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+
+	const b3BroadphasePairArray& getOverlappingPairArray() const
+	{
+		return m_overlappingPairArray;
+	}
+
+	b3BroadphasePair* getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	const b3BroadphasePair* getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+
+	int getNumOverlappingPairs() const
+	{
+		return m_overlappingPairArray.size();
+	}
+
+	b3OverlapFilterCallback* getOverlapFilterCallback()
+	{
+		return m_overlapFilterCallback;
+	}
+
+	void setOverlapFilterCallback(b3OverlapFilterCallback* callback)
+	{
+		m_overlapFilterCallback = callback;
+	}
+
+	virtual bool hasDeferredRemoval()
+	{
+		return m_hasDeferredRemoval;
+	}
+
+	/*		virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* ghostPairCallback)
+		{
+			m_ghostPairCallback = ghostPairCallback;
+		}
+		*/
+	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher);
+};
+
+///b3NullPairCache skips add/removal of overlapping pairs. Userful for benchmarking and unit testing.
+class b3NullPairCache : public b3OverlappingPairCache
+{
+	b3BroadphasePairArray m_overlappingPairArray;
+
+public:
+	virtual b3BroadphasePair* getOverlappingPairArrayPtr()
+	{
+		return &m_overlappingPairArray[0];
+	}
+	const b3BroadphasePair* getOverlappingPairArrayPtr() const
+	{
+		return &m_overlappingPairArray[0];
+	}
+	b3BroadphasePairArray& getOverlappingPairArray()
+	{
+		return m_overlappingPairArray;
+	}
+
+	virtual void cleanOverlappingPair(b3BroadphasePair& /*pair*/, b3Dispatcher* /*dispatcher*/)
+	{
+	}
+
+	virtual int getNumOverlappingPairs() const
+	{
+		return 0;
+	}
+
+	virtual void cleanProxyFromPairs(int /*proxy*/, b3Dispatcher* /*dispatcher*/)
+	{
+	}
+
+	virtual void setOverlapFilterCallback(b3OverlapFilterCallback* /*callback*/)
+	{
+	}
+
+	virtual void processAllOverlappingPairs(b3OverlapCallback*, b3Dispatcher* /*dispatcher*/)
+	{
+	}
+
+	virtual b3BroadphasePair* findPair(int /*proxy0*/, int /*proxy1*/)
+	{
+		return 0;
+	}
+
+	virtual bool hasDeferredRemoval()
+	{
+		return true;
+	}
+
+	//	virtual	void	setInternalGhostPairCallback(b3OverlappingPairCallback* /* ghostPairCallback */)
+	//	{
+	//
+	//	}
+
+	virtual b3BroadphasePair* addOverlappingPair(int /*proxy0*/, int /*proxy1*/)
+	{
+		return 0;
+	}
+
+	virtual void* removeOverlappingPair(int /*proxy0*/, int /*proxy1*/, b3Dispatcher* /*dispatcher*/)
+	{
+		return 0;
+	}
+
+	virtual void removeOverlappingPairsContainingProxy(int /*proxy0*/, b3Dispatcher* /*dispatcher*/)
+	{
+	}
+
+	virtual void sortOverlappingPairs(b3Dispatcher* dispatcher)
+	{
+		(void)dispatcher;
+	}
+};
+
+#endif  //B3_OVERLAPPING_PAIR_CACHE_H

thirdparty/bullet/src/Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h

@@ -0,0 +1,56 @@
+
+#ifndef B3_AABB_H
+#define B3_AABB_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Common/shared/b3Mat3x3.h"
+
+typedef struct b3Aabb b3Aabb_t;
+
+struct b3Aabb
+{
+	union {
+		float m_min[4];
+		b3Float4 m_minVec;
+		int m_minIndices[4];
+	};
+	union {
+		float m_max[4];
+		b3Float4 m_maxVec;
+		int m_signedMaxIndices[4];
+	};
+};
+
+inline void b3TransformAabb2(b3Float4ConstArg localAabbMin, b3Float4ConstArg localAabbMax, float margin,
+							 b3Float4ConstArg pos,
+							 b3QuatConstArg orn,
+							 b3Float4* aabbMinOut, b3Float4* aabbMaxOut)
+{
+	b3Float4 localHalfExtents = 0.5f * (localAabbMax - localAabbMin);
+	localHalfExtents += b3MakeFloat4(margin, margin, margin, 0.f);
+	b3Float4 localCenter = 0.5f * (localAabbMax + localAabbMin);
+	b3Mat3x3 m;
+	m = b3QuatGetRotationMatrix(orn);
+	b3Mat3x3 abs_b = b3AbsoluteMat3x3(m);
+	b3Float4 center = b3TransformPoint(localCenter, pos, orn);
+
+	b3Float4 extent = b3MakeFloat4(b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 0)),
+								   b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 1)),
+								   b3Dot3F4(localHalfExtents, b3GetRow(abs_b, 2)),
+								   0.f);
+	*aabbMinOut = center - extent;
+	*aabbMaxOut = center + extent;
+}
+
+/// conservative test for overlap between two aabbs
+inline bool b3TestAabbAgainstAabb(b3Float4ConstArg aabbMin1, b3Float4ConstArg aabbMax1,
+								  b3Float4ConstArg aabbMin2, b3Float4ConstArg aabbMax2)
+{
+	bool 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;
+}
+
+#endif  //B3_AABB_H

thirdparty/bullet/src/Bullet3Collision/CMakeLists.txt

@@ -0,0 +1,93 @@
+
+INCLUDE_DIRECTORIES(
+	${BULLET_PHYSICS_SOURCE_DIR}/src
+)
+
+SET(Bullet3Collision_SRCS
+	BroadPhaseCollision/b3DynamicBvh.cpp
+	BroadPhaseCollision/b3DynamicBvhBroadphase.cpp
+	BroadPhaseCollision/b3OverlappingPairCache.cpp
+	NarrowPhaseCollision/b3ConvexUtility.cpp
+	NarrowPhaseCollision/b3CpuNarrowPhase.cpp
+)
+
+SET(Bullet3CollisionBroadPhase_HDRS
+	BroadPhaseCollision/b3BroadphaseCallback.h
+	BroadPhaseCollision/b3DynamicBvh.h
+	BroadPhaseCollision/b3DynamicBvhBroadphase.h
+	BroadPhaseCollision/b3OverlappingPair.h
+	BroadPhaseCollision/b3OverlappingPairCache.h
+)
+SET(Bullet3CollisionBroadPhaseShared_HDRS
+	BroadPhaseCollision/shared/b3Aabb.h
+)
+
+SET(Bullet3CollisionNarrowPhase_HDRS
+	NarrowPhaseCollision/b3Config.h
+	NarrowPhaseCollision/b3Contact4.h
+	NarrowPhaseCollision/b3ConvexUtility.h
+	NarrowPhaseCollision/b3CpuNarrowPhase.h
+	NarrowPhaseCollision/b3RaycastInfo.h
+	NarrowPhaseCollision/b3RigidBodyCL.h
+)
+SET(Bullet3CollisionNarrowPhaseShared_HDRS
+
+	NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h
+	NarrowPhaseCollision/shared/b3BvhTraversal.h
+	NarrowPhaseCollision/shared/b3ClipFaces.h
+	NarrowPhaseCollision/shared/b3Collidable.h
+	NarrowPhaseCollision/shared/b3Contact4Data.h
+	NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h
+	NarrowPhaseCollision/shared/b3ContactSphereSphere.h
+	NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h
+	NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h
+	NarrowPhaseCollision/shared/b3FindSeparatingAxis.h
+	NarrowPhaseCollision/shared/b3MprPenetration.h
+	NarrowPhaseCollision/shared/b3NewContactReduction.h
+	NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h
+	NarrowPhaseCollision/shared/b3ReduceContacts.h
+	NarrowPhaseCollision/shared/b3RigidBodyData.h
+	NarrowPhaseCollision/shared/b3UpdateAabbs.h
+)
+
+SET(Bullet3Collision_HDRS
+	${Bullet3CollisionBroadPhase_HDRS}
+	${Bullet3CollisionBroadPhaseShared_HDRS}
+	${Bullet3CollisionNarrowPhaseShared_HDRS}
+	${Bullet3CollisionNarrowPhase_HDRS}
+)
+
+ADD_LIBRARY(Bullet3Collision ${Bullet3Collision_SRCS} ${Bullet3Collision_HDRS})
+if (BUILD_SHARED_LIBS)
+  target_link_libraries(Bullet3Collision Bullet3Geometry)
+endif ()
+SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES SOVERSION ${BULLET_VERSION})
+
+IF (INSTALL_LIBS)
+	IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+		#FILES_MATCHING requires CMake 2.6
+		IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+			IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Collision DESTINATION .)
+			ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Collision
+					RUNTIME DESTINATION bin
+					LIBRARY DESTINATION lib${LIB_SUFFIX}
+					ARCHIVE DESTINATION lib${LIB_SUFFIX})
+				INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h"  PATTERN
+".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
+			ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+		ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+
+		IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+			SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES FRAMEWORK true)
+			SET_TARGET_PROPERTIES(Bullet3Collision PROPERTIES PUBLIC_HEADER "${Bullet3Collision_HDRS}")
+			# Have to list out sub-directories manually:
+			#todo
+			#SET_PROPERTY(SOURCE ${Bullet3CollisionBroadPhase_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/BroadPhaseCollision)
+
+		ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+	ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+ENDIF (INSTALL_LIBS)

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Config.h

@@ -0,0 +1,39 @@
+#ifndef B3_CONFIG_H
+#define B3_CONFIG_H
+
+struct b3Config
+{
+	int m_maxConvexBodies;
+	int m_maxConvexShapes;
+	int m_maxBroadphasePairs;
+	int m_maxContactCapacity;
+	int m_compoundPairCapacity;
+
+	int m_maxVerticesPerFace;
+	int m_maxFacesPerShape;
+	int m_maxConvexVertices;
+	int m_maxConvexIndices;
+	int m_maxConvexUniqueEdges;
+
+	int m_maxCompoundChildShapes;
+
+	int m_maxTriConvexPairCapacity;
+
+	b3Config()
+		: m_maxConvexBodies(128 * 1024),
+		  m_maxVerticesPerFace(64),
+		  m_maxFacesPerShape(12),
+		  m_maxConvexVertices(8192),
+		  m_maxConvexIndices(81920),
+		  m_maxConvexUniqueEdges(8192),
+		  m_maxCompoundChildShapes(8192),
+		  m_maxTriConvexPairCapacity(256 * 1024)
+	{
+		m_maxConvexShapes = m_maxConvexBodies;
+		m_maxBroadphasePairs = 16 * m_maxConvexBodies;
+		m_maxContactCapacity = m_maxBroadphasePairs;
+		m_compoundPairCapacity = 1024 * 1024;
+	}
+};
+
+#endif  //B3_CONFIG_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3Contact4.h

@@ -0,0 +1,55 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_CONTACT4_H
+#define B3_CONTACT4_H
+
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
+
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3Contact4 : public b3Contact4Data
+{
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	int getBodyA() const { return abs(m_bodyAPtrAndSignBit); }
+	int getBodyB() const { return abs(m_bodyBPtrAndSignBit); }
+	bool isBodyAFixed() const { return m_bodyAPtrAndSignBit < 0; }
+	bool isBodyBFixed() const { return m_bodyBPtrAndSignBit < 0; }
+	//	todo. make it safer
+	int& getBatchIdx() { return m_batchIdx; }
+	const int& getBatchIdx() const { return m_batchIdx; }
+	float getRestituitionCoeff() const { return ((float)m_restituitionCoeffCmp / (float)0xffff); }
+	void setRestituitionCoeff(float c)
+	{
+		b3Assert(c >= 0.f && c <= 1.f);
+		m_restituitionCoeffCmp = (unsigned short)(c * 0xffff);
+	}
+	float getFrictionCoeff() const { return ((float)m_frictionCoeffCmp / (float)0xffff); }
+	void setFrictionCoeff(float c)
+	{
+		b3Assert(c >= 0.f && c <= 1.f);
+		m_frictionCoeffCmp = (unsigned short)(c * 0xffff);
+	}
+
+	//float& getNPoints() { return m_worldNormal[3]; }
+	int getNPoints() const { return (int)m_worldNormalOnB.w; }
+
+	float getPenetration(int idx) const { return m_worldPosB[idx].w; }
+
+	bool isInvalid() const { return (getBodyA() == 0 || getBodyB() == 0); }
+};
+
+#endif  //B3_CONTACT4_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.cpp

@@ -0,0 +1,500 @@
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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 Erwin Coumans
+
+#include "b3ConvexUtility.h"
+#include "Bullet3Geometry/b3ConvexHullComputer.h"
+#include "Bullet3Geometry/b3GrahamScan2dConvexHull.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3HashMap.h"
+
+b3ConvexUtility::~b3ConvexUtility()
+{
+}
+
+bool b3ConvexUtility::initializePolyhedralFeatures(const b3Vector3* orgVertices, int numPoints, bool mergeCoplanarTriangles)
+{
+	b3ConvexHullComputer conv;
+	conv.compute(&orgVertices[0].getX(), sizeof(b3Vector3), numPoints, 0.f, 0.f);
+
+	b3AlignedObjectArray<b3Vector3> faceNormals;
+	int numFaces = conv.faces.size();
+	faceNormals.resize(numFaces);
+	b3ConvexHullComputer* convexUtil = &conv;
+
+	b3AlignedObjectArray<b3MyFace> tmpFaces;
+	tmpFaces.resize(numFaces);
+
+	int numVertices = convexUtil->vertices.size();
+	m_vertices.resize(numVertices);
+	for (int p = 0; p < numVertices; p++)
+	{
+		m_vertices[p] = convexUtil->vertices[p];
+	}
+
+	for (int i = 0; i < numFaces; i++)
+	{
+		int face = convexUtil->faces[i];
+		//printf("face=%d\n",face);
+		const b3ConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
+		const b3ConvexHullComputer::Edge* edge = firstEdge;
+
+		b3Vector3 edges[3];
+		int numEdges = 0;
+		//compute face normals
+
+		do
+		{
+			int src = edge->getSourceVertex();
+			tmpFaces[i].m_indices.push_back(src);
+			int targ = edge->getTargetVertex();
+			b3Vector3 wa = convexUtil->vertices[src];
+
+			b3Vector3 wb = convexUtil->vertices[targ];
+			b3Vector3 newEdge = wb - wa;
+			newEdge.normalize();
+			if (numEdges < 2)
+				edges[numEdges++] = newEdge;
+
+			edge = edge->getNextEdgeOfFace();
+		} while (edge != firstEdge);
+
+		b3Scalar planeEq = 1e30f;
+
+		if (numEdges == 2)
+		{
+			faceNormals[i] = edges[0].cross(edges[1]);
+			faceNormals[i].normalize();
+			tmpFaces[i].m_plane[0] = faceNormals[i].getX();
+			tmpFaces[i].m_plane[1] = faceNormals[i].getY();
+			tmpFaces[i].m_plane[2] = faceNormals[i].getZ();
+			tmpFaces[i].m_plane[3] = planeEq;
+		}
+		else
+		{
+			b3Assert(0);  //degenerate?
+			faceNormals[i].setZero();
+		}
+
+		for (int v = 0; v < tmpFaces[i].m_indices.size(); v++)
+		{
+			b3Scalar eq = m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]);
+			if (planeEq > eq)
+			{
+				planeEq = eq;
+			}
+		}
+		tmpFaces[i].m_plane[3] = -planeEq;
+	}
+
+	//merge coplanar faces and copy them to m_polyhedron
+
+	b3Scalar faceWeldThreshold = 0.999f;
+	b3AlignedObjectArray<int> todoFaces;
+	for (int i = 0; i < tmpFaces.size(); i++)
+		todoFaces.push_back(i);
+
+	while (todoFaces.size())
+	{
+		b3AlignedObjectArray<int> coplanarFaceGroup;
+		int refFace = todoFaces[todoFaces.size() - 1];
+
+		coplanarFaceGroup.push_back(refFace);
+		b3MyFace& faceA = tmpFaces[refFace];
+		todoFaces.pop_back();
+
+		b3Vector3 faceNormalA = b3MakeVector3(faceA.m_plane[0], faceA.m_plane[1], faceA.m_plane[2]);
+		for (int j = todoFaces.size() - 1; j >= 0; j--)
+		{
+			int i = todoFaces[j];
+			b3MyFace& faceB = tmpFaces[i];
+			b3Vector3 faceNormalB = b3MakeVector3(faceB.m_plane[0], faceB.m_plane[1], faceB.m_plane[2]);
+			if (faceNormalA.dot(faceNormalB) > faceWeldThreshold)
+			{
+				coplanarFaceGroup.push_back(i);
+				todoFaces.remove(i);
+			}
+		}
+
+		bool did_merge = false;
+		if (coplanarFaceGroup.size() > 1)
+		{
+			//do the merge: use Graham Scan 2d convex hull
+
+			b3AlignedObjectArray<b3GrahamVector3> orgpoints;
+			b3Vector3 averageFaceNormal = b3MakeVector3(0, 0, 0);
+
+			for (int i = 0; i < coplanarFaceGroup.size(); i++)
+			{
+				//				m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]);
+
+				b3MyFace& face = tmpFaces[coplanarFaceGroup[i]];
+				b3Vector3 faceNormal = b3MakeVector3(face.m_plane[0], face.m_plane[1], face.m_plane[2]);
+				averageFaceNormal += faceNormal;
+				for (int f = 0; f < face.m_indices.size(); f++)
+				{
+					int orgIndex = face.m_indices[f];
+					b3Vector3 pt = m_vertices[orgIndex];
+
+					bool found = false;
+
+					for (int i = 0; i < orgpoints.size(); i++)
+					{
+						//if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001))
+						if (orgpoints[i].m_orgIndex == orgIndex)
+						{
+							found = true;
+							break;
+						}
+					}
+					if (!found)
+						orgpoints.push_back(b3GrahamVector3(pt, orgIndex));
+				}
+			}
+
+			b3MyFace combinedFace;
+			for (int i = 0; i < 4; i++)
+				combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i];
+
+			b3AlignedObjectArray<b3GrahamVector3> hull;
+
+			averageFaceNormal.normalize();
+			b3GrahamScanConvexHull2D(orgpoints, hull, averageFaceNormal);
+
+			for (int i = 0; i < hull.size(); i++)
+			{
+				combinedFace.m_indices.push_back(hull[i].m_orgIndex);
+				for (int k = 0; k < orgpoints.size(); k++)
+				{
+					if (orgpoints[k].m_orgIndex == hull[i].m_orgIndex)
+					{
+						orgpoints[k].m_orgIndex = -1;  // invalidate...
+						break;
+					}
+				}
+			}
+
+			// are there rejected vertices?
+			bool reject_merge = false;
+
+			for (int i = 0; i < orgpoints.size(); i++)
+			{
+				if (orgpoints[i].m_orgIndex == -1)
+					continue;  // this is in the hull...
+				// this vertex is rejected -- is anybody else using this vertex?
+				for (int j = 0; j < tmpFaces.size(); j++)
+				{
+					b3MyFace& face = tmpFaces[j];
+					// is this a face of the current coplanar group?
+					bool is_in_current_group = false;
+					for (int k = 0; k < coplanarFaceGroup.size(); k++)
+					{
+						if (coplanarFaceGroup[k] == j)
+						{
+							is_in_current_group = true;
+							break;
+						}
+					}
+					if (is_in_current_group)  // ignore this face...
+						continue;
+					// does this face use this rejected vertex?
+					for (int v = 0; v < face.m_indices.size(); v++)
+					{
+						if (face.m_indices[v] == orgpoints[i].m_orgIndex)
+						{
+							// this rejected vertex is used in another face -- reject merge
+							reject_merge = true;
+							break;
+						}
+					}
+					if (reject_merge)
+						break;
+				}
+				if (reject_merge)
+					break;
+			}
+
+			if (!reject_merge)
+			{
+				// do this merge!
+				did_merge = true;
+				m_faces.push_back(combinedFace);
+			}
+		}
+		if (!did_merge)
+		{
+			for (int i = 0; i < coplanarFaceGroup.size(); i++)
+			{
+				b3MyFace face = tmpFaces[coplanarFaceGroup[i]];
+				m_faces.push_back(face);
+			}
+		}
+	}
+
+	initialize();
+
+	return true;
+}
+
+inline bool IsAlmostZero(const b3Vector3& v)
+{
+	if (fabsf(v.getX()) > 1e-6 || fabsf(v.getY()) > 1e-6 || fabsf(v.getZ()) > 1e-6) return false;
+	return true;
+}
+
+struct b3InternalVertexPair
+{
+	b3InternalVertexPair(short int v0, short int v1)
+		: m_v0(v0),
+		  m_v1(v1)
+	{
+		if (m_v1 > m_v0)
+			b3Swap(m_v0, m_v1);
+	}
+	short int m_v0;
+	short int m_v1;
+	int getHash() const
+	{
+		return m_v0 + (m_v1 << 16);
+	}
+	bool equals(const b3InternalVertexPair& other) const
+	{
+		return m_v0 == other.m_v0 && m_v1 == other.m_v1;
+	}
+};
+
+struct b3InternalEdge
+{
+	b3InternalEdge()
+		: m_face0(-1),
+		  m_face1(-1)
+	{
+	}
+	short int m_face0;
+	short int m_face1;
+};
+
+//
+
+#ifdef TEST_INTERNAL_OBJECTS
+bool b3ConvexUtility::testContainment() const
+{
+	for (int p = 0; p < 8; p++)
+	{
+		b3Vector3 LocalPt;
+		if (p == 0)
+			LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], m_extents[2]);
+		else if (p == 1)
+			LocalPt = m_localCenter + b3Vector3(m_extents[0], m_extents[1], -m_extents[2]);
+		else if (p == 2)
+			LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], m_extents[2]);
+		else if (p == 3)
+			LocalPt = m_localCenter + b3Vector3(m_extents[0], -m_extents[1], -m_extents[2]);
+		else if (p == 4)
+			LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], m_extents[2]);
+		else if (p == 5)
+			LocalPt = m_localCenter + b3Vector3(-m_extents[0], m_extents[1], -m_extents[2]);
+		else if (p == 6)
+			LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], m_extents[2]);
+		else if (p == 7)
+			LocalPt = m_localCenter + b3Vector3(-m_extents[0], -m_extents[1], -m_extents[2]);
+
+		for (int i = 0; i < m_faces.size(); i++)
+		{
+			const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
+			const b3Scalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3];
+			if (d > 0.0f)
+				return false;
+		}
+	}
+	return true;
+}
+#endif
+
+void b3ConvexUtility::initialize()
+{
+	b3HashMap<b3InternalVertexPair, b3InternalEdge> edges;
+
+	b3Scalar TotalArea = 0.0f;
+
+	m_localCenter.setValue(0, 0, 0);
+	for (int i = 0; i < m_faces.size(); i++)
+	{
+		int numVertices = m_faces[i].m_indices.size();
+		int NbTris = numVertices;
+		for (int j = 0; j < NbTris; j++)
+		{
+			int k = (j + 1) % numVertices;
+			b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
+			b3InternalEdge* edptr = edges.find(vp);
+			b3Vector3 edge = m_vertices[vp.m_v1] - m_vertices[vp.m_v0];
+			edge.normalize();
+
+			bool found = false;
+			b3Vector3 diff, diff2;
+
+			for (int p = 0; p < m_uniqueEdges.size(); p++)
+			{
+				diff = m_uniqueEdges[p] - edge;
+				diff2 = m_uniqueEdges[p] + edge;
+
+				//	if ((diff.length2()==0.f) ||
+				//	(diff2.length2()==0.f))
+
+				if (IsAlmostZero(diff) ||
+					IsAlmostZero(diff2))
+				{
+					found = true;
+					break;
+				}
+			}
+
+			if (!found)
+			{
+				m_uniqueEdges.push_back(edge);
+			}
+
+			if (edptr)
+			{
+				//TBD: figure out why I added this assert
+				//				b3Assert(edptr->m_face0>=0);
+				//			b3Assert(edptr->m_face1<0);
+				edptr->m_face1 = i;
+			}
+			else
+			{
+				b3InternalEdge ed;
+				ed.m_face0 = i;
+				edges.insert(vp, ed);
+			}
+		}
+	}
+
+#ifdef USE_CONNECTED_FACES
+	for (int i = 0; i < m_faces.size(); i++)
+	{
+		int numVertices = m_faces[i].m_indices.size();
+		m_faces[i].m_connectedFaces.resize(numVertices);
+
+		for (int j = 0; j < numVertices; j++)
+		{
+			int k = (j + 1) % numVertices;
+			b3InternalVertexPair vp(m_faces[i].m_indices[j], m_faces[i].m_indices[k]);
+			b3InternalEdge* edptr = edges.find(vp);
+			b3Assert(edptr);
+			b3Assert(edptr->m_face0 >= 0);
+			b3Assert(edptr->m_face1 >= 0);
+
+			int connectedFace = (edptr->m_face0 == i) ? edptr->m_face1 : edptr->m_face0;
+			m_faces[i].m_connectedFaces[j] = connectedFace;
+		}
+	}
+#endif  //USE_CONNECTED_FACES
+
+	for (int i = 0; i < m_faces.size(); i++)
+	{
+		int numVertices = m_faces[i].m_indices.size();
+		int NbTris = numVertices - 2;
+
+		const b3Vector3& p0 = m_vertices[m_faces[i].m_indices[0]];
+		for (int j = 1; j <= NbTris; j++)
+		{
+			int k = (j + 1) % numVertices;
+			const b3Vector3& p1 = m_vertices[m_faces[i].m_indices[j]];
+			const b3Vector3& p2 = m_vertices[m_faces[i].m_indices[k]];
+			b3Scalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f;
+			b3Vector3 Center = (p0 + p1 + p2) / 3.0f;
+			m_localCenter += Area * Center;
+			TotalArea += Area;
+		}
+	}
+	m_localCenter /= TotalArea;
+
+#ifdef TEST_INTERNAL_OBJECTS
+	if (1)
+	{
+		m_radius = FLT_MAX;
+		for (int i = 0; i < m_faces.size(); i++)
+		{
+			const b3Vector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]);
+			const b3Scalar dist = b3Fabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]);
+			if (dist < m_radius)
+				m_radius = dist;
+		}
+
+		b3Scalar MinX = FLT_MAX;
+		b3Scalar MinY = FLT_MAX;
+		b3Scalar MinZ = FLT_MAX;
+		b3Scalar MaxX = -FLT_MAX;
+		b3Scalar MaxY = -FLT_MAX;
+		b3Scalar MaxZ = -FLT_MAX;
+		for (int i = 0; i < m_vertices.size(); i++)
+		{
+			const b3Vector3& pt = m_vertices[i];
+			if (pt.getX() < MinX) MinX = pt.getX();
+			if (pt.getX() > MaxX) MaxX = pt.getX();
+			if (pt.getY() < MinY) MinY = pt.getY();
+			if (pt.getY() > MaxY) MaxY = pt.getY();
+			if (pt.getZ() < MinZ) MinZ = pt.getZ();
+			if (pt.getZ() > MaxZ) MaxZ = pt.getZ();
+		}
+		mC.setValue(MaxX + MinX, MaxY + MinY, MaxZ + MinZ);
+		mE.setValue(MaxX - MinX, MaxY - MinY, MaxZ - MinZ);
+
+		//		const b3Scalar r = m_radius / sqrtf(2.0f);
+		const b3Scalar r = m_radius / sqrtf(3.0f);
+		const int LargestExtent = mE.maxAxis();
+		const b3Scalar Step = (mE[LargestExtent] * 0.5f - r) / 1024.0f;
+		m_extents[0] = m_extents[1] = m_extents[2] = r;
+		m_extents[LargestExtent] = mE[LargestExtent] * 0.5f;
+		bool FoundBox = false;
+		for (int j = 0; j < 1024; j++)
+		{
+			if (testContainment())
+			{
+				FoundBox = true;
+				break;
+			}
+
+			m_extents[LargestExtent] -= Step;
+		}
+		if (!FoundBox)
+		{
+			m_extents[0] = m_extents[1] = m_extents[2] = r;
+		}
+		else
+		{
+			// Refine the box
+			const b3Scalar Step = (m_radius - r) / 1024.0f;
+			const int e0 = (1 << LargestExtent) & 3;
+			const int e1 = (1 << e0) & 3;
+
+			for (int j = 0; j < 1024; j++)
+			{
+				const b3Scalar Saved0 = m_extents[e0];
+				const b3Scalar Saved1 = m_extents[e1];
+				m_extents[e0] += Step;
+				m_extents[e1] += Step;
+
+				if (!testContainment())
+				{
+					m_extents[e0] = Saved0;
+					m_extents[e1] = Saved1;
+					break;
+				}
+			}
+		}
+	}
+#endif
+}

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h

@@ -0,0 +1,55 @@
+
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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 Erwin Coumans
+
+#ifndef _BT_CONVEX_UTILITY_H
+#define _BT_CONVEX_UTILITY_H
+
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3Common/b3Transform.h"
+
+struct b3MyFace
+{
+	b3AlignedObjectArray<int> m_indices;
+	b3Scalar m_plane[4];
+};
+
+B3_ATTRIBUTE_ALIGNED16(class)
+b3ConvexUtility
+{
+public:
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	b3Vector3 m_localCenter;
+	b3Vector3 m_extents;
+	b3Vector3 mC;
+	b3Vector3 mE;
+	b3Scalar m_radius;
+
+	b3AlignedObjectArray<b3Vector3> m_vertices;
+	b3AlignedObjectArray<b3MyFace> m_faces;
+	b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
+
+	b3ConvexUtility()
+	{
+	}
+	virtual ~b3ConvexUtility();
+
+	bool initializePolyhedralFeatures(const b3Vector3* orgVertices, int numVertices, bool mergeCoplanarTriangles = true);
+
+	void initialize();
+	bool testContainment() const;
+};
+#endif

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.cpp

@@ -0,0 +1,297 @@
+#include "b3CpuNarrowPhase.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3ConvexUtility.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
+
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h"
+
+struct b3CpuNarrowPhaseInternalData
+{
+	b3AlignedObjectArray<b3Aabb> m_localShapeAABBCPU;
+	b3AlignedObjectArray<b3Collidable> m_collidablesCPU;
+	b3AlignedObjectArray<b3ConvexUtility*> m_convexData;
+	b3Config m_config;
+
+	b3AlignedObjectArray<b3ConvexPolyhedronData> m_convexPolyhedra;
+	b3AlignedObjectArray<b3Vector3> m_uniqueEdges;
+	b3AlignedObjectArray<b3Vector3> m_convexVertices;
+	b3AlignedObjectArray<int> m_convexIndices;
+	b3AlignedObjectArray<b3GpuFace> m_convexFaces;
+
+	b3AlignedObjectArray<b3Contact4Data> m_contacts;
+
+	int m_numAcceleratedShapes;
+};
+
+const b3AlignedObjectArray<b3Contact4Data>& b3CpuNarrowPhase::getContacts() const
+{
+	return m_data->m_contacts;
+}
+
+b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex)
+{
+	return m_data->m_collidablesCPU[collidableIndex];
+}
+
+const b3Collidable& b3CpuNarrowPhase::getCollidableCpu(int collidableIndex) const
+{
+	return m_data->m_collidablesCPU[collidableIndex];
+}
+
+b3CpuNarrowPhase::b3CpuNarrowPhase(const struct b3Config& config)
+{
+	m_data = new b3CpuNarrowPhaseInternalData;
+	m_data->m_config = config;
+	m_data->m_numAcceleratedShapes = 0;
+}
+
+b3CpuNarrowPhase::~b3CpuNarrowPhase()
+{
+	delete m_data;
+}
+
+void b3CpuNarrowPhase::computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies)
+{
+	int nPairs = pairs.size();
+	int numContacts = 0;
+	int maxContactCapacity = m_data->m_config.m_maxContactCapacity;
+	m_data->m_contacts.resize(maxContactCapacity);
+
+	for (int i = 0; i < nPairs; i++)
+	{
+		int bodyIndexA = pairs[i].x;
+		int bodyIndexB = pairs[i].y;
+		int collidableIndexA = bodies[bodyIndexA].m_collidableIdx;
+		int collidableIndexB = bodies[bodyIndexB].m_collidableIdx;
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_SPHERE &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
+		{
+			//			computeContactSphereConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
+			//				&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_SPHERE)
+		{
+			//			computeContactSphereConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+			//				&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//printf("convex-sphere\n");
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
+		{
+			//			computeContactPlaneConvex(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			printf("convex-plane\n");
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
+		{
+			//			computeContactPlaneConvex(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			printf("plane-convex\n");
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
+		{
+			//			computeContactCompoundCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], hostAabbsWorldSpace,hostAabbsLocalSpace,hostVertices,hostUniqueEdges,hostIndices,hostFaces,&hostContacts[0],
+			//			nContacts,maxContactCapacity,treeNodesCPU,subTreesCPU,bvhInfoCPU);
+			//			printf("convex-plane\n");
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_PLANE)
+		{
+			//			computeContactPlaneCompound(i,bodyIndexB,bodyIndexA,collidableIndexB,collidableIndexA,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0], &hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			printf("convex-plane\n");
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_PLANE &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
+		{
+			//			computeContactPlaneCompound(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,&bodies[0],
+			//			&m_data->m_collidablesCPU[0],&hostConvexData[0],&cpuChildShapes[0],&hostVertices[0],&hostIndices[0],&hostFaces[0],&hostContacts[0],nContacts,maxContactCapacity);
+			//			printf("plane-convex\n");
+		}
+
+		if (m_data->m_collidablesCPU[collidableIndexA].m_shapeType == SHAPE_CONVEX_HULL &&
+			m_data->m_collidablesCPU[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
+		{
+			//printf("pairs[i].z=%d\n",pairs[i].z);
+			//int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,bodies,
+			//		m_data->m_collidablesCPU,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
+			int contactIndex = b3ContactConvexConvexSAT(i, bodyIndexA, bodyIndexB, collidableIndexA, collidableIndexB, bodies,
+														m_data->m_collidablesCPU, m_data->m_convexPolyhedra, m_data->m_convexVertices, m_data->m_uniqueEdges, m_data->m_convexIndices, m_data->m_convexFaces, m_data->m_contacts, numContacts, maxContactCapacity);
+
+			if (contactIndex >= 0)
+			{
+				pairs[i].z = contactIndex;
+			}
+			//			printf("plane-convex\n");
+		}
+	}
+
+	m_data->m_contacts.resize(numContacts);
+}
+
+int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
+{
+	int collidableIndex = allocateCollidable();
+	if (collidableIndex < 0)
+		return collidableIndex;
+
+	b3Collidable& col = m_data->m_collidablesCPU[collidableIndex];
+	col.m_shapeType = SHAPE_CONVEX_HULL;
+	col.m_shapeIndex = -1;
+
+	{
+		b3Vector3 localCenter = b3MakeVector3(0, 0, 0);
+		for (int i = 0; i < utilPtr->m_vertices.size(); i++)
+			localCenter += utilPtr->m_vertices[i];
+		localCenter *= (1.f / utilPtr->m_vertices.size());
+		utilPtr->m_localCenter = localCenter;
+
+		col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr, col);
+	}
+
+	if (col.m_shapeIndex >= 0)
+	{
+		b3Aabb aabb;
+
+		b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f);
+		b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f);
+
+		for (int i = 0; i < utilPtr->m_vertices.size(); i++)
+		{
+			myAabbMin.setMin(utilPtr->m_vertices[i]);
+			myAabbMax.setMax(utilPtr->m_vertices[i]);
+		}
+		aabb.m_min[0] = myAabbMin[0];
+		aabb.m_min[1] = myAabbMin[1];
+		aabb.m_min[2] = myAabbMin[2];
+		aabb.m_minIndices[3] = 0;
+
+		aabb.m_max[0] = myAabbMax[0];
+		aabb.m_max[1] = myAabbMax[1];
+		aabb.m_max[2] = myAabbMax[2];
+		aabb.m_signedMaxIndices[3] = 0;
+
+		m_data->m_localShapeAABBCPU.push_back(aabb);
+	}
+
+	return collidableIndex;
+}
+
+int b3CpuNarrowPhase::allocateCollidable()
+{
+	int curSize = m_data->m_collidablesCPU.size();
+	if (curSize < m_data->m_config.m_maxConvexShapes)
+	{
+		m_data->m_collidablesCPU.expand();
+		return curSize;
+	}
+	else
+	{
+		b3Error("allocateCollidable out-of-range %d\n", m_data->m_config.m_maxConvexShapes);
+	}
+	return -1;
+}
+
+int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
+{
+	b3AlignedObjectArray<b3Vector3> verts;
+
+	unsigned char* vts = (unsigned char*)vertices;
+	for (int i = 0; i < numVertices; i++)
+	{
+		float* vertex = (float*)&vts[i * strideInBytes];
+		verts.push_back(b3MakeVector3(vertex[0] * scaling[0], vertex[1] * scaling[1], vertex[2] * scaling[2]));
+	}
+
+	b3ConvexUtility* utilPtr = new b3ConvexUtility();
+	bool merge = true;
+	if (numVertices)
+	{
+		utilPtr->initializePolyhedralFeatures(&verts[0], verts.size(), merge);
+	}
+
+	int collidableIndex = registerConvexHullShape(utilPtr);
+
+	delete utilPtr;
+	return collidableIndex;
+}
+
+int b3CpuNarrowPhase::registerConvexHullShapeInternal(b3ConvexUtility* convexPtr, b3Collidable& col)
+{
+	m_data->m_convexData.resize(m_data->m_numAcceleratedShapes + 1);
+	m_data->m_convexPolyhedra.resize(m_data->m_numAcceleratedShapes + 1);
+
+	b3ConvexPolyhedronData& convex = m_data->m_convexPolyhedra.at(m_data->m_convexPolyhedra.size() - 1);
+	convex.mC = convexPtr->mC;
+	convex.mE = convexPtr->mE;
+	convex.m_extents = convexPtr->m_extents;
+	convex.m_localCenter = convexPtr->m_localCenter;
+	convex.m_radius = convexPtr->m_radius;
+
+	convex.m_numUniqueEdges = convexPtr->m_uniqueEdges.size();
+	int edgeOffset = m_data->m_uniqueEdges.size();
+	convex.m_uniqueEdgesOffset = edgeOffset;
+
+	m_data->m_uniqueEdges.resize(edgeOffset + convex.m_numUniqueEdges);
+
+	//convex data here
+	int i;
+	for (i = 0; i < convexPtr->m_uniqueEdges.size(); i++)
+	{
+		m_data->m_uniqueEdges[edgeOffset + i] = convexPtr->m_uniqueEdges[i];
+	}
+
+	int faceOffset = m_data->m_convexFaces.size();
+	convex.m_faceOffset = faceOffset;
+	convex.m_numFaces = convexPtr->m_faces.size();
+
+	m_data->m_convexFaces.resize(faceOffset + convex.m_numFaces);
+
+	for (i = 0; i < convexPtr->m_faces.size(); i++)
+	{
+		m_data->m_convexFaces[convex.m_faceOffset + i].m_plane = b3MakeVector3(convexPtr->m_faces[i].m_plane[0],
+																			   convexPtr->m_faces[i].m_plane[1],
+																			   convexPtr->m_faces[i].m_plane[2],
+																			   convexPtr->m_faces[i].m_plane[3]);
+
+		int indexOffset = m_data->m_convexIndices.size();
+		int numIndices = convexPtr->m_faces[i].m_indices.size();
+		m_data->m_convexFaces[convex.m_faceOffset + i].m_numIndices = numIndices;
+		m_data->m_convexFaces[convex.m_faceOffset + i].m_indexOffset = indexOffset;
+		m_data->m_convexIndices.resize(indexOffset + numIndices);
+		for (int p = 0; p < numIndices; p++)
+		{
+			m_data->m_convexIndices[indexOffset + p] = convexPtr->m_faces[i].m_indices[p];
+		}
+	}
+
+	convex.m_numVertices = convexPtr->m_vertices.size();
+	int vertexOffset = m_data->m_convexVertices.size();
+	convex.m_vertexOffset = vertexOffset;
+
+	m_data->m_convexVertices.resize(vertexOffset + convex.m_numVertices);
+	for (int i = 0; i < convexPtr->m_vertices.size(); i++)
+	{
+		m_data->m_convexVertices[vertexOffset + i] = convexPtr->m_vertices[i];
+	}
+
+	(m_data->m_convexData)[m_data->m_numAcceleratedShapes] = convexPtr;
+
+	return m_data->m_numAcceleratedShapes++;
+}
+
+const b3Aabb& b3CpuNarrowPhase::getLocalSpaceAabb(int collidableIndex) const
+{
+	return m_data->m_localShapeAABBCPU[collidableIndex];
+}

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h

@@ -0,0 +1,92 @@
+#ifndef B3_CPU_NARROWPHASE_H
+#define B3_CPU_NARROWPHASE_H
+
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
+
+class b3CpuNarrowPhase
+{
+protected:
+	struct b3CpuNarrowPhaseInternalData* m_data;
+	int m_acceleratedCompanionShapeIndex;
+	int m_planeBodyIndex;
+	int m_static0Index;
+
+	int registerConvexHullShapeInternal(class b3ConvexUtility* convexPtr, b3Collidable& col);
+	int registerConcaveMeshShape(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, b3Collidable& col, const float* scaling);
+
+public:
+	b3CpuNarrowPhase(const struct b3Config& config);
+
+	virtual ~b3CpuNarrowPhase(void);
+
+	int registerSphereShape(float radius);
+	int registerPlaneShape(const b3Vector3& planeNormal, float planeConstant);
+
+	int registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes);
+	int registerFace(const b3Vector3& faceNormal, float faceConstant);
+
+	int registerConcaveMesh(b3AlignedObjectArray<b3Vector3>* vertices, b3AlignedObjectArray<int>* indices, const float* scaling);
+
+	//do they need to be merged?
+
+	int registerConvexHullShape(b3ConvexUtility* utilPtr);
+	int registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling);
+
+	//int registerRigidBody(int collidableIndex, float mass, const float* position, const float* orientation, const float* aabbMin, const float* aabbMax,bool writeToGpu);
+	void setObjectTransform(const float* position, const float* orientation, int bodyIndex);
+
+	void writeAllBodiesToGpu();
+	void reset();
+	void readbackAllBodiesToCpu();
+	bool getObjectTransformFromCpu(float* position, float* orientation, int bodyIndex) const;
+
+	void setObjectTransformCpu(float* position, float* orientation, int bodyIndex);
+	void setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex);
+
+	//virtual void computeContacts(cl_mem broadphasePairs, int numBroadphasePairs, cl_mem aabbsWorldSpace, int numObjects);
+	virtual void computeContacts(b3AlignedObjectArray<b3Int4>& pairs, b3AlignedObjectArray<b3Aabb>& aabbsWorldSpace, b3AlignedObjectArray<b3RigidBodyData>& bodies);
+
+	const struct b3RigidBodyData* getBodiesCpu() const;
+	//struct b3RigidBodyData* getBodiesCpu();
+
+	int getNumBodiesGpu() const;
+
+	int getNumBodyInertiasGpu() const;
+
+	const struct b3Collidable* getCollidablesCpu() const;
+	int getNumCollidablesGpu() const;
+
+	/*const struct b3Contact4* getContactsCPU() const;
+
+	
+	int	getNumContactsGpu() const;
+	*/
+
+	const b3AlignedObjectArray<b3Contact4Data>& getContacts() const;
+
+	int getNumRigidBodies() const;
+
+	int allocateCollidable();
+
+	int getStatic0Index() const
+	{
+		return m_static0Index;
+	}
+	b3Collidable& getCollidableCpu(int collidableIndex);
+	const b3Collidable& getCollidableCpu(int collidableIndex) const;
+
+	const b3CpuNarrowPhaseInternalData* getInternalData() const
+	{
+		return m_data;
+	}
+
+	const struct b3Aabb& getLocalSpaceAabb(int collidableIndex) const;
+};
+
+#endif  //B3_CPU_NARROWPHASE_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RaycastInfo.h

@@ -0,0 +1,25 @@
+
+#ifndef B3_RAYCAST_INFO_H
+#define B3_RAYCAST_INFO_H
+
+#include "Bullet3Common/b3Vector3.h"
+
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3RayInfo
+{
+	b3Vector3 m_from;
+	b3Vector3 m_to;
+};
+
+B3_ATTRIBUTE_ALIGNED16(struct)
+b3RayHit
+{
+	b3Scalar m_hitFraction;
+	int m_hitBody;
+	int m_hitResult1;
+	int m_hitResult2;
+	b3Vector3 m_hitPoint;
+	b3Vector3 m_hitNormal;
+};
+
+#endif  //B3_RAYCAST_INFO_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h

@@ -0,0 +1,28 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_RIGID_BODY_CL
+#define B3_RIGID_BODY_CL
+
+#include "Bullet3Common/b3Scalar.h"
+#include "Bullet3Common/b3Matrix3x3.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+
+inline float b3GetInvMass(const b3RigidBodyData& body)
+{
+	return body.m_invMass;
+}
+
+#endif  //B3_RIGID_BODY_CL

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h

@@ -0,0 +1,19 @@
+
+#ifndef B3_BVH_SUBTREE_INFO_DATA_H
+#define B3_BVH_SUBTREE_INFO_DATA_H
+
+typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t;
+
+struct b3BvhSubtreeInfoData
+{
+	//12 bytes
+	unsigned short int m_quantizedAabbMin[3];
+	unsigned short int m_quantizedAabbMax[3];
+	//4 bytes, points to the root of the subtree
+	int m_rootNodeIndex;
+	//4 bytes
+	int m_subtreeSize;
+	int m_padding[3];
+};
+
+#endif  //B3_BVH_SUBTREE_INFO_DATA_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h

@@ -0,0 +1,123 @@
+
+
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
+
+// work-in-progress
+void b3BvhTraversal(__global const b3Int4* pairs,
+					__global const b3RigidBodyData* rigidBodies,
+					__global const b3Collidable* collidables,
+					__global b3Aabb* aabbs,
+					__global b3Int4* concavePairsOut,
+					__global volatile int* numConcavePairsOut,
+					__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
+					__global const b3QuantizedBvhNode* quantizedNodesRoot,
+					__global const b3BvhInfo* bvhInfos,
+					int numPairs,
+					int maxNumConcavePairsCapacity,
+					int id)
+{
+	int bodyIndexA = pairs[id].x;
+	int bodyIndexB = pairs[id].y;
+	int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
+	int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
+
+	//once the broadphase avoids static-static pairs, we can remove this test
+	if ((rigidBodies[bodyIndexA].m_invMass == 0) && (rigidBodies[bodyIndexB].m_invMass == 0))
+	{
+		return;
+	}
+
+	if (collidables[collidableIndexA].m_shapeType != SHAPE_CONCAVE_TRIMESH)
+		return;
+
+	int shapeTypeB = collidables[collidableIndexB].m_shapeType;
+
+	if (shapeTypeB != SHAPE_CONVEX_HULL &&
+		shapeTypeB != SHAPE_SPHERE &&
+		shapeTypeB != SHAPE_COMPOUND_OF_CONVEX_HULLS)
+		return;
+
+	b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
+
+	b3Float4 bvhAabbMin = bvhInfo.m_aabbMin;
+	b3Float4 bvhAabbMax = bvhInfo.m_aabbMax;
+	b3Float4 bvhQuantization = bvhInfo.m_quantization;
+	int numSubtreeHeaders = bvhInfo.m_numSubTrees;
+	__global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
+	__global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
+
+	unsigned short int quantizedQueryAabbMin[3];
+	unsigned short int quantizedQueryAabbMax[3];
+	b3QuantizeWithClamp(quantizedQueryAabbMin, aabbs[bodyIndexB].m_minVec, false, bvhAabbMin, bvhAabbMax, bvhQuantization);
+	b3QuantizeWithClamp(quantizedQueryAabbMax, aabbs[bodyIndexB].m_maxVec, true, bvhAabbMin, bvhAabbMax, bvhQuantization);
+
+	for (int i = 0; i < numSubtreeHeaders; i++)
+	{
+		b3BvhSubtreeInfoData subtree = subtreeHeaders[i];
+
+		int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, subtree.m_quantizedAabbMin, subtree.m_quantizedAabbMax);
+		if (overlap != 0)
+		{
+			int startNodeIndex = subtree.m_rootNodeIndex;
+			int endNodeIndex = subtree.m_rootNodeIndex + subtree.m_subtreeSize;
+			int curIndex = startNodeIndex;
+			int escapeIndex;
+			int isLeafNode;
+			int aabbOverlap;
+			while (curIndex < endNodeIndex)
+			{
+				b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex];
+				aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin, quantizedQueryAabbMax, rootNode.m_quantizedAabbMin, rootNode.m_quantizedAabbMax);
+				isLeafNode = b3IsLeaf(&rootNode);
+				if (aabbOverlap)
+				{
+					if (isLeafNode)
+					{
+						int triangleIndex = b3GetTriangleIndex(&rootNode);
+						if (shapeTypeB == SHAPE_COMPOUND_OF_CONVEX_HULLS)
+						{
+							int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
+							int pairIdx = b3AtomicAdd(numConcavePairsOut, numChildrenB);
+							for (int b = 0; b < numChildrenB; b++)
+							{
+								if ((pairIdx + b) < maxNumConcavePairsCapacity)
+								{
+									int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex + b;
+									b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, childShapeIndexB);
+									concavePairsOut[pairIdx + b] = newPair;
+								}
+							}
+						}
+						else
+						{
+							int pairIdx = b3AtomicInc(numConcavePairsOut);
+							if (pairIdx < maxNumConcavePairsCapacity)
+							{
+								b3Int4 newPair = b3MakeInt4(bodyIndexA, bodyIndexB, triangleIndex, 0);
+								concavePairsOut[pairIdx] = newPair;
+							}
+						}
+					}
+					curIndex++;
+				}
+				else
+				{
+					if (isLeafNode)
+					{
+						curIndex++;
+					}
+					else
+					{
+						escapeIndex = b3GetEscapeIndex(&rootNode);
+						curIndex += escapeIndex;
+					}
+				}
+			}
+		}
+	}
+}

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ClipFaces.h

@@ -0,0 +1,171 @@
+#ifndef B3_CLIP_FACES_H
+#define B3_CLIP_FACES_H
+
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
+
+inline b3Float4 b3Lerp3(b3Float4ConstArg a, b3Float4ConstArg b, float t)
+{
+	return b3MakeFloat4(a.x + (b.x - a.x) * t,
+						a.y + (b.y - a.y) * t,
+						a.z + (b.z - a.z) * t,
+						0.f);
+}
+
+// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
+int clipFaceGlobal(__global const b3Float4* pVtxIn, int numVertsIn, b3Float4ConstArg planeNormalWS, float planeEqWS, __global b3Float4* ppVtxOut)
+{
+	int ve;
+	float ds, de;
+	int numVertsOut = 0;
+	//double-check next test
+	//	if (numVertsIn < 2)
+	//		return 0;
+
+	b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
+	b3Float4 endVertex = pVtxIn[0];
+
+	ds = b3Dot(planeNormalWS, firstVertex) + planeEqWS;
+
+	for (ve = 0; ve < numVertsIn; ve++)
+	{
+		endVertex = pVtxIn[ve];
+		de = b3Dot(planeNormalWS, endVertex) + planeEqWS;
+		if (ds < 0)
+		{
+			if (de < 0)
+			{
+				// Start < 0, end < 0, so output endVertex
+				ppVtxOut[numVertsOut++] = endVertex;
+			}
+			else
+			{
+				// Start < 0, end >= 0, so output intersection
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
+			}
+		}
+		else
+		{
+			if (de < 0)
+			{
+				// Start >= 0, end < 0 so output intersection and end
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
+				ppVtxOut[numVertsOut++] = endVertex;
+			}
+		}
+		firstVertex = endVertex;
+		ds = de;
+	}
+	return numVertsOut;
+}
+
+__kernel void clipFacesAndFindContactsKernel(__global const b3Float4* separatingNormals,
+											 __global const int* hasSeparatingAxis,
+											 __global b3Int4* clippingFacesOut,
+											 __global b3Float4* worldVertsA1,
+											 __global b3Float4* worldNormalsA1,
+											 __global b3Float4* worldVertsB1,
+											 __global b3Float4* worldVertsB2,
+											 int vertexFaceCapacity,
+											 int pairIndex)
+{
+	//    int i = get_global_id(0);
+	//int pairIndex = i;
+	int i = pairIndex;
+
+	float minDist = -1e30f;
+	float maxDist = 0.02f;
+
+	//	if (i<numPairs)
+	{
+		if (hasSeparatingAxis[i])
+		{
+			//			int bodyIndexA = pairs[i].x;
+			//		int bodyIndexB = pairs[i].y;
+
+			int numLocalContactsOut = 0;
+
+			int capacityWorldVertsB2 = vertexFaceCapacity;
+
+			__global b3Float4* pVtxIn = &worldVertsB1[pairIndex * capacityWorldVertsB2];
+			__global b3Float4* pVtxOut = &worldVertsB2[pairIndex * capacityWorldVertsB2];
+
+			{
+				__global b3Int4* clippingFaces = clippingFacesOut;
+
+				int closestFaceA = clippingFaces[pairIndex].x;
+				// int closestFaceB = clippingFaces[pairIndex].y;
+				int numVertsInA = clippingFaces[pairIndex].z;
+				int numVertsInB = clippingFaces[pairIndex].w;
+
+				int numVertsOut = 0;
+
+				if (closestFaceA >= 0)
+				{
+					// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
+
+					for (int e0 = 0; e0 < numVertsInA; e0++)
+					{
+						const b3Float4 aw = worldVertsA1[pairIndex * capacityWorldVertsB2 + e0];
+						const b3Float4 bw = worldVertsA1[pairIndex * capacityWorldVertsB2 + ((e0 + 1) % numVertsInA)];
+						const b3Float4 WorldEdge0 = aw - bw;
+						b3Float4 worldPlaneAnormal1 = worldNormalsA1[pairIndex];
+						b3Float4 planeNormalWS1 = -b3Cross(WorldEdge0, worldPlaneAnormal1);
+						b3Float4 worldA1 = aw;
+						float planeEqWS1 = -b3Dot(worldA1, planeNormalWS1);
+						b3Float4 planeNormalWS = planeNormalWS1;
+						float planeEqWS = planeEqWS1;
+						numVertsOut = clipFaceGlobal(pVtxIn, numVertsInB, planeNormalWS, planeEqWS, pVtxOut);
+						__global b3Float4* tmp = pVtxOut;
+						pVtxOut = pVtxIn;
+						pVtxIn = tmp;
+						numVertsInB = numVertsOut;
+						numVertsOut = 0;
+					}
+
+					b3Float4 planeNormalWS = worldNormalsA1[pairIndex];
+					float planeEqWS = -b3Dot(planeNormalWS, worldVertsA1[pairIndex * capacityWorldVertsB2]);
+
+					for (int i = 0; i < numVertsInB; i++)
+					{
+						float depth = b3Dot(planeNormalWS, pVtxIn[i]) + planeEqWS;
+						if (depth <= minDist)
+						{
+							depth = minDist;
+						}
+						/*
+						static float maxDepth = 0.f;
+						if (depth < maxDepth)
+						{
+							maxDepth = depth;
+							if (maxDepth < -10)
+							{
+								printf("error at framecount %d?\n",myframecount);
+							}
+							printf("maxDepth = %f\n", maxDepth);
+
+						}
+*/
+						if (depth <= maxDist)
+						{
+							b3Float4 pointInWorld = pVtxIn[i];
+							pVtxOut[numLocalContactsOut++] = b3MakeFloat4(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
+						}
+					}
+				}
+				clippingFaces[pairIndex].w = numLocalContactsOut;
+			}
+
+			for (int i = 0; i < numLocalContactsOut; i++)
+				pVtxIn[i] = pVtxOut[i];
+
+		}  //		if (hasSeparatingAxis[i])
+	}      //	if (i<numPairs)
+}
+
+#endif  //B3_CLIP_FACES_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h

@@ -0,0 +1,69 @@
+
+#ifndef B3_COLLIDABLE_H
+#define B3_COLLIDABLE_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Common/shared/b3Quat.h"
+
+enum b3ShapeTypes
+{
+	SHAPE_HEIGHT_FIELD = 1,
+
+	SHAPE_CONVEX_HULL = 3,
+	SHAPE_PLANE = 4,
+	SHAPE_CONCAVE_TRIMESH = 5,
+	SHAPE_COMPOUND_OF_CONVEX_HULLS = 6,
+	SHAPE_SPHERE = 7,
+	MAX_NUM_SHAPE_TYPES,
+};
+
+typedef struct b3Collidable b3Collidable_t;
+
+struct b3Collidable
+{
+	union {
+		int m_numChildShapes;
+		int m_bvhIndex;
+	};
+	union {
+		float m_radius;
+		int m_compoundBvhIndex;
+	};
+
+	int m_shapeType;
+	union {
+		int m_shapeIndex;
+		float m_height;
+	};
+};
+
+typedef struct b3GpuChildShape b3GpuChildShape_t;
+struct b3GpuChildShape
+{
+	b3Float4 m_childPosition;
+	b3Quat m_childOrientation;
+	union {
+		int m_shapeIndex;  //used for SHAPE_COMPOUND_OF_CONVEX_HULLS
+		int m_capsuleAxis;
+	};
+	union {
+		float m_radius;        //used for childshape of SHAPE_COMPOUND_OF_SPHERES or SHAPE_COMPOUND_OF_CAPSULES
+		int m_numChildShapes;  //used for compound shape
+	};
+	union {
+		float m_height;  //used for childshape of SHAPE_COMPOUND_OF_CAPSULES
+		int m_collidableShapeIndex;
+	};
+	int m_shapeType;
+};
+
+struct b3CompoundOverlappingPair
+{
+	int m_bodyIndexA;
+	int m_bodyIndexB;
+	//	int	m_pairType;
+	int m_childShapeIndexA;
+	int m_childShapeIndexB;
+};
+
+#endif  //B3_COLLIDABLE_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h

@@ -0,0 +1,36 @@
+#ifndef B3_CONTACT4DATA_H
+#define B3_CONTACT4DATA_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+
+typedef struct b3Contact4Data b3Contact4Data_t;
+
+struct b3Contact4Data
+{
+	b3Float4 m_worldPosB[4];
+	//	b3Float4	m_localPosA[4];
+	//	b3Float4	m_localPosB[4];
+	b3Float4 m_worldNormalOnB;  //	w: m_nPoints
+	unsigned short m_restituitionCoeffCmp;
+	unsigned short m_frictionCoeffCmp;
+	int m_batchIdx;
+	int m_bodyAPtrAndSignBit;  //x:m_bodyAPtr, y:m_bodyBPtr
+	int m_bodyBPtrAndSignBit;
+
+	int m_childIndexA;
+	int m_childIndexB;
+	int m_unused1;
+	int m_unused2;
+};
+
+inline int b3Contact4Data_getNumPoints(const struct b3Contact4Data* contact)
+{
+	return (int)contact->m_worldNormalOnB.w;
+};
+
+inline void b3Contact4Data_setNumPoints(struct b3Contact4Data* contact, int numPoints)
+{
+	contact->m_worldNormalOnB.w = (float)numPoints;
+};
+
+#endif  //B3_CONTACT4DATA_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactConvexConvexSAT.h

@@ -0,0 +1,486 @@
+
+#ifndef B3_CONTACT_CONVEX_CONVEX_SAT_H
+#define B3_CONTACT_CONVEX_CONVEX_SAT_H
+
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h"
+
+#define B3_MAX_VERTS 1024
+
+inline b3Float4 b3Lerp3(const b3Float4& a, const b3Float4& b, float t)
+{
+	return b3MakeVector3(a.x + (b.x - a.x) * t,
+						 a.y + (b.y - a.y) * t,
+						 a.z + (b.z - a.z) * t,
+						 0.f);
+}
+
+// Clips a face to the back of a plane, return the number of vertices out, stored in ppVtxOut
+inline int b3ClipFace(const b3Float4* pVtxIn, int numVertsIn, b3Float4& planeNormalWS, float planeEqWS, b3Float4* ppVtxOut)
+{
+	int ve;
+	float ds, de;
+	int numVertsOut = 0;
+	if (numVertsIn < 2)
+		return 0;
+
+	b3Float4 firstVertex = pVtxIn[numVertsIn - 1];
+	b3Float4 endVertex = pVtxIn[0];
+
+	ds = b3Dot3F4(planeNormalWS, firstVertex) + planeEqWS;
+
+	for (ve = 0; ve < numVertsIn; ve++)
+	{
+		endVertex = pVtxIn[ve];
+
+		de = b3Dot3F4(planeNormalWS, endVertex) + planeEqWS;
+
+		if (ds < 0)
+		{
+			if (de < 0)
+			{
+				// Start < 0, end < 0, so output endVertex
+				ppVtxOut[numVertsOut++] = endVertex;
+			}
+			else
+			{
+				// Start < 0, end >= 0, so output intersection
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
+			}
+		}
+		else
+		{
+			if (de < 0)
+			{
+				// Start >= 0, end < 0 so output intersection and end
+				ppVtxOut[numVertsOut++] = b3Lerp3(firstVertex, endVertex, (ds * 1.f / (ds - de)));
+				ppVtxOut[numVertsOut++] = endVertex;
+			}
+		}
+		firstVertex = endVertex;
+		ds = de;
+	}
+	return numVertsOut;
+}
+
+inline int b3ClipFaceAgainstHull(const b3Float4& separatingNormal, const b3ConvexPolyhedronData* hullA,
+								 const b3Float4& posA, const b3Quaternion& ornA, b3Float4* worldVertsB1, int numWorldVertsB1,
+								 b3Float4* worldVertsB2, int capacityWorldVertsB2,
+								 const float minDist, float maxDist,
+								 const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
+								 //const b3Float4* verticesB,	const b3GpuFace* facesB,	const int* indicesB,
+								 b3Float4* contactsOut,
+								 int contactCapacity)
+{
+	int numContactsOut = 0;
+
+	b3Float4* pVtxIn = worldVertsB1;
+	b3Float4* pVtxOut = worldVertsB2;
+
+	int numVertsIn = numWorldVertsB1;
+	int numVertsOut = 0;
+
+	int closestFaceA = -1;
+	{
+		float dmin = FLT_MAX;
+		for (int face = 0; face < hullA->m_numFaces; face++)
+		{
+			const b3Float4 Normal = b3MakeVector3(
+				facesA[hullA->m_faceOffset + face].m_plane.x,
+				facesA[hullA->m_faceOffset + face].m_plane.y,
+				facesA[hullA->m_faceOffset + face].m_plane.z, 0.f);
+			const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
+
+			float d = b3Dot3F4(faceANormalWS, separatingNormal);
+			if (d < dmin)
+			{
+				dmin = d;
+				closestFaceA = face;
+			}
+		}
+	}
+	if (closestFaceA < 0)
+		return numContactsOut;
+
+	b3GpuFace polyA = facesA[hullA->m_faceOffset + closestFaceA];
+
+	// clip polygon to back of planes of all faces of hull A that are adjacent to witness face
+	//int numContacts = numWorldVertsB1;
+	int numVerticesA = polyA.m_numIndices;
+	for (int e0 = 0; e0 < numVerticesA; e0++)
+	{
+		const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + e0]];
+		const b3Float4 b = verticesA[hullA->m_vertexOffset + indicesA[polyA.m_indexOffset + ((e0 + 1) % numVerticesA)]];
+		const b3Float4 edge0 = a - b;
+		const b3Float4 WorldEdge0 = b3QuatRotate(ornA, edge0);
+		b3Float4 planeNormalA = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
+		b3Float4 worldPlaneAnormal1 = b3QuatRotate(ornA, planeNormalA);
+
+		b3Float4 planeNormalWS1 = -b3Cross3(WorldEdge0, worldPlaneAnormal1);
+		b3Float4 worldA1 = b3TransformPoint(a, posA, ornA);
+		float planeEqWS1 = -b3Dot3F4(worldA1, planeNormalWS1);
+
+		b3Float4 planeNormalWS = planeNormalWS1;
+		float planeEqWS = planeEqWS1;
+
+		//clip face
+		//clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS);
+		numVertsOut = b3ClipFace(pVtxIn, numVertsIn, planeNormalWS, planeEqWS, pVtxOut);
+
+		//btSwap(pVtxIn,pVtxOut);
+		b3Float4* tmp = pVtxOut;
+		pVtxOut = pVtxIn;
+		pVtxIn = tmp;
+		numVertsIn = numVertsOut;
+		numVertsOut = 0;
+	}
+
+	// only keep points that are behind the witness face
+	{
+		b3Float4 localPlaneNormal = b3MakeFloat4(polyA.m_plane.x, polyA.m_plane.y, polyA.m_plane.z, 0.f);
+		float localPlaneEq = polyA.m_plane.w;
+		b3Float4 planeNormalWS = b3QuatRotate(ornA, localPlaneNormal);
+		float planeEqWS = localPlaneEq - b3Dot3F4(planeNormalWS, posA);
+		for (int i = 0; i < numVertsIn; i++)
+		{
+			float depth = b3Dot3F4(planeNormalWS, pVtxIn[i]) + planeEqWS;
+			if (depth <= minDist)
+			{
+				depth = minDist;
+			}
+			if (numContactsOut < contactCapacity)
+			{
+				if (depth <= maxDist)
+				{
+					b3Float4 pointInWorld = pVtxIn[i];
+					//resultOut.addContactPoint(separatingNormal,point,depth);
+					contactsOut[numContactsOut++] = b3MakeVector3(pointInWorld.x, pointInWorld.y, pointInWorld.z, depth);
+					//printf("depth=%f\n",depth);
+				}
+			}
+			else
+			{
+				b3Error("exceeding contact capacity (%d,%df)\n", numContactsOut, contactCapacity);
+			}
+		}
+	}
+
+	return numContactsOut;
+}
+
+inline int b3ClipHullAgainstHull(const b3Float4& separatingNormal,
+								 const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
+								 const b3Float4& posA, const b3Quaternion& ornA, const b3Float4& posB, const b3Quaternion& ornB,
+								 b3Float4* worldVertsB1, b3Float4* worldVertsB2, int capacityWorldVerts,
+								 const float minDist, float maxDist,
+								 const b3AlignedObjectArray<b3Float4>& verticesA, const b3AlignedObjectArray<b3GpuFace>& facesA, const b3AlignedObjectArray<int>& indicesA,
+								 const b3AlignedObjectArray<b3Float4>& verticesB, const b3AlignedObjectArray<b3GpuFace>& facesB, const b3AlignedObjectArray<int>& indicesB,
+
+								 b3Float4* contactsOut,
+								 int contactCapacity)
+{
+	int numContactsOut = 0;
+	int numWorldVertsB1 = 0;
+
+	B3_PROFILE("clipHullAgainstHull");
+
+	//float curMaxDist=maxDist;
+	int closestFaceB = -1;
+	float dmax = -FLT_MAX;
+
+	{
+		//B3_PROFILE("closestFaceB");
+		if (hullB.m_numFaces != 1)
+		{
+			//printf("wtf\n");
+		}
+		static bool once = true;
+		//printf("separatingNormal=%f,%f,%f\n",separatingNormal.x,separatingNormal.y,separatingNormal.z);
+
+		for (int face = 0; face < hullB.m_numFaces; face++)
+		{
+#ifdef BT_DEBUG_SAT_FACE
+			if (once)
+				printf("face %d\n", face);
+			const b3GpuFace* faceB = &facesB[hullB.m_faceOffset + face];
+			if (once)
+			{
+				for (int i = 0; i < faceB->m_numIndices; i++)
+				{
+					b3Float4 vert = verticesB[hullB.m_vertexOffset + indicesB[faceB->m_indexOffset + i]];
+					printf("vert[%d] = %f,%f,%f\n", i, vert.x, vert.y, vert.z);
+				}
+			}
+#endif  //BT_DEBUG_SAT_FACE \
+	//if (facesB[hullB.m_faceOffset+face].m_numIndices>2)
+			{
+				const b3Float4 Normal = b3MakeVector3(facesB[hullB.m_faceOffset + face].m_plane.x,
+													  facesB[hullB.m_faceOffset + face].m_plane.y, facesB[hullB.m_faceOffset + face].m_plane.z, 0.f);
+				const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
+#ifdef BT_DEBUG_SAT_FACE
+				if (once)
+					printf("faceNormal = %f,%f,%f\n", Normal.x, Normal.y, Normal.z);
+#endif
+				float d = b3Dot3F4(WorldNormal, separatingNormal);
+				if (d > dmax)
+				{
+					dmax = d;
+					closestFaceB = face;
+				}
+			}
+		}
+		once = false;
+	}
+
+	b3Assert(closestFaceB >= 0);
+	{
+		//B3_PROFILE("worldVertsB1");
+		const b3GpuFace& polyB = facesB[hullB.m_faceOffset + closestFaceB];
+		const int numVertices = polyB.m_numIndices;
+		for (int e0 = 0; e0 < numVertices; e0++)
+		{
+			const b3Float4& b = verticesB[hullB.m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
+			worldVertsB1[numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
+		}
+	}
+
+	if (closestFaceB >= 0)
+	{
+		//B3_PROFILE("clipFaceAgainstHull");
+		numContactsOut = b3ClipFaceAgainstHull((b3Float4&)separatingNormal, &hullA,
+											   posA, ornA,
+											   worldVertsB1, numWorldVertsB1, worldVertsB2, capacityWorldVerts, minDist, maxDist,
+											   verticesA, facesA, indicesA,
+											   contactsOut, contactCapacity);
+	}
+
+	return numContactsOut;
+}
+
+inline int b3ClipHullHullSingle(
+	int bodyIndexA, int bodyIndexB,
+	const b3Float4& posA,
+	const b3Quaternion& ornA,
+	const b3Float4& posB,
+	const b3Quaternion& ornB,
+
+	int collidableIndexA, int collidableIndexB,
+
+	const b3AlignedObjectArray<b3RigidBodyData>* bodyBuf,
+	b3AlignedObjectArray<b3Contact4Data>* globalContactOut,
+	int& nContacts,
+
+	const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataA,
+	const b3AlignedObjectArray<b3ConvexPolyhedronData>& hostConvexDataB,
+
+	const b3AlignedObjectArray<b3Vector3>& verticesA,
+	const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
+	const b3AlignedObjectArray<b3GpuFace>& facesA,
+	const b3AlignedObjectArray<int>& indicesA,
+
+	const b3AlignedObjectArray<b3Vector3>& verticesB,
+	const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
+	const b3AlignedObjectArray<b3GpuFace>& facesB,
+	const b3AlignedObjectArray<int>& indicesB,
+
+	const b3AlignedObjectArray<b3Collidable>& hostCollidablesA,
+	const b3AlignedObjectArray<b3Collidable>& hostCollidablesB,
+	const b3Vector3& sepNormalWorldSpace,
+	int maxContactCapacity)
+{
+	int contactIndex = -1;
+	b3ConvexPolyhedronData hullA, hullB;
+
+	b3Collidable colA = hostCollidablesA[collidableIndexA];
+	hullA = hostConvexDataA[colA.m_shapeIndex];
+	//printf("numvertsA = %d\n",hullA.m_numVertices);
+
+	b3Collidable colB = hostCollidablesB[collidableIndexB];
+	hullB = hostConvexDataB[colB.m_shapeIndex];
+	//printf("numvertsB = %d\n",hullB.m_numVertices);
+
+	b3Float4 contactsOut[B3_MAX_VERTS];
+	int localContactCapacity = B3_MAX_VERTS;
+
+#ifdef _WIN32
+	b3Assert(_finite(bodyBuf->at(bodyIndexA).m_pos.x));
+	b3Assert(_finite(bodyBuf->at(bodyIndexB).m_pos.x));
+#endif
+
+	{
+		b3Float4 worldVertsB1[B3_MAX_VERTS];
+		b3Float4 worldVertsB2[B3_MAX_VERTS];
+		int capacityWorldVerts = B3_MAX_VERTS;
+
+		b3Float4 hostNormal = b3MakeFloat4(sepNormalWorldSpace.x, sepNormalWorldSpace.y, sepNormalWorldSpace.z, 0.f);
+		int shapeA = hostCollidablesA[collidableIndexA].m_shapeIndex;
+		int shapeB = hostCollidablesB[collidableIndexB].m_shapeIndex;
+
+		b3Scalar minDist = -1;
+		b3Scalar maxDist = 0.;
+
+		b3Transform trA, trB;
+		{
+			//B3_PROFILE("b3TransformPoint computation");
+			//trA.setIdentity();
+			trA.setOrigin(b3MakeVector3(posA.x, posA.y, posA.z));
+			trA.setRotation(b3Quaternion(ornA.x, ornA.y, ornA.z, ornA.w));
+
+			//trB.setIdentity();
+			trB.setOrigin(b3MakeVector3(posB.x, posB.y, posB.z));
+			trB.setRotation(b3Quaternion(ornB.x, ornB.y, ornB.z, ornB.w));
+		}
+
+		b3Quaternion trAorn = trA.getRotation();
+		b3Quaternion trBorn = trB.getRotation();
+
+		int numContactsOut = b3ClipHullAgainstHull(hostNormal,
+												   hostConvexDataA.at(shapeA),
+												   hostConvexDataB.at(shapeB),
+												   (b3Float4&)trA.getOrigin(), (b3Quaternion&)trAorn,
+												   (b3Float4&)trB.getOrigin(), (b3Quaternion&)trBorn,
+												   worldVertsB1, worldVertsB2, capacityWorldVerts,
+												   minDist, maxDist,
+												   verticesA, facesA, indicesA,
+												   verticesB, facesB, indicesB,
+
+												   contactsOut, localContactCapacity);
+
+		if (numContactsOut > 0)
+		{
+			B3_PROFILE("overlap");
+
+			b3Float4 normalOnSurfaceB = (b3Float4&)hostNormal;
+			//			b3Float4 centerOut;
+
+			b3Int4 contactIdx;
+			contactIdx.x = 0;
+			contactIdx.y = 1;
+			contactIdx.z = 2;
+			contactIdx.w = 3;
+
+			int numPoints = 0;
+
+			{
+				B3_PROFILE("extractManifold");
+				numPoints = b3ReduceContacts(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
+			}
+
+			b3Assert(numPoints);
+
+			if (nContacts < maxContactCapacity)
+			{
+				contactIndex = nContacts;
+				globalContactOut->expand();
+				b3Contact4Data& contact = globalContactOut->at(nContacts);
+				contact.m_batchIdx = 0;  //i;
+				contact.m_bodyAPtrAndSignBit = (bodyBuf->at(bodyIndexA).m_invMass == 0) ? -bodyIndexA : bodyIndexA;
+				contact.m_bodyBPtrAndSignBit = (bodyBuf->at(bodyIndexB).m_invMass == 0) ? -bodyIndexB : bodyIndexB;
+
+				contact.m_frictionCoeffCmp = 45874;
+				contact.m_restituitionCoeffCmp = 0;
+
+				//	float distance = 0.f;
+				for (int p = 0; p < numPoints; p++)
+				{
+					contact.m_worldPosB[p] = contactsOut[contactIdx.s[p]];  //check if it is actually on B
+					contact.m_worldNormalOnB = normalOnSurfaceB;
+				}
+				//printf("bodyIndexA %d,bodyIndexB %d,normal=%f,%f,%f numPoints %d\n",bodyIndexA,bodyIndexB,normalOnSurfaceB.x,normalOnSurfaceB.y,normalOnSurfaceB.z,numPoints);
+				contact.m_worldNormalOnB.w = (b3Scalar)numPoints;
+				nContacts++;
+			}
+			else
+			{
+				b3Error("Error: exceeding contact capacity (%d/%d)\n", nContacts, maxContactCapacity);
+			}
+		}
+	}
+	return contactIndex;
+}
+
+inline int b3ContactConvexConvexSAT(
+	int pairIndex,
+	int bodyIndexA, int bodyIndexB,
+	int collidableIndexA, int collidableIndexB,
+	const b3AlignedObjectArray<b3RigidBodyData>& rigidBodies,
+	const b3AlignedObjectArray<b3Collidable>& collidables,
+	const b3AlignedObjectArray<b3ConvexPolyhedronData>& convexShapes,
+	const b3AlignedObjectArray<b3Float4>& convexVertices,
+	const b3AlignedObjectArray<b3Float4>& uniqueEdges,
+	const b3AlignedObjectArray<int>& convexIndices,
+	const b3AlignedObjectArray<b3GpuFace>& faces,
+	b3AlignedObjectArray<b3Contact4Data>& globalContactsOut,
+	int& nGlobalContactsOut,
+	int maxContactCapacity)
+{
+	int contactIndex = -1;
+
+	b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
+	b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
+	b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
+	b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
+
+	b3ConvexPolyhedronData hullA, hullB;
+
+	b3Float4 sepNormalWorldSpace;
+
+	b3Collidable colA = collidables[collidableIndexA];
+	hullA = convexShapes[colA.m_shapeIndex];
+	//printf("numvertsA = %d\n",hullA.m_numVertices);
+
+	b3Collidable colB = collidables[collidableIndexB];
+	hullB = convexShapes[colB.m_shapeIndex];
+	//printf("numvertsB = %d\n",hullB.m_numVertices);
+
+#ifdef _WIN32
+	b3Assert(_finite(rigidBodies[bodyIndexA].m_pos.x));
+	b3Assert(_finite(rigidBodies[bodyIndexB].m_pos.x));
+#endif
+
+	bool foundSepAxis = b3FindSeparatingAxis(hullA, hullB,
+											 posA,
+											 ornA,
+											 posB,
+											 ornB,
+
+											 convexVertices, uniqueEdges, faces, convexIndices,
+											 convexVertices, uniqueEdges, faces, convexIndices,
+
+											 sepNormalWorldSpace);
+
+	if (foundSepAxis)
+	{
+		contactIndex = b3ClipHullHullSingle(
+			bodyIndexA, bodyIndexB,
+			posA, ornA,
+			posB, ornB,
+			collidableIndexA, collidableIndexB,
+			&rigidBodies,
+			&globalContactsOut,
+			nGlobalContactsOut,
+
+			convexShapes,
+			convexShapes,
+
+			convexVertices,
+			uniqueEdges,
+			faces,
+			convexIndices,
+
+			convexVertices,
+			uniqueEdges,
+			faces,
+			convexIndices,
+
+			collidables,
+			collidables,
+			sepNormalWorldSpace,
+			maxContactCapacity);
+	}
+
+	return contactIndex;
+}
+
+#endif  //B3_CONTACT_CONVEX_CONVEX_SAT_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ContactSphereSphere.h

@@ -0,0 +1,153 @@
+
+#ifndef B3_CONTACT_SPHERE_SPHERE_H
+#define B3_CONTACT_SPHERE_SPHERE_H
+
+void computeContactSphereConvex(int pairIndex,
+								int bodyIndexA, int bodyIndexB,
+								int collidableIndexA, int collidableIndexB,
+								const b3RigidBodyData* rigidBodies,
+								const b3Collidable* collidables,
+								const b3ConvexPolyhedronData* convexShapes,
+								const b3Vector3* convexVertices,
+								const int* convexIndices,
+								const b3GpuFace* faces,
+								b3Contact4* globalContactsOut,
+								int& nGlobalContactsOut,
+								int maxContactCapacity)
+{
+	float radius = collidables[collidableIndexA].m_radius;
+	float4 spherePos1 = rigidBodies[bodyIndexA].m_pos;
+	b3Quaternion sphereOrn = rigidBodies[bodyIndexA].m_quat;
+
+	float4 pos = rigidBodies[bodyIndexB].m_pos;
+
+	b3Quaternion quat = rigidBodies[bodyIndexB].m_quat;
+
+	b3Transform tr;
+	tr.setIdentity();
+	tr.setOrigin(pos);
+	tr.setRotation(quat);
+	b3Transform trInv = tr.inverse();
+
+	float4 spherePos = trInv(spherePos1);
+
+	int collidableIndex = rigidBodies[bodyIndexB].m_collidableIdx;
+	int shapeIndex = collidables[collidableIndex].m_shapeIndex;
+	int numFaces = convexShapes[shapeIndex].m_numFaces;
+	float4 closestPnt = b3MakeVector3(0, 0, 0, 0);
+	float4 hitNormalWorld = b3MakeVector3(0, 0, 0, 0);
+	float minDist = -1000000.f;  // TODO: What is the largest/smallest float?
+	bool bCollide = true;
+	int region = -1;
+	float4 localHitNormal;
+	for (int f = 0; f < numFaces; f++)
+	{
+		b3GpuFace face = faces[convexShapes[shapeIndex].m_faceOffset + f];
+		float4 planeEqn;
+		float4 localPlaneNormal = b3MakeVector3(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
+		float4 n1 = localPlaneNormal;  //quatRotate(quat,localPlaneNormal);
+		planeEqn = n1;
+		planeEqn[3] = face.m_plane.w;
+
+		float4 pntReturn;
+		float dist = signedDistanceFromPointToPlane(spherePos, planeEqn, &pntReturn);
+
+		if (dist > radius)
+		{
+			bCollide = false;
+			break;
+		}
+
+		if (dist > 0)
+		{
+			//might hit an edge or vertex
+			b3Vector3 out;
+
+			bool isInPoly = IsPointInPolygon(spherePos,
+											 &face,
+											 &convexVertices[convexShapes[shapeIndex].m_vertexOffset],
+											 convexIndices,
+											 &out);
+			if (isInPoly)
+			{
+				if (dist > minDist)
+				{
+					minDist = dist;
+					closestPnt = pntReturn;
+					localHitNormal = planeEqn;
+					region = 1;
+				}
+			}
+			else
+			{
+				b3Vector3 tmp = spherePos - out;
+				b3Scalar l2 = tmp.length2();
+				if (l2 < radius * radius)
+				{
+					dist = b3Sqrt(l2);
+					if (dist > minDist)
+					{
+						minDist = dist;
+						closestPnt = out;
+						localHitNormal = tmp / dist;
+						region = 2;
+					}
+				}
+				else
+				{
+					bCollide = false;
+					break;
+				}
+			}
+		}
+		else
+		{
+			if (dist > minDist)
+			{
+				minDist = dist;
+				closestPnt = pntReturn;
+				localHitNormal = planeEqn;
+				region = 3;
+			}
+		}
+	}
+	static int numChecks = 0;
+	numChecks++;
+
+	if (bCollide && minDist > -10000)
+	{
+		float4 normalOnSurfaceB1 = tr.getBasis() * localHitNormal;  //-hitNormalWorld;
+		float4 pOnB1 = tr(closestPnt);
+		//printf("dist ,%f,",minDist);
+		float actualDepth = minDist - radius;
+		if (actualDepth < 0)
+		{
+			//printf("actualDepth = ,%f,", actualDepth);
+			//printf("normalOnSurfaceB1 = ,%f,%f,%f,", normalOnSurfaceB1.x,normalOnSurfaceB1.y,normalOnSurfaceB1.z);
+			//printf("region=,%d,\n", region);
+			pOnB1[3] = actualDepth;
+
+			int dstIdx;
+			//    dstIdx = nGlobalContactsOut++;//AppendInc( nGlobalContactsOut, dstIdx );
+
+			if (nGlobalContactsOut < maxContactCapacity)
+			{
+				dstIdx = nGlobalContactsOut;
+				nGlobalContactsOut++;
+
+				b3Contact4* c = &globalContactsOut[dstIdx];
+				c->m_worldNormalOnB = normalOnSurfaceB1;
+				c->setFrictionCoeff(0.7);
+				c->setRestituitionCoeff(0.f);
+
+				c->m_batchIdx = pairIndex;
+				c->m_bodyAPtrAndSignBit = rigidBodies[bodyIndexA].m_invMass == 0 ? -bodyIndexA : bodyIndexA;
+				c->m_bodyBPtrAndSignBit = rigidBodies[bodyIndexB].m_invMass == 0 ? -bodyIndexB : bodyIndexB;
+				c->m_worldPosB[0] = pOnB1;
+				int numPoints = 1;
+				c->m_worldNormalOnB.w = (b3Scalar)numPoints;
+			}  //if (dstIdx < numPairs)
+		}
+	}  //if (hasCollision)
+}
+#endif  //B3_CONTACT_SPHERE_SPHERE_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h

@@ -0,0 +1,38 @@
+
+#ifndef B3_CONVEX_POLYHEDRON_DATA_H
+#define B3_CONVEX_POLYHEDRON_DATA_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Common/shared/b3Quat.h"
+
+typedef struct b3GpuFace b3GpuFace_t;
+struct b3GpuFace
+{
+	b3Float4 m_plane;
+	int m_indexOffset;
+	int m_numIndices;
+	int m_unusedPadding1;
+	int m_unusedPadding2;
+};
+
+typedef struct b3ConvexPolyhedronData b3ConvexPolyhedronData_t;
+
+struct b3ConvexPolyhedronData
+{
+	b3Float4 m_localCenter;
+	b3Float4 m_extents;
+	b3Float4 mC;
+	b3Float4 mE;
+
+	float m_radius;
+	int m_faceOffset;
+	int m_numFaces;
+	int m_numVertices;
+
+	int m_vertexOffset;
+	int m_uniqueEdgesOffset;
+	int m_numUniqueEdges;
+	int m_unused;
+};
+
+#endif  //B3_CONVEX_POLYHEDRON_DATA_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h

@@ -0,0 +1,797 @@
+#ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H
+#define B3_FIND_CONCAVE_SEPARATING_AXIS_H
+
+#define B3_TRIANGLE_NUM_CONVEX_FACES 5
+
+#include "Bullet3Common/shared/b3Int4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
+
+inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn,
+					  const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
+{
+	min[0] = FLT_MAX;
+	max[0] = -FLT_MAX;
+	int numVerts = hull->m_numVertices;
+
+	const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn), *dir);
+	float offset = b3Dot(pos, *dir);
+	for (int i = 0; i < numVerts; i++)
+	{
+		float dp = b3Dot(vertices[hull->m_vertexOffset + i], localDir);
+		if (dp < min[0])
+			min[0] = dp;
+		if (dp > max[0])
+			max[0] = dp;
+	}
+	if (min[0] > max[0])
+	{
+		float tmp = min[0];
+		min[0] = max[0];
+		max[0] = tmp;
+	}
+	min[0] += offset;
+	max[0] += offset;
+}
+
+inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
+						  b3Float4ConstArg posA, b3QuatConstArg ornA,
+						  b3Float4ConstArg posB, b3QuatConstArg ornB,
+						  b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB, float* depth)
+{
+	float Min0, Max0;
+	float Min1, Max1;
+	b3Project(hullA, posA, ornA, sep_axis, verticesA, &Min0, &Max0);
+	b3Project(hullB, posB, ornB, sep_axis, verticesB, &Min1, &Max1);
+
+	if (Max0 < Min1 || Max1 < Min0)
+		return false;
+
+	float d0 = Max0 - Min1;
+	float d1 = Max1 - Min0;
+	*depth = d0 < d1 ? d0 : d1;
+	return true;
+}
+
+bool b3FindSeparatingAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
+						  b3Float4ConstArg posA1,
+						  b3QuatConstArg ornA,
+						  b3Float4ConstArg posB1,
+						  b3QuatConstArg ornB,
+						  b3Float4ConstArg DeltaC2,
+
+						  const b3Float4* verticesA,
+						  const b3Float4* uniqueEdgesA,
+						  const b3GpuFace* facesA,
+						  const int* indicesA,
+
+						  __global const b3Float4* verticesB,
+						  __global const b3Float4* uniqueEdgesB,
+						  __global const b3GpuFace* facesB,
+						  __global const int* indicesB,
+						  b3Float4* sep,
+						  float* dmin)
+{
+	b3Float4 posA = posA1;
+	posA.w = 0.f;
+	b3Float4 posB = posB1;
+	posB.w = 0.f;
+	/*
+	static int maxFaceVertex = 0;
+
+	int curFaceVertexAB = hullA->m_numFaces*hullB->m_numVertices;
+	curFaceVertexAB+= hullB->m_numFaces*hullA->m_numVertices;
+
+	if (curFaceVertexAB>maxFaceVertex)
+	{
+		maxFaceVertex = curFaceVertexAB;
+		printf("curFaceVertexAB = %d\n",curFaceVertexAB);
+		printf("hullA->m_numFaces = %d\n",hullA->m_numFaces);
+		printf("hullA->m_numVertices = %d\n",hullA->m_numVertices);
+		printf("hullB->m_numVertices = %d\n",hullB->m_numVertices);
+	}
+*/
+
+	int curPlaneTests = 0;
+	{
+		int numFacesA = hullA->m_numFaces;
+		// Test normals from hullA
+		for (int i = 0; i < numFacesA; i++)
+		{
+			const b3Float4 normal = facesA[hullA->m_faceOffset + i].m_plane;
+			b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
+			if (b3Dot(DeltaC2, faceANormalWS) < 0)
+				faceANormalWS *= -1.f;
+			curPlaneTests++;
+			float d;
+			if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, &faceANormalWS, verticesA, verticesB, &d))
+				return false;
+			if (d < *dmin)
+			{
+				*dmin = d;
+				*sep = faceANormalWS;
+			}
+		}
+	}
+	if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
+	{
+		*sep = -(*sep);
+	}
+	return true;
+}
+
+b3Vector3 unitSphere162[] =
+	{
+		b3MakeVector3(0.000000, -1.000000, 0.000000),
+		b3MakeVector3(0.203181, -0.967950, 0.147618),
+		b3MakeVector3(-0.077607, -0.967950, 0.238853),
+		b3MakeVector3(0.723607, -0.447220, 0.525725),
+		b3MakeVector3(0.609547, -0.657519, 0.442856),
+		b3MakeVector3(0.812729, -0.502301, 0.295238),
+		b3MakeVector3(-0.251147, -0.967949, 0.000000),
+		b3MakeVector3(-0.077607, -0.967950, -0.238853),
+		b3MakeVector3(0.203181, -0.967950, -0.147618),
+		b3MakeVector3(0.860698, -0.251151, 0.442858),
+		b3MakeVector3(-0.276388, -0.447220, 0.850649),
+		b3MakeVector3(-0.029639, -0.502302, 0.864184),
+		b3MakeVector3(-0.155215, -0.251152, 0.955422),
+		b3MakeVector3(-0.894426, -0.447216, 0.000000),
+		b3MakeVector3(-0.831051, -0.502299, 0.238853),
+		b3MakeVector3(-0.956626, -0.251149, 0.147618),
+		b3MakeVector3(-0.276388, -0.447220, -0.850649),
+		b3MakeVector3(-0.483971, -0.502302, -0.716565),
+		b3MakeVector3(-0.436007, -0.251152, -0.864188),
+		b3MakeVector3(0.723607, -0.447220, -0.525725),
+		b3MakeVector3(0.531941, -0.502302, -0.681712),
+		b3MakeVector3(0.687159, -0.251152, -0.681715),
+		b3MakeVector3(0.687159, -0.251152, 0.681715),
+		b3MakeVector3(-0.436007, -0.251152, 0.864188),
+		b3MakeVector3(-0.956626, -0.251149, -0.147618),
+		b3MakeVector3(-0.155215, -0.251152, -0.955422),
+		b3MakeVector3(0.860698, -0.251151, -0.442858),
+		b3MakeVector3(0.276388, 0.447220, 0.850649),
+		b3MakeVector3(0.483971, 0.502302, 0.716565),
+		b3MakeVector3(0.232822, 0.657519, 0.716563),
+		b3MakeVector3(-0.723607, 0.447220, 0.525725),
+		b3MakeVector3(-0.531941, 0.502302, 0.681712),
+		b3MakeVector3(-0.609547, 0.657519, 0.442856),
+		b3MakeVector3(-0.723607, 0.447220, -0.525725),
+		b3MakeVector3(-0.812729, 0.502301, -0.295238),
+		b3MakeVector3(-0.609547, 0.657519, -0.442856),
+		b3MakeVector3(0.276388, 0.447220, -0.850649),
+		b3MakeVector3(0.029639, 0.502302, -0.864184),
+		b3MakeVector3(0.232822, 0.657519, -0.716563),
+		b3MakeVector3(0.894426, 0.447216, 0.000000),
+		b3MakeVector3(0.831051, 0.502299, -0.238853),
+		b3MakeVector3(0.753442, 0.657515, 0.000000),
+		b3MakeVector3(-0.232822, -0.657519, 0.716563),
+		b3MakeVector3(-0.162456, -0.850654, 0.499995),
+		b3MakeVector3(0.052790, -0.723612, 0.688185),
+		b3MakeVector3(0.138199, -0.894429, 0.425321),
+		b3MakeVector3(0.262869, -0.525738, 0.809012),
+		b3MakeVector3(0.361805, -0.723611, 0.587779),
+		b3MakeVector3(0.531941, -0.502302, 0.681712),
+		b3MakeVector3(0.425323, -0.850654, 0.309011),
+		b3MakeVector3(0.812729, -0.502301, -0.295238),
+		b3MakeVector3(0.609547, -0.657519, -0.442856),
+		b3MakeVector3(0.850648, -0.525736, 0.000000),
+		b3MakeVector3(0.670817, -0.723611, -0.162457),
+		b3MakeVector3(0.670817, -0.723610, 0.162458),
+		b3MakeVector3(0.425323, -0.850654, -0.309011),
+		b3MakeVector3(0.447211, -0.894428, 0.000001),
+		b3MakeVector3(-0.753442, -0.657515, 0.000000),
+		b3MakeVector3(-0.525730, -0.850652, 0.000000),
+		b3MakeVector3(-0.638195, -0.723609, 0.262864),
+		b3MakeVector3(-0.361801, -0.894428, 0.262864),
+		b3MakeVector3(-0.688189, -0.525736, 0.499997),
+		b3MakeVector3(-0.447211, -0.723610, 0.525729),
+		b3MakeVector3(-0.483971, -0.502302, 0.716565),
+		b3MakeVector3(-0.232822, -0.657519, -0.716563),
+		b3MakeVector3(-0.162456, -0.850654, -0.499995),
+		b3MakeVector3(-0.447211, -0.723611, -0.525727),
+		b3MakeVector3(-0.361801, -0.894429, -0.262863),
+		b3MakeVector3(-0.688189, -0.525736, -0.499997),
+		b3MakeVector3(-0.638195, -0.723609, -0.262863),
+		b3MakeVector3(-0.831051, -0.502299, -0.238853),
+		b3MakeVector3(0.361804, -0.723612, -0.587779),
+		b3MakeVector3(0.138197, -0.894429, -0.425321),
+		b3MakeVector3(0.262869, -0.525738, -0.809012),
+		b3MakeVector3(0.052789, -0.723611, -0.688186),
+		b3MakeVector3(-0.029639, -0.502302, -0.864184),
+		b3MakeVector3(0.956626, 0.251149, 0.147618),
+		b3MakeVector3(0.956626, 0.251149, -0.147618),
+		b3MakeVector3(0.951058, -0.000000, 0.309013),
+		b3MakeVector3(1.000000, 0.000000, 0.000000),
+		b3MakeVector3(0.947213, -0.276396, 0.162458),
+		b3MakeVector3(0.951058, 0.000000, -0.309013),
+		b3MakeVector3(0.947213, -0.276396, -0.162458),
+		b3MakeVector3(0.155215, 0.251152, 0.955422),
+		b3MakeVector3(0.436007, 0.251152, 0.864188),
+		b3MakeVector3(-0.000000, -0.000000, 1.000000),
+		b3MakeVector3(0.309017, 0.000000, 0.951056),
+		b3MakeVector3(0.138199, -0.276398, 0.951055),
+		b3MakeVector3(0.587786, 0.000000, 0.809017),
+		b3MakeVector3(0.447216, -0.276398, 0.850648),
+		b3MakeVector3(-0.860698, 0.251151, 0.442858),
+		b3MakeVector3(-0.687159, 0.251152, 0.681715),
+		b3MakeVector3(-0.951058, -0.000000, 0.309013),
+		b3MakeVector3(-0.809018, 0.000000, 0.587783),
+		b3MakeVector3(-0.861803, -0.276396, 0.425324),
+		b3MakeVector3(-0.587786, 0.000000, 0.809017),
+		b3MakeVector3(-0.670819, -0.276397, 0.688191),
+		b3MakeVector3(-0.687159, 0.251152, -0.681715),
+		b3MakeVector3(-0.860698, 0.251151, -0.442858),
+		b3MakeVector3(-0.587786, -0.000000, -0.809017),
+		b3MakeVector3(-0.809018, -0.000000, -0.587783),
+		b3MakeVector3(-0.670819, -0.276397, -0.688191),
+		b3MakeVector3(-0.951058, 0.000000, -0.309013),
+		b3MakeVector3(-0.861803, -0.276396, -0.425324),
+		b3MakeVector3(0.436007, 0.251152, -0.864188),
+		b3MakeVector3(0.155215, 0.251152, -0.955422),
+		b3MakeVector3(0.587786, -0.000000, -0.809017),
+		b3MakeVector3(0.309017, -0.000000, -0.951056),
+		b3MakeVector3(0.447216, -0.276398, -0.850648),
+		b3MakeVector3(0.000000, 0.000000, -1.000000),
+		b3MakeVector3(0.138199, -0.276398, -0.951055),
+		b3MakeVector3(0.670820, 0.276396, 0.688190),
+		b3MakeVector3(0.809019, -0.000002, 0.587783),
+		b3MakeVector3(0.688189, 0.525736, 0.499997),
+		b3MakeVector3(0.861804, 0.276394, 0.425323),
+		b3MakeVector3(0.831051, 0.502299, 0.238853),
+		b3MakeVector3(-0.447216, 0.276397, 0.850649),
+		b3MakeVector3(-0.309017, -0.000001, 0.951056),
+		b3MakeVector3(-0.262869, 0.525738, 0.809012),
+		b3MakeVector3(-0.138199, 0.276397, 0.951055),
+		b3MakeVector3(0.029639, 0.502302, 0.864184),
+		b3MakeVector3(-0.947213, 0.276396, -0.162458),
+		b3MakeVector3(-1.000000, 0.000001, 0.000000),
+		b3MakeVector3(-0.850648, 0.525736, -0.000000),
+		b3MakeVector3(-0.947213, 0.276397, 0.162458),
+		b3MakeVector3(-0.812729, 0.502301, 0.295238),
+		b3MakeVector3(-0.138199, 0.276397, -0.951055),
+		b3MakeVector3(-0.309016, -0.000000, -0.951057),
+		b3MakeVector3(-0.262869, 0.525738, -0.809012),
+		b3MakeVector3(-0.447215, 0.276397, -0.850649),
+		b3MakeVector3(-0.531941, 0.502302, -0.681712),
+		b3MakeVector3(0.861804, 0.276396, -0.425322),
+		b3MakeVector3(0.809019, 0.000000, -0.587782),
+		b3MakeVector3(0.688189, 0.525736, -0.499997),
+		b3MakeVector3(0.670821, 0.276397, -0.688189),
+		b3MakeVector3(0.483971, 0.502302, -0.716565),
+		b3MakeVector3(0.077607, 0.967950, 0.238853),
+		b3MakeVector3(0.251147, 0.967949, 0.000000),
+		b3MakeVector3(0.000000, 1.000000, 0.000000),
+		b3MakeVector3(0.162456, 0.850654, 0.499995),
+		b3MakeVector3(0.361800, 0.894429, 0.262863),
+		b3MakeVector3(0.447209, 0.723612, 0.525728),
+		b3MakeVector3(0.525730, 0.850652, 0.000000),
+		b3MakeVector3(0.638194, 0.723610, 0.262864),
+		b3MakeVector3(-0.203181, 0.967950, 0.147618),
+		b3MakeVector3(-0.425323, 0.850654, 0.309011),
+		b3MakeVector3(-0.138197, 0.894430, 0.425320),
+		b3MakeVector3(-0.361804, 0.723612, 0.587778),
+		b3MakeVector3(-0.052790, 0.723612, 0.688185),
+		b3MakeVector3(-0.203181, 0.967950, -0.147618),
+		b3MakeVector3(-0.425323, 0.850654, -0.309011),
+		b3MakeVector3(-0.447210, 0.894429, 0.000000),
+		b3MakeVector3(-0.670817, 0.723611, -0.162457),
+		b3MakeVector3(-0.670817, 0.723611, 0.162457),
+		b3MakeVector3(0.077607, 0.967950, -0.238853),
+		b3MakeVector3(0.162456, 0.850654, -0.499995),
+		b3MakeVector3(-0.138197, 0.894430, -0.425320),
+		b3MakeVector3(-0.052790, 0.723612, -0.688185),
+		b3MakeVector3(-0.361804, 0.723612, -0.587778),
+		b3MakeVector3(0.361800, 0.894429, -0.262863),
+		b3MakeVector3(0.638194, 0.723610, -0.262864),
+		b3MakeVector3(0.447209, 0.723612, -0.525728)};
+
+bool b3FindSeparatingAxisEdgeEdge(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
+								  b3Float4ConstArg posA1,
+								  b3QuatConstArg ornA,
+								  b3Float4ConstArg posB1,
+								  b3QuatConstArg ornB,
+								  b3Float4ConstArg DeltaC2,
+								  const b3Float4* verticesA,
+								  const b3Float4* uniqueEdgesA,
+								  const b3GpuFace* facesA,
+								  const int* indicesA,
+								  __global const b3Float4* verticesB,
+								  __global const b3Float4* uniqueEdgesB,
+								  __global const b3GpuFace* facesB,
+								  __global const int* indicesB,
+								  b3Float4* sep,
+								  float* dmin,
+								  bool searchAllEdgeEdge)
+{
+	b3Float4 posA = posA1;
+	posA.w = 0.f;
+	b3Float4 posB = posB1;
+	posB.w = 0.f;
+
+	//	int curPlaneTests=0;
+
+	int curEdgeEdge = 0;
+	// Test edges
+	static int maxEdgeTests = 0;
+	int curEdgeTests = hullA->m_numUniqueEdges * hullB->m_numUniqueEdges;
+	if (curEdgeTests > maxEdgeTests)
+	{
+		maxEdgeTests = curEdgeTests;
+		printf("maxEdgeTests = %d\n", maxEdgeTests);
+		printf("hullA->m_numUniqueEdges = %d\n", hullA->m_numUniqueEdges);
+		printf("hullB->m_numUniqueEdges = %d\n", hullB->m_numUniqueEdges);
+	}
+
+	if (searchAllEdgeEdge)
+	{
+		for (int e0 = 0; e0 < hullA->m_numUniqueEdges; e0++)
+		{
+			const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset + e0];
+			b3Float4 edge0World = b3QuatRotate(ornA, edge0);
+
+			for (int e1 = 0; e1 < hullB->m_numUniqueEdges; e1++)
+			{
+				const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset + e1];
+				b3Float4 edge1World = b3QuatRotate(ornB, edge1);
+
+				b3Float4 crossje = b3Cross(edge0World, edge1World);
+
+				curEdgeEdge++;
+				if (!b3IsAlmostZero(crossje))
+				{
+					crossje = b3Normalized(crossje);
+					if (b3Dot(DeltaC2, crossje) < 0)
+						crossje *= -1.f;
+
+					float dist;
+					bool result = true;
+					{
+						float Min0, Max0;
+						float Min1, Max1;
+						b3Project(hullA, posA, ornA, &crossje, verticesA, &Min0, &Max0);
+						b3Project(hullB, posB, ornB, &crossje, verticesB, &Min1, &Max1);
+
+						if (Max0 < Min1 || Max1 < Min0)
+							return false;
+
+						float d0 = Max0 - Min1;
+						float d1 = Max1 - Min0;
+						dist = d0 < d1 ? d0 : d1;
+						result = true;
+					}
+
+					if (dist < *dmin)
+					{
+						*dmin = dist;
+						*sep = crossje;
+					}
+				}
+			}
+		}
+	}
+	else
+	{
+		int numDirections = sizeof(unitSphere162) / sizeof(b3Vector3);
+		//printf("numDirections =%d\n",numDirections );
+
+		for (int i = 0; i < numDirections; i++)
+		{
+			b3Float4 crossje = unitSphere162[i];
+			{
+				//if (b3Dot(DeltaC2,crossje)>0)
+				{
+					float dist;
+					bool result = true;
+					{
+						float Min0, Max0;
+						float Min1, Max1;
+						b3Project(hullA, posA, ornA, &crossje, verticesA, &Min0, &Max0);
+						b3Project(hullB, posB, ornB, &crossje, verticesB, &Min1, &Max1);
+
+						if (Max0 < Min1 || Max1 < Min0)
+							return false;
+
+						float d0 = Max0 - Min1;
+						float d1 = Max1 - Min0;
+						dist = d0 < d1 ? d0 : d1;
+						result = true;
+					}
+
+					if (dist < *dmin)
+					{
+						*dmin = dist;
+						*sep = crossje;
+					}
+				}
+			}
+		}
+	}
+
+	if ((b3Dot(-DeltaC2, *sep)) > 0.0f)
+	{
+		*sep = -(*sep);
+	}
+	return true;
+}
+
+inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
+							   __global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
+							   b3Float4ConstArg posA, b3QuatConstArg ornA, b3Float4ConstArg posB, b3QuatConstArg ornB,
+							   __global b3Float4* worldVertsA1,
+							   __global b3Float4* worldNormalsA1,
+							   __global b3Float4* worldVertsB1,
+							   int capacityWorldVerts,
+							   const float minDist, float maxDist,
+							   __global const b3Float4* verticesA,
+							   __global const b3GpuFace_t* facesA,
+							   __global const int* indicesA,
+							   __global const b3Float4* verticesB,
+							   __global const b3GpuFace_t* facesB,
+							   __global const int* indicesB,
+
+							   __global b3Int4* clippingFaces, int pairIndex)
+{
+	int numContactsOut = 0;
+	int numWorldVertsB1 = 0;
+
+	int closestFaceB = -1;
+	float dmax = -FLT_MAX;
+
+	{
+		for (int face = 0; face < hullB->m_numFaces; face++)
+		{
+			const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset + face].m_plane.x,
+												 facesB[hullB->m_faceOffset + face].m_plane.y, facesB[hullB->m_faceOffset + face].m_plane.z, 0.f);
+			const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
+			float d = b3Dot(WorldNormal, separatingNormal);
+			if (d > dmax)
+			{
+				dmax = d;
+				closestFaceB = face;
+			}
+		}
+	}
+
+	{
+		const b3GpuFace_t polyB = facesB[hullB->m_faceOffset + closestFaceB];
+		const int numVertices = polyB.m_numIndices;
+		for (int e0 = 0; e0 < numVertices; e0++)
+		{
+			const b3Float4 b = verticesB[hullB->m_vertexOffset + indicesB[polyB.m_indexOffset + e0]];
+			worldVertsB1[pairIndex * capacityWorldVerts + numWorldVertsB1++] = b3TransformPoint(b, posB, ornB);
+		}
+	}
+
+	int closestFaceA = -1;
+	{
+		float dmin = FLT_MAX;
+		for (int face = 0; face < hullA->m_numFaces; face++)
+		{
+			const b3Float4 Normal = b3MakeFloat4(
+				facesA[hullA->m_faceOffset + face].m_plane.x,
+				facesA[hullA->m_faceOffset + face].m_plane.y,
+				facesA[hullA->m_faceOffset + face].m_plane.z,
+				0.f);
+			const b3Float4 faceANormalWS = b3QuatRotate(ornA, Normal);
+
+			float d = b3Dot(faceANormalWS, separatingNormal);
+			if (d < dmin)
+			{
+				dmin = d;
+				closestFaceA = face;
+				worldNormalsA1[pairIndex] = faceANormalWS;
+			}
+		}
+	}
+
+	int numVerticesA = facesA[hullA->m_faceOffset + closestFaceA].m_numIndices;
+	for (int e0 = 0; e0 < numVerticesA; e0++)
+	{
+		const b3Float4 a = verticesA[hullA->m_vertexOffset + indicesA[facesA[hullA->m_faceOffset + closestFaceA].m_indexOffset + e0]];
+		worldVertsA1[pairIndex * capacityWorldVerts + e0] = b3TransformPoint(a, posA, ornA);
+	}
+
+	clippingFaces[pairIndex].x = closestFaceA;
+	clippingFaces[pairIndex].y = closestFaceB;
+	clippingFaces[pairIndex].z = numVerticesA;
+	clippingFaces[pairIndex].w = numWorldVertsB1;
+
+	return numContactsOut;
+}
+
+__kernel void b3FindConcaveSeparatingAxisKernel(__global b3Int4* concavePairs,
+												__global const b3RigidBodyData* rigidBodies,
+												__global const b3Collidable* collidables,
+												__global const b3ConvexPolyhedronData* convexShapes,
+												__global const b3Float4* vertices,
+												__global const b3Float4* uniqueEdges,
+												__global const b3GpuFace* faces,
+												__global const int* indices,
+												__global const b3GpuChildShape* gpuChildShapes,
+												__global b3Aabb* aabbs,
+												__global b3Float4* concaveSeparatingNormalsOut,
+												__global b3Int4* clippingFacesOut,
+												__global b3Vector3* worldVertsA1Out,
+												__global b3Vector3* worldNormalsA1Out,
+												__global b3Vector3* worldVertsB1Out,
+												__global int* hasSeparatingNormals,
+												int vertexFaceCapacity,
+												int numConcavePairs,
+												int pairIdx)
+{
+	int i = pairIdx;
+	/*	int i = get_global_id(0);
+	if (i>=numConcavePairs)
+		return;
+	int pairIdx = i;
+	*/
+
+	int bodyIndexA = concavePairs[i].x;
+	int bodyIndexB = concavePairs[i].y;
+
+	int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
+	int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
+
+	int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
+	int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
+
+	if (collidables[collidableIndexB].m_shapeType != SHAPE_CONVEX_HULL &&
+		collidables[collidableIndexB].m_shapeType != SHAPE_COMPOUND_OF_CONVEX_HULLS)
+	{
+		concavePairs[pairIdx].w = -1;
+		return;
+	}
+
+	hasSeparatingNormals[i] = 0;
+
+	//	int numFacesA = convexShapes[shapeIndexA].m_numFaces;
+	int numActualConcaveConvexTests = 0;
+
+	int f = concavePairs[i].z;
+
+	bool overlap = false;
+
+	b3ConvexPolyhedronData convexPolyhedronA;
+
+	//add 3 vertices of the triangle
+	convexPolyhedronA.m_numVertices = 3;
+	convexPolyhedronA.m_vertexOffset = 0;
+	b3Float4 localCenter = b3MakeFloat4(0.f, 0.f, 0.f, 0.f);
+
+	b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset + f];
+	b3Aabb triAabb;
+	triAabb.m_minVec = b3MakeFloat4(1e30f, 1e30f, 1e30f, 0.f);
+	triAabb.m_maxVec = b3MakeFloat4(-1e30f, -1e30f, -1e30f, 0.f);
+
+	b3Float4 verticesA[3];
+	for (int i = 0; i < 3; i++)
+	{
+		int index = indices[face.m_indexOffset + i];
+		b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset + index];
+		verticesA[i] = vert;
+		localCenter += vert;
+
+		triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec, vert);
+		triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec, vert);
+	}
+
+	overlap = true;
+	overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap;
+	overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap;
+	overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap;
+
+	if (overlap)
+	{
+		float dmin = FLT_MAX;
+		int hasSeparatingAxis = 5;
+		b3Float4 sepAxis = b3MakeFloat4(1, 2, 3, 4);
+
+		//	int localCC=0;
+		numActualConcaveConvexTests++;
+
+		//a triangle has 3 unique edges
+		convexPolyhedronA.m_numUniqueEdges = 3;
+		convexPolyhedronA.m_uniqueEdgesOffset = 0;
+		b3Float4 uniqueEdgesA[3];
+
+		uniqueEdgesA[0] = (verticesA[1] - verticesA[0]);
+		uniqueEdgesA[1] = (verticesA[2] - verticesA[1]);
+		uniqueEdgesA[2] = (verticesA[0] - verticesA[2]);
+
+		convexPolyhedronA.m_faceOffset = 0;
+
+		b3Float4 normal = b3MakeFloat4(face.m_plane.x, face.m_plane.y, face.m_plane.z, 0.f);
+
+		b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES];
+		int indicesA[3 + 3 + 2 + 2 + 2];
+		int curUsedIndices = 0;
+		int fidx = 0;
+
+		//front size of triangle
+		{
+			facesA[fidx].m_indexOffset = curUsedIndices;
+			indicesA[0] = 0;
+			indicesA[1] = 1;
+			indicesA[2] = 2;
+			curUsedIndices += 3;
+			float c = face.m_plane.w;
+			facesA[fidx].m_plane.x = normal.x;
+			facesA[fidx].m_plane.y = normal.y;
+			facesA[fidx].m_plane.z = normal.z;
+			facesA[fidx].m_plane.w = c;
+			facesA[fidx].m_numIndices = 3;
+		}
+		fidx++;
+		//back size of triangle
+		{
+			facesA[fidx].m_indexOffset = curUsedIndices;
+			indicesA[3] = 2;
+			indicesA[4] = 1;
+			indicesA[5] = 0;
+			curUsedIndices += 3;
+			float c = b3Dot(normal, verticesA[0]);
+			//	float c1 = -face.m_plane.w;
+			facesA[fidx].m_plane.x = -normal.x;
+			facesA[fidx].m_plane.y = -normal.y;
+			facesA[fidx].m_plane.z = -normal.z;
+			facesA[fidx].m_plane.w = c;
+			facesA[fidx].m_numIndices = 3;
+		}
+		fidx++;
+
+		bool addEdgePlanes = true;
+		if (addEdgePlanes)
+		{
+			int numVertices = 3;
+			int prevVertex = numVertices - 1;
+			for (int i = 0; i < numVertices; i++)
+			{
+				b3Float4 v0 = verticesA[i];
+				b3Float4 v1 = verticesA[prevVertex];
+
+				b3Float4 edgeNormal = b3Normalized(b3Cross(normal, v1 - v0));
+				float c = -b3Dot(edgeNormal, v0);
+
+				facesA[fidx].m_numIndices = 2;
+				facesA[fidx].m_indexOffset = curUsedIndices;
+				indicesA[curUsedIndices++] = i;
+				indicesA[curUsedIndices++] = prevVertex;
+
+				facesA[fidx].m_plane.x = edgeNormal.x;
+				facesA[fidx].m_plane.y = edgeNormal.y;
+				facesA[fidx].m_plane.z = edgeNormal.z;
+				facesA[fidx].m_plane.w = c;
+				fidx++;
+				prevVertex = i;
+			}
+		}
+		convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES;
+		convexPolyhedronA.m_localCenter = localCenter * (1.f / 3.f);
+
+		b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
+		posA.w = 0.f;
+		b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
+		posB.w = 0.f;
+
+		b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
+		b3Quaternion ornB = rigidBodies[bodyIndexB].m_quat;
+
+		///////////////////
+		///compound shape support
+
+		if (collidables[collidableIndexB].m_shapeType == SHAPE_COMPOUND_OF_CONVEX_HULLS)
+		{
+			int compoundChild = concavePairs[pairIdx].w;
+			int childShapeIndexB = compoundChild;  //collidables[collidableIndexB].m_shapeIndex+compoundChild;
+			int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
+			b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
+			b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
+			b3Float4 newPosB = b3TransformPoint(childPosB, posB, ornB);
+			b3Quaternion newOrnB = b3QuatMul(ornB, childOrnB);
+			posB = newPosB;
+			ornB = newOrnB;
+			shapeIndexB = collidables[childColIndexB].m_shapeIndex;
+		}
+		//////////////////
+
+		b3Float4 c0local = convexPolyhedronA.m_localCenter;
+		b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
+		b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter;
+		b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
+		const b3Float4 DeltaC2 = c0 - c1;
+
+		bool sepA = b3FindSeparatingAxis(&convexPolyhedronA, &convexShapes[shapeIndexB],
+										 posA, ornA,
+										 posB, ornB,
+										 DeltaC2,
+										 verticesA, uniqueEdgesA, facesA, indicesA,
+										 vertices, uniqueEdges, faces, indices,
+										 &sepAxis, &dmin);
+		hasSeparatingAxis = 4;
+		if (!sepA)
+		{
+			hasSeparatingAxis = 0;
+		}
+		else
+		{
+			bool sepB = b3FindSeparatingAxis(&convexShapes[shapeIndexB], &convexPolyhedronA,
+											 posB, ornB,
+											 posA, ornA,
+											 DeltaC2,
+											 vertices, uniqueEdges, faces, indices,
+											 verticesA, uniqueEdgesA, facesA, indicesA,
+											 &sepAxis, &dmin);
+
+			if (!sepB)
+			{
+				hasSeparatingAxis = 0;
+			}
+			else
+			{
+				bool sepEE = b3FindSeparatingAxisEdgeEdge(&convexPolyhedronA, &convexShapes[shapeIndexB],
+														  posA, ornA,
+														  posB, ornB,
+														  DeltaC2,
+														  verticesA, uniqueEdgesA, facesA, indicesA,
+														  vertices, uniqueEdges, faces, indices,
+														  &sepAxis, &dmin, true);
+
+				if (!sepEE)
+				{
+					hasSeparatingAxis = 0;
+				}
+				else
+				{
+					hasSeparatingAxis = 1;
+				}
+			}
+		}
+
+		if (hasSeparatingAxis)
+		{
+			hasSeparatingNormals[i] = 1;
+			sepAxis.w = dmin;
+			concaveSeparatingNormalsOut[pairIdx] = sepAxis;
+
+			//now compute clipping faces A and B, and world-space clipping vertices A and B...
+
+			float minDist = -1e30f;
+			float maxDist = 0.02f;
+
+			b3FindClippingFaces(sepAxis,
+								&convexPolyhedronA,
+								&convexShapes[shapeIndexB],
+								posA, ornA,
+								posB, ornB,
+								worldVertsA1Out,
+								worldNormalsA1Out,
+								worldVertsB1Out,
+								vertexFaceCapacity,
+								minDist, maxDist,
+								verticesA,
+								facesA,
+								indicesA,
+
+								vertices,
+								faces,
+								indices,
+								clippingFacesOut, pairIdx);
+		}
+		else
+		{
+			//mark this pair as in-active
+			concavePairs[pairIdx].w = -1;
+		}
+	}
+	else
+	{
+		//mark this pair as in-active
+		concavePairs[pairIdx].w = -1;
+	}
+}
+
+#endif  //B3_FIND_CONCAVE_SEPARATING_AXIS_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3FindSeparatingAxis.h

@@ -0,0 +1,197 @@
+#ifndef B3_FIND_SEPARATING_AXIS_H
+#define B3_FIND_SEPARATING_AXIS_H
+
+inline void b3ProjectAxis(const b3ConvexPolyhedronData& hull, const b3Float4& pos, const b3Quaternion& orn, const b3Float4& dir, const b3AlignedObjectArray<b3Vector3>& vertices, b3Scalar& min, b3Scalar& max)
+{
+	min = FLT_MAX;
+	max = -FLT_MAX;
+	int numVerts = hull.m_numVertices;
+
+	const b3Float4 localDir = b3QuatRotate(orn.inverse(), dir);
+
+	b3Scalar offset = b3Dot3F4(pos, dir);
+
+	for (int i = 0; i < numVerts; i++)
+	{
+		//b3Vector3 pt = trans * vertices[m_vertexOffset+i];
+		//b3Scalar dp = pt.dot(dir);
+		//b3Vector3 vertex = vertices[hull.m_vertexOffset+i];
+		b3Scalar dp = b3Dot3F4((b3Float4&)vertices[hull.m_vertexOffset + i], localDir);
+		//b3Assert(dp==dpL);
+		if (dp < min) min = dp;
+		if (dp > max) max = dp;
+	}
+	if (min > max)
+	{
+		b3Scalar tmp = min;
+		min = max;
+		max = tmp;
+	}
+	min += offset;
+	max += offset;
+}
+
+inline bool b3TestSepAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
+						  const b3Float4& posA, const b3Quaternion& ornA,
+						  const b3Float4& posB, const b3Quaternion& ornB,
+						  const b3Float4& sep_axis, const b3AlignedObjectArray<b3Vector3>& verticesA, const b3AlignedObjectArray<b3Vector3>& verticesB, b3Scalar& depth)
+{
+	b3Scalar Min0, Max0;
+	b3Scalar Min1, Max1;
+	b3ProjectAxis(hullA, posA, ornA, sep_axis, verticesA, Min0, Max0);
+	b3ProjectAxis(hullB, posB, ornB, sep_axis, verticesB, Min1, Max1);
+
+	if (Max0 < Min1 || Max1 < Min0)
+		return false;
+
+	b3Scalar d0 = Max0 - Min1;
+	b3Assert(d0 >= 0.0f);
+	b3Scalar d1 = Max1 - Min0;
+	b3Assert(d1 >= 0.0f);
+	depth = d0 < d1 ? d0 : d1;
+	return true;
+}
+
+inline bool b3FindSeparatingAxis(const b3ConvexPolyhedronData& hullA, const b3ConvexPolyhedronData& hullB,
+								 const b3Float4& posA1,
+								 const b3Quaternion& ornA,
+								 const b3Float4& posB1,
+								 const b3Quaternion& ornB,
+								 const b3AlignedObjectArray<b3Vector3>& verticesA,
+								 const b3AlignedObjectArray<b3Vector3>& uniqueEdgesA,
+								 const b3AlignedObjectArray<b3GpuFace>& facesA,
+								 const b3AlignedObjectArray<int>& indicesA,
+								 const b3AlignedObjectArray<b3Vector3>& verticesB,
+								 const b3AlignedObjectArray<b3Vector3>& uniqueEdgesB,
+								 const b3AlignedObjectArray<b3GpuFace>& facesB,
+								 const b3AlignedObjectArray<int>& indicesB,
+
+								 b3Vector3& sep)
+{
+	B3_PROFILE("findSeparatingAxis");
+
+	b3Float4 posA = posA1;
+	posA.w = 0.f;
+	b3Float4 posB = posB1;
+	posB.w = 0.f;
+	//#ifdef TEST_INTERNAL_OBJECTS
+	b3Float4 c0local = (b3Float4&)hullA.m_localCenter;
+
+	b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
+	b3Float4 c1local = (b3Float4&)hullB.m_localCenter;
+	b3Float4 c1 = b3TransformPoint(c1local, posB, ornB);
+	const b3Float4 deltaC2 = c0 - c1;
+	//#endif
+
+	b3Scalar dmin = FLT_MAX;
+	int curPlaneTests = 0;
+
+	int numFacesA = hullA.m_numFaces;
+	// Test normals from hullA
+	for (int i = 0; i < numFacesA; i++)
+	{
+		const b3Float4& normal = (b3Float4&)facesA[hullA.m_faceOffset + i].m_plane;
+		b3Float4 faceANormalWS = b3QuatRotate(ornA, normal);
+
+		if (b3Dot3F4(deltaC2, faceANormalWS) < 0)
+			faceANormalWS *= -1.f;
+
+		curPlaneTests++;
+#ifdef TEST_INTERNAL_OBJECTS
+		gExpectedNbTests++;
+		if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, faceANormalWS, hullA, hullB, dmin))
+			continue;
+		gActualNbTests++;
+#endif
+
+		b3Scalar d;
+		if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, faceANormalWS, verticesA, verticesB, d))
+			return false;
+
+		if (d < dmin)
+		{
+			dmin = d;
+			sep = (b3Vector3&)faceANormalWS;
+		}
+	}
+
+	int numFacesB = hullB.m_numFaces;
+	// Test normals from hullB
+	for (int i = 0; i < numFacesB; i++)
+	{
+		b3Float4 normal = (b3Float4&)facesB[hullB.m_faceOffset + i].m_plane;
+		b3Float4 WorldNormal = b3QuatRotate(ornB, normal);
+
+		if (b3Dot3F4(deltaC2, WorldNormal) < 0)
+		{
+			WorldNormal *= -1.f;
+		}
+		curPlaneTests++;
+#ifdef TEST_INTERNAL_OBJECTS
+		gExpectedNbTests++;
+		if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, WorldNormal, hullA, hullB, dmin))
+			continue;
+		gActualNbTests++;
+#endif
+
+		b3Scalar d;
+		if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, WorldNormal, verticesA, verticesB, d))
+			return false;
+
+		if (d < dmin)
+		{
+			dmin = d;
+			sep = (b3Vector3&)WorldNormal;
+		}
+	}
+
+	//	b3Vector3 edgeAstart,edgeAend,edgeBstart,edgeBend;
+
+	int curEdgeEdge = 0;
+	// Test edges
+	for (int e0 = 0; e0 < hullA.m_numUniqueEdges; e0++)
+	{
+		const b3Float4& edge0 = (b3Float4&)uniqueEdgesA[hullA.m_uniqueEdgesOffset + e0];
+		b3Float4 edge0World = b3QuatRotate(ornA, (b3Float4&)edge0);
+
+		for (int e1 = 0; e1 < hullB.m_numUniqueEdges; e1++)
+		{
+			const b3Vector3 edge1 = uniqueEdgesB[hullB.m_uniqueEdgesOffset + e1];
+			b3Float4 edge1World = b3QuatRotate(ornB, (b3Float4&)edge1);
+
+			b3Float4 crossje = b3Cross3(edge0World, edge1World);
+
+			curEdgeEdge++;
+			if (!b3IsAlmostZero((b3Vector3&)crossje))
+			{
+				crossje = b3FastNormalized3(crossje);
+				if (b3Dot3F4(deltaC2, crossje) < 0)
+					crossje *= -1.f;
+
+#ifdef TEST_INTERNAL_OBJECTS
+				gExpectedNbTests++;
+				if (gUseInternalObject && !TestInternalObjects(transA, transB, DeltaC2, Cross, hullA, hullB, dmin))
+					continue;
+				gActualNbTests++;
+#endif
+
+				b3Scalar dist;
+				if (!b3TestSepAxis(hullA, hullB, posA, ornA, posB, ornB, crossje, verticesA, verticesB, dist))
+					return false;
+
+				if (dist < dmin)
+				{
+					dmin = dist;
+					sep = (b3Vector3&)crossje;
+				}
+			}
+		}
+	}
+
+	if ((b3Dot3F4(-deltaC2, (b3Float4&)sep)) > 0.0f)
+		sep = -sep;
+
+	return true;
+}
+
+#endif  //B3_FIND_SEPARATING_AXIS_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3MprPenetration.h

@@ -0,0 +1,888 @@
+
+/***
+ * ---------------------------------
+ * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
+ *
+ *  This file was ported from mpr.c file, part of libccd.
+ *  The Minkoski Portal Refinement implementation was ported 
+ *  to OpenCL by Erwin Coumans for the Bullet 3 Physics library.
+ *  at http://github.com/erwincoumans/bullet3
+ *
+ *  Distributed under the OSI-approved BSD License (the "License");
+ *  see <http://www.opensource.org/licenses/bsd-license.php>.
+ *  This software is distributed WITHOUT ANY WARRANTY; without even the
+ *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *  See the License for more information.
+ */
+
+#ifndef B3_MPR_PENETRATION_H
+#define B3_MPR_PENETRATION_H
+
+#include "Bullet3Common/shared/b3PlatformDefinitions.h"
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+
+#ifdef __cplusplus
+#define B3_MPR_SQRT sqrtf
+#else
+#define B3_MPR_SQRT sqrt
+#endif
+#define B3_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
+#define B3_MPR_FABS fabs
+
+#define B3_MPR_TOLERANCE 1E-6f
+#define B3_MPR_MAX_ITERATIONS 1000
+
+struct _b3MprSupport_t
+{
+	b3Float4 v;   //!< Support point in minkowski sum
+	b3Float4 v1;  //!< Support point in obj1
+	b3Float4 v2;  //!< Support point in obj2
+};
+typedef struct _b3MprSupport_t b3MprSupport_t;
+
+struct _b3MprSimplex_t
+{
+	b3MprSupport_t ps[4];
+	int last;  //!< index of last added point
+};
+typedef struct _b3MprSimplex_t b3MprSimplex_t;
+
+inline b3MprSupport_t *b3MprSimplexPointW(b3MprSimplex_t *s, int idx)
+{
+	return &s->ps[idx];
+}
+
+inline void b3MprSimplexSetSize(b3MprSimplex_t *s, int size)
+{
+	s->last = size - 1;
+}
+
+inline int b3MprSimplexSize(const b3MprSimplex_t *s)
+{
+	return s->last + 1;
+}
+
+inline const b3MprSupport_t *b3MprSimplexPoint(const b3MprSimplex_t *s, int idx)
+{
+	// here is no check on boundaries
+	return &s->ps[idx];
+}
+
+inline void b3MprSupportCopy(b3MprSupport_t *d, const b3MprSupport_t *s)
+{
+	*d = *s;
+}
+
+inline void b3MprSimplexSet(b3MprSimplex_t *s, size_t pos, const b3MprSupport_t *a)
+{
+	b3MprSupportCopy(s->ps + pos, a);
+}
+
+inline void b3MprSimplexSwap(b3MprSimplex_t *s, size_t pos1, size_t pos2)
+{
+	b3MprSupport_t supp;
+
+	b3MprSupportCopy(&supp, &s->ps[pos1]);
+	b3MprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
+	b3MprSupportCopy(&s->ps[pos2], &supp);
+}
+
+inline int b3MprIsZero(float val)
+{
+	return B3_MPR_FABS(val) < FLT_EPSILON;
+}
+
+inline int b3MprEq(float _a, float _b)
+{
+	float ab;
+	float a, b;
+
+	ab = B3_MPR_FABS(_a - _b);
+	if (B3_MPR_FABS(ab) < FLT_EPSILON)
+		return 1;
+
+	a = B3_MPR_FABS(_a);
+	b = B3_MPR_FABS(_b);
+	if (b > a)
+	{
+		return ab < FLT_EPSILON * b;
+	}
+	else
+	{
+		return ab < FLT_EPSILON * a;
+	}
+}
+
+inline int b3MprVec3Eq(const b3Float4 *a, const b3Float4 *b)
+{
+	return b3MprEq((*a).x, (*b).x) && b3MprEq((*a).y, (*b).y) && b3MprEq((*a).z, (*b).z);
+}
+
+inline b3Float4 b3LocalGetSupportVertex(b3Float4ConstArg supportVec, __global const b3ConvexPolyhedronData_t *hull, b3ConstArray(b3Float4) verticesA)
+{
+	b3Float4 supVec = b3MakeFloat4(0, 0, 0, 0);
+	float maxDot = -B3_LARGE_FLOAT;
+
+	if (0 < hull->m_numVertices)
+	{
+		const b3Float4 scaled = supportVec;
+		int index = b3MaxDot(scaled, &verticesA[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
+		return verticesA[hull->m_vertexOffset + index];
+	}
+
+	return supVec;
+}
+
+B3_STATIC void b3MprConvexSupport(int pairIndex, int bodyIndex, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+								  b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+								  b3ConstArray(b3Collidable_t) cpuCollidables,
+								  b3ConstArray(b3Float4) cpuVertices,
+								  __global b3Float4 *sepAxis,
+								  const b3Float4 *_dir, b3Float4 *outp, int logme)
+{
+	//dir is in worldspace, move to local space
+
+	b3Float4 pos = cpuBodyBuf[bodyIndex].m_pos;
+	b3Quat orn = cpuBodyBuf[bodyIndex].m_quat;
+
+	b3Float4 dir = b3MakeFloat4((*_dir).x, (*_dir).y, (*_dir).z, 0.f);
+
+	const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn), dir);
+
+	//find local support vertex
+	int colIndex = cpuBodyBuf[bodyIndex].m_collidableIdx;
+
+	b3Assert(cpuCollidables[colIndex].m_shapeType == SHAPE_CONVEX_HULL);
+	__global const b3ConvexPolyhedronData_t *hull = &cpuConvexData[cpuCollidables[colIndex].m_shapeIndex];
+
+	b3Float4 pInA;
+	if (logme)
+	{
+		//	b3Float4 supVec = b3MakeFloat4(0,0,0,0);
+		float maxDot = -B3_LARGE_FLOAT;
+
+		if (0 < hull->m_numVertices)
+		{
+			const b3Float4 scaled = localDir;
+			int index = b3MaxDot(scaled, &cpuVertices[hull->m_vertexOffset], hull->m_numVertices, &maxDot);
+			pInA = cpuVertices[hull->m_vertexOffset + index];
+		}
+	}
+	else
+	{
+		pInA = b3LocalGetSupportVertex(localDir, hull, cpuVertices);
+	}
+
+	//move vertex to world space
+	*outp = b3TransformPoint(pInA, pos, orn);
+}
+
+inline void b3MprSupport(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+						 b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+						 b3ConstArray(b3Collidable_t) cpuCollidables,
+						 b3ConstArray(b3Float4) cpuVertices,
+						 __global b3Float4 *sepAxis,
+						 const b3Float4 *_dir, b3MprSupport_t *supp)
+{
+	b3Float4 dir;
+	dir = *_dir;
+	b3MprConvexSupport(pairIndex, bodyIndexA, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &supp->v1, 0);
+	dir = *_dir * -1.f;
+	b3MprConvexSupport(pairIndex, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &supp->v2, 0);
+	supp->v = supp->v1 - supp->v2;
+}
+
+inline void b3FindOrigin(int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf, b3MprSupport_t *center)
+{
+	center->v1 = cpuBodyBuf[bodyIndexA].m_pos;
+	center->v2 = cpuBodyBuf[bodyIndexB].m_pos;
+	center->v = center->v1 - center->v2;
+}
+
+inline void b3MprVec3Set(b3Float4 *v, float x, float y, float z)
+{
+	(*v).x = x;
+	(*v).y = y;
+	(*v).z = z;
+	(*v).w = 0.f;
+}
+
+inline void b3MprVec3Add(b3Float4 *v, const b3Float4 *w)
+{
+	(*v).x += (*w).x;
+	(*v).y += (*w).y;
+	(*v).z += (*w).z;
+}
+
+inline void b3MprVec3Copy(b3Float4 *v, const b3Float4 *w)
+{
+	*v = *w;
+}
+
+inline void b3MprVec3Scale(b3Float4 *d, float k)
+{
+	*d *= k;
+}
+
+inline float b3MprVec3Dot(const b3Float4 *a, const b3Float4 *b)
+{
+	float dot;
+
+	dot = b3Dot3F4(*a, *b);
+	return dot;
+}
+
+inline float b3MprVec3Len2(const b3Float4 *v)
+{
+	return b3MprVec3Dot(v, v);
+}
+
+inline void b3MprVec3Normalize(b3Float4 *d)
+{
+	float k = 1.f / B3_MPR_SQRT(b3MprVec3Len2(d));
+	b3MprVec3Scale(d, k);
+}
+
+inline void b3MprVec3Cross(b3Float4 *d, const b3Float4 *a, const b3Float4 *b)
+{
+	*d = b3Cross3(*a, *b);
+}
+
+inline void b3MprVec3Sub2(b3Float4 *d, const b3Float4 *v, const b3Float4 *w)
+{
+	*d = *v - *w;
+}
+
+inline void b3PortalDir(const b3MprSimplex_t *portal, b3Float4 *dir)
+{
+	b3Float4 v2v1, v3v1;
+
+	b3MprVec3Sub2(&v2v1, &b3MprSimplexPoint(portal, 2)->v,
+				  &b3MprSimplexPoint(portal, 1)->v);
+	b3MprVec3Sub2(&v3v1, &b3MprSimplexPoint(portal, 3)->v,
+				  &b3MprSimplexPoint(portal, 1)->v);
+	b3MprVec3Cross(dir, &v2v1, &v3v1);
+	b3MprVec3Normalize(dir);
+}
+
+inline int portalEncapsulesOrigin(const b3MprSimplex_t *portal,
+								  const b3Float4 *dir)
+{
+	float dot;
+	dot = b3MprVec3Dot(dir, &b3MprSimplexPoint(portal, 1)->v);
+	return b3MprIsZero(dot) || dot > 0.f;
+}
+
+inline int portalReachTolerance(const b3MprSimplex_t *portal,
+								const b3MprSupport_t *v4,
+								const b3Float4 *dir)
+{
+	float dv1, dv2, dv3, dv4;
+	float dot1, dot2, dot3;
+
+	// find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
+
+	dv1 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, dir);
+	dv2 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, dir);
+	dv3 = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, dir);
+	dv4 = b3MprVec3Dot(&v4->v, dir);
+
+	dot1 = dv4 - dv1;
+	dot2 = dv4 - dv2;
+	dot3 = dv4 - dv3;
+
+	dot1 = B3_MPR_FMIN(dot1, dot2);
+	dot1 = B3_MPR_FMIN(dot1, dot3);
+
+	return b3MprEq(dot1, B3_MPR_TOLERANCE) || dot1 < B3_MPR_TOLERANCE;
+}
+
+inline int portalCanEncapsuleOrigin(const b3MprSimplex_t *portal,
+									const b3MprSupport_t *v4,
+									const b3Float4 *dir)
+{
+	float dot;
+	dot = b3MprVec3Dot(&v4->v, dir);
+	return b3MprIsZero(dot) || dot > 0.f;
+}
+
+inline void b3ExpandPortal(b3MprSimplex_t *portal,
+						   const b3MprSupport_t *v4)
+{
+	float dot;
+	b3Float4 v4v0;
+
+	b3MprVec3Cross(&v4v0, &v4->v, &b3MprSimplexPoint(portal, 0)->v);
+	dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &v4v0);
+	if (dot > 0.f)
+	{
+		dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &v4v0);
+		if (dot > 0.f)
+		{
+			b3MprSimplexSet(portal, 1, v4);
+		}
+		else
+		{
+			b3MprSimplexSet(portal, 3, v4);
+		}
+	}
+	else
+	{
+		dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &v4v0);
+		if (dot > 0.f)
+		{
+			b3MprSimplexSet(portal, 2, v4);
+		}
+		else
+		{
+			b3MprSimplexSet(portal, 1, v4);
+		}
+	}
+}
+
+B3_STATIC int b3DiscoverPortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+							   b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+							   b3ConstArray(b3Collidable_t) cpuCollidables,
+							   b3ConstArray(b3Float4) cpuVertices,
+							   __global b3Float4 *sepAxis,
+							   __global int *hasSepAxis,
+							   b3MprSimplex_t *portal)
+{
+	b3Float4 dir, va, vb;
+	float dot;
+	int cont;
+
+	// vertex 0 is center of portal
+	b3FindOrigin(bodyIndexA, bodyIndexB, cpuBodyBuf, b3MprSimplexPointW(portal, 0));
+	// vertex 0 is center of portal
+	b3MprSimplexSetSize(portal, 1);
+
+	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
+	b3Float4 *b3mpr_vec3_origin = &zero;
+
+	if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 0)->v, b3mpr_vec3_origin))
+	{
+		// Portal's center lies on origin (0,0,0) => we know that objects
+		// intersect but we would need to know penetration info.
+		// So move center little bit...
+		b3MprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
+		b3MprVec3Add(&b3MprSimplexPointW(portal, 0)->v, &va);
+	}
+
+	// vertex 1 = support in direction of origin
+	b3MprVec3Copy(&dir, &b3MprSimplexPoint(portal, 0)->v);
+	b3MprVec3Scale(&dir, -1.f);
+	b3MprVec3Normalize(&dir);
+
+	b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 1));
+
+	b3MprSimplexSetSize(portal, 2);
+
+	// test if origin isn't outside of v1
+	dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 1)->v, &dir);
+
+	if (b3MprIsZero(dot) || dot < 0.f)
+		return -1;
+
+	// vertex 2
+	b3MprVec3Cross(&dir, &b3MprSimplexPoint(portal, 0)->v,
+				   &b3MprSimplexPoint(portal, 1)->v);
+	if (b3MprIsZero(b3MprVec3Len2(&dir)))
+	{
+		if (b3MprVec3Eq(&b3MprSimplexPoint(portal, 1)->v, b3mpr_vec3_origin))
+		{
+			// origin lies on v1
+			return 1;
+		}
+		else
+		{
+			// origin lies on v0-v1 segment
+			return 2;
+		}
+	}
+
+	b3MprVec3Normalize(&dir);
+	b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 2));
+
+	dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 2)->v, &dir);
+	if (b3MprIsZero(dot) || dot < 0.f)
+		return -1;
+
+	b3MprSimplexSetSize(portal, 3);
+
+	// vertex 3 direction
+	b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
+				  &b3MprSimplexPoint(portal, 0)->v);
+	b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
+				  &b3MprSimplexPoint(portal, 0)->v);
+	b3MprVec3Cross(&dir, &va, &vb);
+	b3MprVec3Normalize(&dir);
+
+	// it is better to form portal faces to be oriented "outside" origin
+	dot = b3MprVec3Dot(&dir, &b3MprSimplexPoint(portal, 0)->v);
+	if (dot > 0.f)
+	{
+		b3MprSimplexSwap(portal, 1, 2);
+		b3MprVec3Scale(&dir, -1.f);
+	}
+
+	while (b3MprSimplexSize(portal) < 4)
+	{
+		b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, b3MprSimplexPointW(portal, 3));
+
+		dot = b3MprVec3Dot(&b3MprSimplexPoint(portal, 3)->v, &dir);
+		if (b3MprIsZero(dot) || dot < 0.f)
+			return -1;
+
+		cont = 0;
+
+		// test if origin is outside (v1, v0, v3) - set v2 as v3 and
+		// continue
+		b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 1)->v,
+					   &b3MprSimplexPoint(portal, 3)->v);
+		dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
+		if (dot < 0.f && !b3MprIsZero(dot))
+		{
+			b3MprSimplexSet(portal, 2, b3MprSimplexPoint(portal, 3));
+			cont = 1;
+		}
+
+		if (!cont)
+		{
+			// test if origin is outside (v3, v0, v2) - set v1 as v3 and
+			// continue
+			b3MprVec3Cross(&va, &b3MprSimplexPoint(portal, 3)->v,
+						   &b3MprSimplexPoint(portal, 2)->v);
+			dot = b3MprVec3Dot(&va, &b3MprSimplexPoint(portal, 0)->v);
+			if (dot < 0.f && !b3MprIsZero(dot))
+			{
+				b3MprSimplexSet(portal, 1, b3MprSimplexPoint(portal, 3));
+				cont = 1;
+			}
+		}
+
+		if (cont)
+		{
+			b3MprVec3Sub2(&va, &b3MprSimplexPoint(portal, 1)->v,
+						  &b3MprSimplexPoint(portal, 0)->v);
+			b3MprVec3Sub2(&vb, &b3MprSimplexPoint(portal, 2)->v,
+						  &b3MprSimplexPoint(portal, 0)->v);
+			b3MprVec3Cross(&dir, &va, &vb);
+			b3MprVec3Normalize(&dir);
+		}
+		else
+		{
+			b3MprSimplexSetSize(portal, 4);
+		}
+	}
+
+	return 0;
+}
+
+B3_STATIC int b3RefinePortal(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+							 b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+							 b3ConstArray(b3Collidable_t) cpuCollidables,
+							 b3ConstArray(b3Float4) cpuVertices,
+							 __global b3Float4 *sepAxis,
+							 b3MprSimplex_t *portal)
+{
+	b3Float4 dir;
+	b3MprSupport_t v4;
+
+	for (int i = 0; i < B3_MPR_MAX_ITERATIONS; i++)
+	//while (1)
+	{
+		// compute direction outside the portal (from v0 throught v1,v2,v3
+		// face)
+		b3PortalDir(portal, &dir);
+
+		// test if origin is inside the portal
+		if (portalEncapsulesOrigin(portal, &dir))
+			return 0;
+
+		// get next support point
+
+		b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4);
+
+		// test if v4 can expand portal to contain origin and if portal
+		// expanding doesn't reach given tolerance
+		if (!portalCanEncapsuleOrigin(portal, &v4, &dir) || portalReachTolerance(portal, &v4, &dir))
+		{
+			return -1;
+		}
+
+		// v1-v2-v3 triangle must be rearranged to face outside Minkowski
+		// difference (direction from v0).
+		b3ExpandPortal(portal, &v4);
+	}
+
+	return -1;
+}
+
+B3_STATIC void b3FindPos(const b3MprSimplex_t *portal, b3Float4 *pos)
+{
+	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
+	b3Float4 *b3mpr_vec3_origin = &zero;
+
+	b3Float4 dir;
+	size_t i;
+	float b[4], sum, inv;
+	b3Float4 vec, p1, p2;
+
+	b3PortalDir(portal, &dir);
+
+	// use barycentric coordinates of tetrahedron to find origin
+	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
+				   &b3MprSimplexPoint(portal, 2)->v);
+	b[0] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
+
+	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
+				   &b3MprSimplexPoint(portal, 2)->v);
+	b[1] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
+
+	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 0)->v,
+				   &b3MprSimplexPoint(portal, 1)->v);
+	b[2] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 3)->v);
+
+	b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
+				   &b3MprSimplexPoint(portal, 1)->v);
+	b[3] = b3MprVec3Dot(&vec, &b3MprSimplexPoint(portal, 0)->v);
+
+	sum = b[0] + b[1] + b[2] + b[3];
+
+	if (b3MprIsZero(sum) || sum < 0.f)
+	{
+		b[0] = 0.f;
+
+		b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 2)->v,
+					   &b3MprSimplexPoint(portal, 3)->v);
+		b[1] = b3MprVec3Dot(&vec, &dir);
+		b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 3)->v,
+					   &b3MprSimplexPoint(portal, 1)->v);
+		b[2] = b3MprVec3Dot(&vec, &dir);
+		b3MprVec3Cross(&vec, &b3MprSimplexPoint(portal, 1)->v,
+					   &b3MprSimplexPoint(portal, 2)->v);
+		b[3] = b3MprVec3Dot(&vec, &dir);
+
+		sum = b[1] + b[2] + b[3];
+	}
+
+	inv = 1.f / sum;
+
+	b3MprVec3Copy(&p1, b3mpr_vec3_origin);
+	b3MprVec3Copy(&p2, b3mpr_vec3_origin);
+	for (i = 0; i < 4; i++)
+	{
+		b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v1);
+		b3MprVec3Scale(&vec, b[i]);
+		b3MprVec3Add(&p1, &vec);
+
+		b3MprVec3Copy(&vec, &b3MprSimplexPoint(portal, i)->v2);
+		b3MprVec3Scale(&vec, b[i]);
+		b3MprVec3Add(&p2, &vec);
+	}
+	b3MprVec3Scale(&p1, inv);
+	b3MprVec3Scale(&p2, inv);
+
+	b3MprVec3Copy(pos, &p1);
+	b3MprVec3Add(pos, &p2);
+	b3MprVec3Scale(pos, 0.5);
+}
+
+inline float b3MprVec3Dist2(const b3Float4 *a, const b3Float4 *b)
+{
+	b3Float4 ab;
+	b3MprVec3Sub2(&ab, a, b);
+	return b3MprVec3Len2(&ab);
+}
+
+inline float _b3MprVec3PointSegmentDist2(const b3Float4 *P,
+										 const b3Float4 *x0,
+										 const b3Float4 *b,
+										 b3Float4 *witness)
+{
+	// The computation comes from solving equation of segment:
+	//      S(t) = x0 + t.d
+	//          where - x0 is initial point of segment
+	//                - d is direction of segment from x0 (|d| > 0)
+	//                - t belongs to <0, 1> interval
+	//
+	// Than, distance from a segment to some point P can be expressed:
+	//      D(t) = |x0 + t.d - P|^2
+	//          which is distance from any point on segment. Minimization
+	//          of this function brings distance from P to segment.
+	// Minimization of D(t) leads to simple quadratic equation that's
+	// solving is straightforward.
+	//
+	// Bonus of this method is witness point for free.
+
+	float dist, t;
+	b3Float4 d, a;
+
+	// direction of segment
+	b3MprVec3Sub2(&d, b, x0);
+
+	// precompute vector from P to x0
+	b3MprVec3Sub2(&a, x0, P);
+
+	t = -1.f * b3MprVec3Dot(&a, &d);
+	t /= b3MprVec3Len2(&d);
+
+	if (t < 0.f || b3MprIsZero(t))
+	{
+		dist = b3MprVec3Dist2(x0, P);
+		if (witness)
+			b3MprVec3Copy(witness, x0);
+	}
+	else if (t > 1.f || b3MprEq(t, 1.f))
+	{
+		dist = b3MprVec3Dist2(b, P);
+		if (witness)
+			b3MprVec3Copy(witness, b);
+	}
+	else
+	{
+		if (witness)
+		{
+			b3MprVec3Copy(witness, &d);
+			b3MprVec3Scale(witness, t);
+			b3MprVec3Add(witness, x0);
+			dist = b3MprVec3Dist2(witness, P);
+		}
+		else
+		{
+			// recycling variables
+			b3MprVec3Scale(&d, t);
+			b3MprVec3Add(&d, &a);
+			dist = b3MprVec3Len2(&d);
+		}
+	}
+
+	return dist;
+}
+
+inline float b3MprVec3PointTriDist2(const b3Float4 *P,
+									const b3Float4 *x0, const b3Float4 *B,
+									const b3Float4 *C,
+									b3Float4 *witness)
+{
+	// Computation comes from analytic expression for triangle (x0, B, C)
+	//      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
+	// Then equation for distance is:
+	//      D(s, t) = | T(s, t) - P |^2
+	// This leads to minimization of quadratic function of two variables.
+	// The solution from is taken only if s is between 0 and 1, t is
+	// between 0 and 1 and t + s < 1, otherwise distance from segment is
+	// computed.
+
+	b3Float4 d1, d2, a;
+	float u, v, w, p, q, r;
+	float s, t, dist, dist2;
+	b3Float4 witness2;
+
+	b3MprVec3Sub2(&d1, B, x0);
+	b3MprVec3Sub2(&d2, C, x0);
+	b3MprVec3Sub2(&a, x0, P);
+
+	u = b3MprVec3Dot(&a, &a);
+	v = b3MprVec3Dot(&d1, &d1);
+	w = b3MprVec3Dot(&d2, &d2);
+	p = b3MprVec3Dot(&a, &d1);
+	q = b3MprVec3Dot(&a, &d2);
+	r = b3MprVec3Dot(&d1, &d2);
+
+	s = (q * r - w * p) / (w * v - r * r);
+	t = (-s * r - q) / w;
+
+	if ((b3MprIsZero(s) || s > 0.f) && (b3MprEq(s, 1.f) || s < 1.f) && (b3MprIsZero(t) || t > 0.f) && (b3MprEq(t, 1.f) || t < 1.f) && (b3MprEq(t + s, 1.f) || t + s < 1.f))
+	{
+		if (witness)
+		{
+			b3MprVec3Scale(&d1, s);
+			b3MprVec3Scale(&d2, t);
+			b3MprVec3Copy(witness, x0);
+			b3MprVec3Add(witness, &d1);
+			b3MprVec3Add(witness, &d2);
+
+			dist = b3MprVec3Dist2(witness, P);
+		}
+		else
+		{
+			dist = s * s * v;
+			dist += t * t * w;
+			dist += 2.f * s * t * r;
+			dist += 2.f * s * p;
+			dist += 2.f * t * q;
+			dist += u;
+		}
+	}
+	else
+	{
+		dist = _b3MprVec3PointSegmentDist2(P, x0, B, witness);
+
+		dist2 = _b3MprVec3PointSegmentDist2(P, x0, C, &witness2);
+		if (dist2 < dist)
+		{
+			dist = dist2;
+			if (witness)
+				b3MprVec3Copy(witness, &witness2);
+		}
+
+		dist2 = _b3MprVec3PointSegmentDist2(P, B, C, &witness2);
+		if (dist2 < dist)
+		{
+			dist = dist2;
+			if (witness)
+				b3MprVec3Copy(witness, &witness2);
+		}
+	}
+
+	return dist;
+}
+
+B3_STATIC void b3FindPenetr(int pairIndex, int bodyIndexA, int bodyIndexB, b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+							b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+							b3ConstArray(b3Collidable_t) cpuCollidables,
+							b3ConstArray(b3Float4) cpuVertices,
+							__global b3Float4 *sepAxis,
+							b3MprSimplex_t *portal,
+							float *depth, b3Float4 *pdir, b3Float4 *pos)
+{
+	b3Float4 dir;
+	b3MprSupport_t v4;
+	unsigned long iterations;
+
+	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
+	b3Float4 *b3mpr_vec3_origin = &zero;
+
+	iterations = 1UL;
+	for (int i = 0; i < B3_MPR_MAX_ITERATIONS; i++)
+	//while (1)
+	{
+		// compute portal direction and obtain next support point
+		b3PortalDir(portal, &dir);
+
+		b3MprSupport(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &dir, &v4);
+
+		// reached tolerance -> find penetration info
+		if (portalReachTolerance(portal, &v4, &dir) || iterations == B3_MPR_MAX_ITERATIONS)
+		{
+			*depth = b3MprVec3PointTriDist2(b3mpr_vec3_origin, &b3MprSimplexPoint(portal, 1)->v, &b3MprSimplexPoint(portal, 2)->v, &b3MprSimplexPoint(portal, 3)->v, pdir);
+			*depth = B3_MPR_SQRT(*depth);
+
+			if (b3MprIsZero((*pdir).x) && b3MprIsZero((*pdir).y) && b3MprIsZero((*pdir).z))
+			{
+				*pdir = dir;
+			}
+			b3MprVec3Normalize(pdir);
+
+			// barycentric coordinates:
+			b3FindPos(portal, pos);
+
+			return;
+		}
+
+		b3ExpandPortal(portal, &v4);
+
+		iterations++;
+	}
+}
+
+B3_STATIC void b3FindPenetrTouch(b3MprSimplex_t *portal, float *depth, b3Float4 *dir, b3Float4 *pos)
+{
+	// Touching contact on portal's v1 - so depth is zero and direction
+	// is unimportant and pos can be guessed
+	*depth = 0.f;
+	b3Float4 zero = b3MakeFloat4(0, 0, 0, 0);
+	b3Float4 *b3mpr_vec3_origin = &zero;
+
+	b3MprVec3Copy(dir, b3mpr_vec3_origin);
+
+	b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
+	b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
+	b3MprVec3Scale(pos, 0.5);
+}
+
+B3_STATIC void b3FindPenetrSegment(b3MprSimplex_t *portal,
+								   float *depth, b3Float4 *dir, b3Float4 *pos)
+{
+	// Origin lies on v0-v1 segment.
+	// Depth is distance to v1, direction also and position must be
+	// computed
+
+	b3MprVec3Copy(pos, &b3MprSimplexPoint(portal, 1)->v1);
+	b3MprVec3Add(pos, &b3MprSimplexPoint(portal, 1)->v2);
+	b3MprVec3Scale(pos, 0.5f);
+
+	b3MprVec3Copy(dir, &b3MprSimplexPoint(portal, 1)->v);
+	*depth = B3_MPR_SQRT(b3MprVec3Len2(dir));
+	b3MprVec3Normalize(dir);
+}
+
+inline int b3MprPenetration(int pairIndex, int bodyIndexA, int bodyIndexB,
+							b3ConstArray(b3RigidBodyData_t) cpuBodyBuf,
+							b3ConstArray(b3ConvexPolyhedronData_t) cpuConvexData,
+							b3ConstArray(b3Collidable_t) cpuCollidables,
+							b3ConstArray(b3Float4) cpuVertices,
+							__global b3Float4 *sepAxis,
+							__global int *hasSepAxis,
+							float *depthOut, b3Float4 *dirOut, b3Float4 *posOut)
+{
+	b3MprSimplex_t portal;
+
+	//	if (!hasSepAxis[pairIndex])
+	//	return -1;
+
+	hasSepAxis[pairIndex] = 0;
+	int res;
+
+	// Phase 1: Portal discovery
+	res = b3DiscoverPortal(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, hasSepAxis, &portal);
+
+	//sepAxis[pairIndex] = *pdir;//or -dir?
+
+	switch (res)
+	{
+		case 0:
+		{
+			// Phase 2: Portal refinement
+
+			res = b3RefinePortal(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &portal);
+			if (res < 0)
+				return -1;
+
+			// Phase 3. Penetration info
+			b3FindPenetr(pairIndex, bodyIndexA, bodyIndexB, cpuBodyBuf, cpuConvexData, cpuCollidables, cpuVertices, sepAxis, &portal, depthOut, dirOut, posOut);
+			hasSepAxis[pairIndex] = 1;
+			sepAxis[pairIndex] = -*dirOut;
+			break;
+		}
+		case 1:
+		{
+			// Touching contact on portal's v1.
+			b3FindPenetrTouch(&portal, depthOut, dirOut, posOut);
+			break;
+		}
+		case 2:
+		{
+			b3FindPenetrSegment(&portal, depthOut, dirOut, posOut);
+			break;
+		}
+		default:
+		{
+			hasSepAxis[pairIndex] = 0;
+			//if (res < 0)
+			//{
+			// Origin isn't inside portal - no collision.
+			return -1;
+			//}
+		}
+	};
+
+	return 0;
+};
+
+#endif  //B3_MPR_PENETRATION_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3NewContactReduction.h

@@ -0,0 +1,175 @@
+
+#ifndef B3_NEW_CONTACT_REDUCTION_H
+#define B3_NEW_CONTACT_REDUCTION_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Contact4Data.h"
+
+#define GET_NPOINTS(x) (x).m_worldNormalOnB.w
+
+int b3ExtractManifoldSequentialGlobal(__global const b3Float4* p, int nPoints, b3Float4ConstArg nearNormal, b3Int4* contactIdx)
+{
+	if (nPoints == 0)
+		return 0;
+
+	if (nPoints <= 4)
+		return nPoints;
+
+	if (nPoints > 64)
+		nPoints = 64;
+
+	b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
+	{
+		for (int i = 0; i < nPoints; i++)
+			center += p[i];
+		center /= (float)nPoints;
+	}
+
+	//	sample 4 directions
+
+	b3Float4 aVector = p[0] - center;
+	b3Float4 u = b3Cross(nearNormal, aVector);
+	b3Float4 v = b3Cross(nearNormal, u);
+	u = b3Normalized(u);
+	v = b3Normalized(v);
+
+	//keep point with deepest penetration
+	float minW = FLT_MAX;
+
+	int minIndex = -1;
+
+	b3Float4 maxDots;
+	maxDots.x = FLT_MIN;
+	maxDots.y = FLT_MIN;
+	maxDots.z = FLT_MIN;
+	maxDots.w = FLT_MIN;
+
+	//	idx, distance
+	for (int ie = 0; ie < nPoints; ie++)
+	{
+		if (p[ie].w < minW)
+		{
+			minW = p[ie].w;
+			minIndex = ie;
+		}
+		float f;
+		b3Float4 r = p[ie] - center;
+		f = b3Dot(u, r);
+		if (f < maxDots.x)
+		{
+			maxDots.x = f;
+			contactIdx[0].x = ie;
+		}
+
+		f = b3Dot(-u, r);
+		if (f < maxDots.y)
+		{
+			maxDots.y = f;
+			contactIdx[0].y = ie;
+		}
+
+		f = b3Dot(v, r);
+		if (f < maxDots.z)
+		{
+			maxDots.z = f;
+			contactIdx[0].z = ie;
+		}
+
+		f = b3Dot(-v, r);
+		if (f < maxDots.w)
+		{
+			maxDots.w = f;
+			contactIdx[0].w = ie;
+		}
+	}
+
+	if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
+	{
+		//replace the first contact with minimum (todo: replace contact with least penetration)
+		contactIdx[0].x = minIndex;
+	}
+
+	return 4;
+}
+
+__kernel void b3NewContactReductionKernel(__global b3Int4* pairs,
+										  __global const b3RigidBodyData_t* rigidBodies,
+										  __global const b3Float4* separatingNormals,
+										  __global const int* hasSeparatingAxis,
+										  __global struct b3Contact4Data* globalContactsOut,
+										  __global b3Int4* clippingFaces,
+										  __global b3Float4* worldVertsB2,
+										  volatile __global int* nGlobalContactsOut,
+										  int vertexFaceCapacity,
+										  int contactCapacity,
+										  int numPairs,
+										  int pairIndex)
+{
+	//    int i = get_global_id(0);
+	//int pairIndex = i;
+	int i = pairIndex;
+
+	b3Int4 contactIdx;
+	contactIdx = b3MakeInt4(0, 1, 2, 3);
+
+	if (i < numPairs)
+	{
+		if (hasSeparatingAxis[i])
+		{
+			int nPoints = clippingFaces[pairIndex].w;
+
+			if (nPoints > 0)
+			{
+				__global b3Float4* pointsIn = &worldVertsB2[pairIndex * vertexFaceCapacity];
+				b3Float4 normal = -separatingNormals[i];
+
+				int nReducedContacts = b3ExtractManifoldSequentialGlobal(pointsIn, nPoints, normal, &contactIdx);
+
+				int dstIdx;
+				dstIdx = b3AtomicInc(nGlobalContactsOut);
+
+				//#if 0
+				b3Assert(dstIdx < contactCapacity);
+				if (dstIdx < contactCapacity)
+				{
+					__global struct b3Contact4Data* c = &globalContactsOut[dstIdx];
+					c->m_worldNormalOnB = -normal;
+					c->m_restituitionCoeffCmp = (0.f * 0xffff);
+					c->m_frictionCoeffCmp = (0.7f * 0xffff);
+					c->m_batchIdx = pairIndex;
+					int bodyA = pairs[pairIndex].x;
+					int bodyB = pairs[pairIndex].y;
+
+					pairs[pairIndex].w = dstIdx;
+
+					c->m_bodyAPtrAndSignBit = rigidBodies[bodyA].m_invMass == 0 ? -bodyA : bodyA;
+					c->m_bodyBPtrAndSignBit = rigidBodies[bodyB].m_invMass == 0 ? -bodyB : bodyB;
+					c->m_childIndexA = -1;
+					c->m_childIndexB = -1;
+
+					switch (nReducedContacts)
+					{
+						case 4:
+							c->m_worldPosB[3] = pointsIn[contactIdx.w];
+						case 3:
+							c->m_worldPosB[2] = pointsIn[contactIdx.z];
+						case 2:
+							c->m_worldPosB[1] = pointsIn[contactIdx.y];
+						case 1:
+							c->m_worldPosB[0] = pointsIn[contactIdx.x];
+						default:
+						{
+						}
+					};
+
+					GET_NPOINTS(*c) = nReducedContacts;
+				}
+
+				//#endif
+
+			}  //		if (numContactsOut>0)
+		}      //		if (hasSeparatingAxis[i])
+	}          //	if (i<numPairs)
+}
+#endif

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h

@@ -0,0 +1,88 @@
+
+
+#ifndef B3_QUANTIZED_BVH_NODE_H
+#define B3_QUANTIZED_BVH_NODE_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+
+#define B3_MAX_NUM_PARTS_IN_BITS 10
+
+///b3QuantizedBvhNodeData is a compressed aabb node, 16 bytes.
+///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
+typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t;
+
+struct b3QuantizedBvhNodeData
+{
+	//12 bytes
+	unsigned short int m_quantizedAabbMin[3];
+	unsigned short int m_quantizedAabbMax[3];
+	//4 bytes
+	int m_escapeIndexOrTriangleIndex;
+};
+
+inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
+{
+	unsigned int x = 0;
+	unsigned int y = (~(x & 0)) << (31 - B3_MAX_NUM_PARTS_IN_BITS);
+	// Get only the lower bits where the triangle index is stored
+	return (rootNode->m_escapeIndexOrTriangleIndex & ~(y));
+}
+
+inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode)
+{
+	//skipindex is negative (internal node), triangleindex >=0 (leafnode)
+	return (rootNode->m_escapeIndexOrTriangleIndex >= 0) ? 1 : 0;
+}
+
+inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode)
+{
+	return -rootNode->m_escapeIndexOrTriangleIndex;
+}
+
+inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2, int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
+{
+	b3Float4 clampedPoint = b3MaxFloat4(point2, bvhAabbMin);
+	clampedPoint = b3MinFloat4(clampedPoint, bvhAabbMax);
+
+	b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
+	if (isMax)
+	{
+		out[0] = (unsigned short)(((unsigned short)(v.x + 1.f) | 1));
+		out[1] = (unsigned short)(((unsigned short)(v.y + 1.f) | 1));
+		out[2] = (unsigned short)(((unsigned short)(v.z + 1.f) | 1));
+	}
+	else
+	{
+		out[0] = (unsigned short)(((unsigned short)(v.x) & 0xfffe));
+		out[1] = (unsigned short)(((unsigned short)(v.y) & 0xfffe));
+		out[2] = (unsigned short)(((unsigned short)(v.z) & 0xfffe));
+	}
+}
+
+inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
+	const unsigned short int* aabbMin1,
+	const unsigned short int* aabbMax1,
+	const unsigned short int* aabbMin2,
+	const unsigned short int* aabbMax2)
+{
+	//int overlap = 1;
+	if (aabbMin1[0] > aabbMax2[0])
+		return 0;
+	if (aabbMax1[0] < aabbMin2[0])
+		return 0;
+	if (aabbMin1[1] > aabbMax2[1])
+		return 0;
+	if (aabbMax1[1] < aabbMin2[1])
+		return 0;
+	if (aabbMin1[2] > aabbMax2[2])
+		return 0;
+	if (aabbMax1[2] < aabbMin2[2])
+		return 0;
+	return 1;
+	//overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap;
+	//overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap;
+	//overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap;
+	//return overlap;
+}
+
+#endif  //B3_QUANTIZED_BVH_NODE_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3ReduceContacts.h

@@ -0,0 +1,89 @@
+#ifndef B3_REDUCE_CONTACTS_H
+#define B3_REDUCE_CONTACTS_H
+
+inline int b3ReduceContacts(const b3Float4* p, int nPoints, const b3Float4& nearNormal, b3Int4* contactIdx)
+{
+	if (nPoints == 0)
+		return 0;
+
+	if (nPoints <= 4)
+		return nPoints;
+
+	if (nPoints > 64)
+		nPoints = 64;
+
+	b3Float4 center = b3MakeFloat4(0, 0, 0, 0);
+	{
+		for (int i = 0; i < nPoints; i++)
+			center += p[i];
+		center /= (float)nPoints;
+	}
+
+	//	sample 4 directions
+
+	b3Float4 aVector = p[0] - center;
+	b3Float4 u = b3Cross3(nearNormal, aVector);
+	b3Float4 v = b3Cross3(nearNormal, u);
+	u = b3FastNormalized3(u);
+	v = b3FastNormalized3(v);
+
+	//keep point with deepest penetration
+	float minW = FLT_MAX;
+
+	int minIndex = -1;
+
+	b3Float4 maxDots;
+	maxDots.x = FLT_MIN;
+	maxDots.y = FLT_MIN;
+	maxDots.z = FLT_MIN;
+	maxDots.w = FLT_MIN;
+
+	//	idx, distance
+	for (int ie = 0; ie < nPoints; ie++)
+	{
+		if (p[ie].w < minW)
+		{
+			minW = p[ie].w;
+			minIndex = ie;
+		}
+		float f;
+		b3Float4 r = p[ie] - center;
+		f = b3Dot3F4(u, r);
+		if (f < maxDots.x)
+		{
+			maxDots.x = f;
+			contactIdx[0].x = ie;
+		}
+
+		f = b3Dot3F4(-u, r);
+		if (f < maxDots.y)
+		{
+			maxDots.y = f;
+			contactIdx[0].y = ie;
+		}
+
+		f = b3Dot3F4(v, r);
+		if (f < maxDots.z)
+		{
+			maxDots.z = f;
+			contactIdx[0].z = ie;
+		}
+
+		f = b3Dot3F4(-v, r);
+		if (f < maxDots.w)
+		{
+			maxDots.w = f;
+			contactIdx[0].w = ie;
+		}
+	}
+
+	if (contactIdx[0].x != minIndex && contactIdx[0].y != minIndex && contactIdx[0].z != minIndex && contactIdx[0].w != minIndex)
+	{
+		//replace the first contact with minimum (todo: replace contact with least penetration)
+		contactIdx[0].x = minIndex;
+	}
+
+	return 4;
+}
+
+#endif  //B3_REDUCE_CONTACTS_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h

@@ -0,0 +1,31 @@
+#ifndef B3_RIGIDBODY_DATA_H
+#define B3_RIGIDBODY_DATA_H
+
+#include "Bullet3Common/shared/b3Float4.h"
+#include "Bullet3Common/shared/b3Quat.h"
+#include "Bullet3Common/shared/b3Mat3x3.h"
+
+typedef struct b3RigidBodyData b3RigidBodyData_t;
+
+struct b3RigidBodyData
+{
+	b3Float4 m_pos;
+	b3Quat m_quat;
+	b3Float4 m_linVel;
+	b3Float4 m_angVel;
+
+	int m_collidableIdx;
+	float m_invMass;
+	float m_restituitionCoeff;
+	float m_frictionCoeff;
+};
+
+typedef struct b3InertiaData b3InertiaData_t;
+
+struct b3InertiaData
+{
+	b3Mat3x3 m_invInertiaWorld;
+	b3Mat3x3 m_initInvInertia;
+};
+
+#endif  //B3_RIGIDBODY_DATA_H

thirdparty/bullet/src/Bullet3Collision/NarrowPhaseCollision/shared/b3UpdateAabbs.h

@@ -0,0 +1,35 @@
+#ifndef B3_UPDATE_AABBS_H
+#define B3_UPDATE_AABBS_H
+
+#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
+#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
+
+void b3ComputeWorldAabb(int bodyId, __global const b3RigidBodyData_t* bodies, __global const b3Collidable_t* collidables, __global const b3Aabb_t* localShapeAABB, __global b3Aabb_t* worldAabbs)
+{
+	__global const b3RigidBodyData_t* body = &bodies[bodyId];
+
+	b3Float4 position = body->m_pos;
+	b3Quat orientation = body->m_quat;
+
+	int collidableIndex = body->m_collidableIdx;
+	int shapeIndex = collidables[collidableIndex].m_shapeIndex;
+
+	if (shapeIndex >= 0)
+	{
+		b3Aabb_t localAabb = localShapeAABB[collidableIndex];
+		b3Aabb_t worldAabb;
+
+		b3Float4 aabbAMinOut, aabbAMaxOut;
+		float margin = 0.f;
+		b3TransformAabb2(localAabb.m_minVec, localAabb.m_maxVec, margin, position, orientation, &aabbAMinOut, &aabbAMaxOut);
+
+		worldAabb.m_minVec = aabbAMinOut;
+		worldAabb.m_minIndices[3] = bodyId;
+		worldAabb.m_maxVec = aabbAMaxOut;
+		worldAabb.m_signedMaxIndices[3] = body[bodyId].m_invMass == 0.f ? 0 : 1;
+		worldAabbs[bodyId] = worldAabb;
+	}
+}
+
+#endif  //B3_UPDATE_AABBS_H

thirdparty/bullet/src/Bullet3Collision/premake4.lua

@@ -0,0 +1,16 @@
+	project "Bullet3Collision"
+
+	language "C++"
+				
+	kind "StaticLib"
+		
+	includedirs {".."}
+
+    if os.is("Linux") then
+        buildoptions{"-fPIC"}
+    end
+
+	files {
+		"**.cpp",
+		"**.h"
+	}

thirdparty/bullet/src/Bullet3Common/CMakeLists.txt

@@ -0,0 +1,63 @@
+
+INCLUDE_DIRECTORIES(
+	${BULLET_PHYSICS_SOURCE_DIR}/src
+)
+
+SET(Bullet3Common_SRCS
+	b3AlignedAllocator.cpp
+	b3Vector3.cpp
+	b3Logging.cpp
+)
+
+SET(Bullet3Common_HDRS
+	b3AlignedAllocator.h
+	b3AlignedObjectArray.h
+	b3CommandLineArgs.h
+	b3HashMap.h
+	b3Logging.h
+	b3Matrix3x3.h
+	b3MinMax.h
+	b3PoolAllocator.h
+	b3QuadWord.h
+	b3Quaternion.h
+	b3Random.h
+	b3Scalar.h
+	b3StackAlloc.h
+	b3Transform.h
+	b3TransformUtil.h
+	b3Vector3.h
+	shared/b3Float4
+	shared/b3Int2.h
+	shared/b3Int4.h
+	shared/b3Mat3x3.h
+	shared/b3PlatformDefinitions
+	shared/b3Quat.h
+)
+
+ADD_LIBRARY(Bullet3Common ${Bullet3Common_SRCS} ${Bullet3Common_HDRS})
+SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES SOVERSION ${BULLET_VERSION})
+
+IF (INSTALL_LIBS)
+	IF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+		#FILES_MATCHING requires CMake 2.6
+		IF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+			IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Common DESTINATION .)
+			ELSE (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+				INSTALL(TARGETS Bullet3Common
+					RUNTIME DESTINATION bin
+					LIBRARY DESTINATION lib${LIB_SUFFIX}
+					ARCHIVE DESTINATION lib${LIB_SUFFIX})
+				INSTALL(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+DESTINATION ${INCLUDE_INSTALL_DIR} FILES_MATCHING PATTERN "*.h"  PATTERN
+".svn" EXCLUDE PATTERN "CMakeFiles" EXCLUDE)
+			ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+		ENDIF (${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} GREATER 2.5)
+
+		IF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+			SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES FRAMEWORK true)
+			SET_TARGET_PROPERTIES(Bullet3Common PROPERTIES PUBLIC_HEADER "${Bullet3Common_HDRS}")
+		ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
+	ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
+ENDIF (INSTALL_LIBS)

thirdparty/bullet/src/Bullet3Common/b3AlignedAllocator.cpp

@@ -0,0 +1,186 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "b3AlignedAllocator.h"
+
+#ifdef B3_ALLOCATOR_STATISTICS
+int b3g_numAlignedAllocs = 0;
+int b3g_numAlignedFree = 0;
+int b3g_totalBytesAlignedAllocs = 0;  //detect memory leaks
+#endif
+
+static void *b3AllocDefault(size_t size)
+{
+	return malloc(size);
+}
+
+static void b3FreeDefault(void *ptr)
+{
+	free(ptr);
+}
+
+static b3AllocFunc *b3s_allocFunc = b3AllocDefault;
+static b3FreeFunc *b3s_freeFunc = b3FreeDefault;
+
+#if defined(B3_HAS_ALIGNED_ALLOCATOR)
+#include <malloc.h>
+static void *b3AlignedAllocDefault(size_t size, int alignment)
+{
+	return _aligned_malloc(size, (size_t)alignment);
+}
+
+static void b3AlignedFreeDefault(void *ptr)
+{
+	_aligned_free(ptr);
+}
+#elif defined(__CELLOS_LV2__)
+#include <stdlib.h>
+
+static inline void *b3AlignedAllocDefault(size_t size, int alignment)
+{
+	return memalign(alignment, size);
+}
+
+static inline void b3AlignedFreeDefault(void *ptr)
+{
+	free(ptr);
+}
+#else
+
+static inline void *b3AlignedAllocDefault(size_t size, int alignment)
+{
+	void *ret;
+	char *real;
+	real = (char *)b3s_allocFunc(size + sizeof(void *) + (alignment - 1));
+	if (real)
+	{
+		ret = b3AlignPointer(real + sizeof(void *), alignment);
+		*((void **)(ret)-1) = (void *)(real);
+	}
+	else
+	{
+		ret = (void *)(real);
+	}
+	return (ret);
+}
+
+static inline void b3AlignedFreeDefault(void *ptr)
+{
+	void *real;
+
+	if (ptr)
+	{
+		real = *((void **)(ptr)-1);
+		b3s_freeFunc(real);
+	}
+}
+#endif
+
+static b3AlignedAllocFunc *b3s_alignedAllocFunc = b3AlignedAllocDefault;
+static b3AlignedFreeFunc *b3s_alignedFreeFunc = b3AlignedFreeDefault;
+
+void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc *allocFunc, b3AlignedFreeFunc *freeFunc)
+{
+	b3s_alignedAllocFunc = allocFunc ? allocFunc : b3AlignedAllocDefault;
+	b3s_alignedFreeFunc = freeFunc ? freeFunc : b3AlignedFreeDefault;
+}
+
+void b3AlignedAllocSetCustom(b3AllocFunc *allocFunc, b3FreeFunc *freeFunc)
+{
+	b3s_allocFunc = allocFunc ? allocFunc : b3AllocDefault;
+	b3s_freeFunc = freeFunc ? freeFunc : b3FreeDefault;
+}
+
+#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
+//this generic allocator provides the total allocated number of bytes
+#include <stdio.h>
+
+void *b3AlignedAllocInternal(size_t size, int alignment, int line, char *filename)
+{
+	void *ret;
+	char *real;
+#ifdef B3_ALLOCATOR_STATISTICS
+	b3g_totalBytesAlignedAllocs += size;
+	b3g_numAlignedAllocs++;
+#endif
+	real = (char *)b3s_allocFunc(size + 2 * sizeof(void *) + (alignment - 1));
+	if (real)
+	{
+		ret = (void *)b3AlignPointer(real + 2 * sizeof(void *), alignment);
+		*((void **)(ret)-1) = (void *)(real);
+		*((int *)(ret)-2) = size;
+	}
+	else
+	{
+		ret = (void *)(real);  //??
+	}
+
+	b3Printf("allocation#%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedAllocs, real, filename, line, size);
+
+	int *ptr = (int *)ret;
+	*ptr = 12;
+	return (ret);
+}
+
+void b3AlignedFreeInternal(void *ptr, int line, char *filename)
+{
+	void *real;
+#ifdef B3_ALLOCATOR_STATISTICS
+	b3g_numAlignedFree++;
+#endif
+	if (ptr)
+	{
+		real = *((void **)(ptr)-1);
+		int size = *((int *)(ptr)-2);
+#ifdef B3_ALLOCATOR_STATISTICS
+		b3g_totalBytesAlignedAllocs -= size;
+#endif
+		b3Printf("free #%d at address %x, from %s,line %d, size %d\n", b3g_numAlignedFree, real, filename, line, size);
+
+		b3s_freeFunc(real);
+	}
+	else
+	{
+		b3Printf("NULL ptr\n");
+	}
+}
+
+#else  //B3_DEBUG_MEMORY_ALLOCATIONS
+
+void *b3AlignedAllocInternal(size_t size, int alignment)
+{
+#ifdef B3_ALLOCATOR_STATISTICS
+	b3g_numAlignedAllocs++;
+#endif
+	void *ptr;
+	ptr = b3s_alignedAllocFunc(size, alignment);
+	//	b3Printf("b3AlignedAllocInternal %d, %x\n",size,ptr);
+	return ptr;
+}
+
+void b3AlignedFreeInternal(void *ptr)
+{
+	if (!ptr)
+	{
+		return;
+	}
+#ifdef B3_ALLOCATOR_STATISTICS
+	b3g_numAlignedFree++;
+#endif
+	//	b3Printf("b3AlignedFreeInternal %x\n",ptr);
+	b3s_alignedFreeFunc(ptr);
+}
+
+#endif  //B3_DEBUG_MEMORY_ALLOCATIONS

thirdparty/bullet/src/Bullet3Common/b3AlignedAllocator.h

@@ -0,0 +1,110 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_ALIGNED_ALLOCATOR
+#define B3_ALIGNED_ALLOCATOR
+
+///we probably replace this with our own aligned memory allocator
+///so we replace _aligned_malloc and _aligned_free with our own
+///that is better portable and more predictable
+
+#include "b3Scalar.h"
+//#define B3_DEBUG_MEMORY_ALLOCATIONS 1
+#ifdef B3_DEBUG_MEMORY_ALLOCATIONS
+
+#define b3AlignedAlloc(a, b) \
+	b3AlignedAllocInternal(a, b, __LINE__, __FILE__)
+
+#define b3AlignedFree(ptr) \
+	b3AlignedFreeInternal(ptr, __LINE__, __FILE__)
+
+void* b3AlignedAllocInternal(size_t size, int alignment, int line, char* filename);
+
+void b3AlignedFreeInternal(void* ptr, int line, char* filename);
+
+#else
+void* b3AlignedAllocInternal(size_t size, int alignment);
+void b3AlignedFreeInternal(void* ptr);
+
+#define b3AlignedAlloc(size, alignment) b3AlignedAllocInternal(size, alignment)
+#define b3AlignedFree(ptr) b3AlignedFreeInternal(ptr)
+
+#endif
+typedef int btSizeType;
+
+typedef void*(b3AlignedAllocFunc)(size_t size, int alignment);
+typedef void(b3AlignedFreeFunc)(void* memblock);
+typedef void*(b3AllocFunc)(size_t size);
+typedef void(b3FreeFunc)(void* memblock);
+
+///The developer can let all Bullet memory allocations go through a custom memory allocator, using b3AlignedAllocSetCustom
+void b3AlignedAllocSetCustom(b3AllocFunc* allocFunc, b3FreeFunc* freeFunc);
+///If the developer has already an custom aligned allocator, then b3AlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
+void b3AlignedAllocSetCustomAligned(b3AlignedAllocFunc* allocFunc, b3AlignedFreeFunc* freeFunc);
+
+///The b3AlignedAllocator is a portable class for aligned memory allocations.
+///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using b3AlignedAllocSetCustom and b3AlignedAllocSetCustomAligned.
+template <typename T, unsigned Alignment>
+class b3AlignedAllocator
+{
+	typedef b3AlignedAllocator<T, Alignment> self_type;
+
+public:
+	//just going down a list:
+	b3AlignedAllocator() {}
+	/*
+	b3AlignedAllocator( const self_type & ) {}
+	*/
+
+	template <typename Other>
+	b3AlignedAllocator(const b3AlignedAllocator<Other, Alignment>&)
+	{
+	}
+
+	typedef const T* const_pointer;
+	typedef const T& const_reference;
+	typedef T* pointer;
+	typedef T& reference;
+	typedef T value_type;
+
+	pointer address(reference ref) const { return &ref; }
+	const_pointer address(const_reference ref) const { return &ref; }
+	pointer allocate(btSizeType n, const_pointer* hint = 0)
+	{
+		(void)hint;
+		return reinterpret_cast<pointer>(b3AlignedAlloc(sizeof(value_type) * n, Alignment));
+	}
+	void construct(pointer ptr, const value_type& value) { new (ptr) value_type(value); }
+	void deallocate(pointer ptr)
+	{
+		b3AlignedFree(reinterpret_cast<void*>(ptr));
+	}
+	void destroy(pointer ptr) { ptr->~value_type(); }
+
+	template <typename O>
+	struct rebind
+	{
+		typedef b3AlignedAllocator<O, Alignment> other;
+	};
+	template <typename O>
+	self_type& operator=(const b3AlignedAllocator<O, Alignment>&)
+	{
+		return *this;
+	}
+
+	friend bool operator==(const self_type&, const self_type&) { return true; }
+};
+
+#endif  //B3_ALIGNED_ALLOCATOR

thirdparty/bullet/src/Bullet3Common/b3AlignedObjectArray.h

@@ -0,0 +1,522 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_OBJECT_ARRAY__
+#define B3_OBJECT_ARRAY__
+
+#include "b3Scalar.h"  // has definitions like B3_FORCE_INLINE
+#include "b3AlignedAllocator.h"
+
+///If the platform doesn't support placement new, you can disable B3_USE_PLACEMENT_NEW
+///then the b3AlignedObjectArray doesn't support objects with virtual methods, and non-trivial constructors/destructors
+///You can enable B3_USE_MEMCPY, then swapping elements in the array will use memcpy instead of operator=
+///see discussion here: http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1231 and
+///http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=1240
+
+#define B3_USE_PLACEMENT_NEW 1
+//#define B3_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
+#define B3_ALLOW_ARRAY_COPY_OPERATOR  // enabling this can accidently perform deep copies of data if you are not careful
+
+#ifdef B3_USE_MEMCPY
+#include <memory.h>
+#include <string.h>
+#endif  //B3_USE_MEMCPY
+
+#ifdef B3_USE_PLACEMENT_NEW
+#include <new>  //for placement new
+#endif          //B3_USE_PLACEMENT_NEW
+
+///The b3AlignedObjectArray template class uses a subset of the stl::vector interface for its methods
+///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
+template <typename T>
+//template <class T>
+class b3AlignedObjectArray
+{
+	b3AlignedAllocator<T, 16> m_allocator;
+
+	int m_size;
+	int m_capacity;
+	T* m_data;
+	//PCK: added this line
+	bool m_ownsMemory;
+
+#ifdef B3_ALLOW_ARRAY_COPY_OPERATOR
+public:
+	B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T>& other)
+	{
+		copyFromArray(other);
+		return *this;
+	}
+#else   //B3_ALLOW_ARRAY_COPY_OPERATOR
+private:
+	B3_FORCE_INLINE b3AlignedObjectArray<T>& operator=(const b3AlignedObjectArray<T>& other);
+#endif  //B3_ALLOW_ARRAY_COPY_OPERATOR
+
+protected:
+	B3_FORCE_INLINE int allocSize(int size)
+	{
+		return (size ? size * 2 : 1);
+	}
+	B3_FORCE_INLINE void copy(int start, int end, T* dest) const
+	{
+		int i;
+		for (i = start; i < end; ++i)
+#ifdef B3_USE_PLACEMENT_NEW
+			new (&dest[i]) T(m_data[i]);
+#else
+			dest[i] = m_data[i];
+#endif  //B3_USE_PLACEMENT_NEW
+	}
+
+	B3_FORCE_INLINE void init()
+	{
+		//PCK: added this line
+		m_ownsMemory = true;
+		m_data = 0;
+		m_size = 0;
+		m_capacity = 0;
+	}
+	B3_FORCE_INLINE void destroy(int first, int last)
+	{
+		int i;
+		for (i = first; i < last; i++)
+		{
+			m_data[i].~T();
+		}
+	}
+
+	B3_FORCE_INLINE void* allocate(int size)
+	{
+		if (size)
+			return m_allocator.allocate(size);
+		return 0;
+	}
+
+	B3_FORCE_INLINE void deallocate()
+	{
+		if (m_data)
+		{
+			//PCK: enclosed the deallocation in this block
+			if (m_ownsMemory)
+			{
+				m_allocator.deallocate(m_data);
+			}
+			m_data = 0;
+		}
+	}
+
+public:
+	b3AlignedObjectArray()
+	{
+		init();
+	}
+
+	~b3AlignedObjectArray()
+	{
+		clear();
+	}
+
+	///Generally it is best to avoid using the copy constructor of an b3AlignedObjectArray, and use a (const) reference to the array instead.
+	b3AlignedObjectArray(const b3AlignedObjectArray& otherArray)
+	{
+		init();
+
+		int otherSize = otherArray.size();
+		resize(otherSize);
+		otherArray.copy(0, otherSize, m_data);
+	}
+
+	/// return the number of elements in the array
+	B3_FORCE_INLINE int size() const
+	{
+		return m_size;
+	}
+
+	B3_FORCE_INLINE const T& at(int n) const
+	{
+		b3Assert(n >= 0);
+		b3Assert(n < size());
+		return m_data[n];
+	}
+
+	B3_FORCE_INLINE T& at(int n)
+	{
+		b3Assert(n >= 0);
+		b3Assert(n < size());
+		return m_data[n];
+	}
+
+	B3_FORCE_INLINE const T& operator[](int n) const
+	{
+		b3Assert(n >= 0);
+		b3Assert(n < size());
+		return m_data[n];
+	}
+
+	B3_FORCE_INLINE T& operator[](int n)
+	{
+		b3Assert(n >= 0);
+		b3Assert(n < size());
+		return m_data[n];
+	}
+
+	///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
+	B3_FORCE_INLINE void clear()
+	{
+		destroy(0, size());
+
+		deallocate();
+
+		init();
+	}
+
+	B3_FORCE_INLINE void pop_back()
+	{
+		b3Assert(m_size > 0);
+		m_size--;
+		m_data[m_size].~T();
+	}
+
+	///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
+	///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
+	B3_FORCE_INLINE void resizeNoInitialize(int newsize)
+	{
+		int curSize = size();
+
+		if (newsize < curSize)
+		{
+		}
+		else
+		{
+			if (newsize > size())
+			{
+				reserve(newsize);
+			}
+			//leave this uninitialized
+		}
+		m_size = newsize;
+	}
+
+	B3_FORCE_INLINE void resize(int newsize, const T& fillData = T())
+	{
+		int curSize = size();
+
+		if (newsize < curSize)
+		{
+			for (int i = newsize; i < curSize; i++)
+			{
+				m_data[i].~T();
+			}
+		}
+		else
+		{
+			if (newsize > size())
+			{
+				reserve(newsize);
+			}
+#ifdef B3_USE_PLACEMENT_NEW
+			for (int i = curSize; i < newsize; i++)
+			{
+				new (&m_data[i]) T(fillData);
+			}
+#endif  //B3_USE_PLACEMENT_NEW
+		}
+
+		m_size = newsize;
+	}
+	B3_FORCE_INLINE T& expandNonInitializing()
+	{
+		int sz = size();
+		if (sz == capacity())
+		{
+			reserve(allocSize(size()));
+		}
+		m_size++;
+
+		return m_data[sz];
+	}
+
+	B3_FORCE_INLINE T& expand(const T& fillValue = T())
+	{
+		int sz = size();
+		if (sz == capacity())
+		{
+			reserve(allocSize(size()));
+		}
+		m_size++;
+#ifdef B3_USE_PLACEMENT_NEW
+		new (&m_data[sz]) T(fillValue);  //use the in-place new (not really allocating heap memory)
+#endif
+
+		return m_data[sz];
+	}
+
+	B3_FORCE_INLINE void push_back(const T& _Val)
+	{
+		int sz = size();
+		if (sz == capacity())
+		{
+			reserve(allocSize(size()));
+		}
+
+#ifdef B3_USE_PLACEMENT_NEW
+		new (&m_data[m_size]) T(_Val);
+#else
+		m_data[size()] = _Val;
+#endif  //B3_USE_PLACEMENT_NEW
+
+		m_size++;
+	}
+
+	/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
+	B3_FORCE_INLINE int capacity() const
+	{
+		return m_capacity;
+	}
+
+	B3_FORCE_INLINE void reserve(int _Count)
+	{  // determine new minimum length of allocated storage
+		if (capacity() < _Count)
+		{  // not enough room, reallocate
+			T* s = (T*)allocate(_Count);
+			b3Assert(s);
+			if (s == 0)
+			{
+				b3Error("b3AlignedObjectArray reserve out-of-memory\n");
+				_Count = 0;
+				m_size = 0;
+			}
+			copy(0, size(), s);
+
+			destroy(0, size());
+
+			deallocate();
+
+			//PCK: added this line
+			m_ownsMemory = true;
+
+			m_data = s;
+
+			m_capacity = _Count;
+		}
+	}
+
+	class less
+	{
+	public:
+		bool operator()(const T& a, const T& b)
+		{
+			return (a < b);
+		}
+	};
+
+	template <typename L>
+	void quickSortInternal(const L& CompareFunc, int lo, int hi)
+	{
+		//  lo is the lower index, hi is the upper index
+		//  of the region of array a that is to be sorted
+		int i = lo, j = hi;
+		T x = m_data[(lo + hi) / 2];
+
+		//  partition
+		do
+		{
+			while (CompareFunc(m_data[i], x))
+				i++;
+			while (CompareFunc(x, m_data[j]))
+				j--;
+			if (i <= j)
+			{
+				swap(i, j);
+				i++;
+				j--;
+			}
+		} while (i <= j);
+
+		//  recursion
+		if (lo < j)
+			quickSortInternal(CompareFunc, lo, j);
+		if (i < hi)
+			quickSortInternal(CompareFunc, i, hi);
+	}
+
+	template <typename L>
+	void quickSort(const L& CompareFunc)
+	{
+		//don't sort 0 or 1 elements
+		if (size() > 1)
+		{
+			quickSortInternal(CompareFunc, 0, size() - 1);
+		}
+	}
+
+	///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
+	template <typename L>
+	void downHeap(T* pArr, int k, int n, const L& CompareFunc)
+	{
+		/*  PRE: a[k+1..N] is a heap */
+		/* POST:  a[k..N]  is a heap */
+
+		T temp = pArr[k - 1];
+		/* k has child(s) */
+		while (k <= n / 2)
+		{
+			int child = 2 * k;
+
+			if ((child < n) && CompareFunc(pArr[child - 1], pArr[child]))
+			{
+				child++;
+			}
+			/* pick larger child */
+			if (CompareFunc(temp, pArr[child - 1]))
+			{
+				/* move child up */
+				pArr[k - 1] = pArr[child - 1];
+				k = child;
+			}
+			else
+			{
+				break;
+			}
+		}
+		pArr[k - 1] = temp;
+	} /*downHeap*/
+
+	void swap(int index0, int index1)
+	{
+#ifdef B3_USE_MEMCPY
+		char temp[sizeof(T)];
+		memcpy(temp, &m_data[index0], sizeof(T));
+		memcpy(&m_data[index0], &m_data[index1], sizeof(T));
+		memcpy(&m_data[index1], temp, sizeof(T));
+#else
+		T temp = m_data[index0];
+		m_data[index0] = m_data[index1];
+		m_data[index1] = temp;
+#endif  //B3_USE_PLACEMENT_NEW
+	}
+
+	template <typename L>
+	void heapSort(const L& CompareFunc)
+	{
+		/* sort a[0..N-1],  N.B. 0 to N-1 */
+		int k;
+		int n = m_size;
+		for (k = n / 2; k > 0; k--)
+		{
+			downHeap(m_data, k, n, CompareFunc);
+		}
+
+		/* a[1..N] is now a heap */
+		while (n >= 1)
+		{
+			swap(0, n - 1); /* largest of a[0..n-1] */
+
+			n = n - 1;
+			/* restore a[1..i-1] heap */
+			downHeap(m_data, 1, n, CompareFunc);
+		}
+	}
+
+	///non-recursive binary search, assumes sorted array
+	int findBinarySearch(const T& key) const
+	{
+		int first = 0;
+		int last = size() - 1;
+
+		//assume sorted array
+		while (first <= last)
+		{
+			int mid = (first + last) / 2;  // compute mid point.
+			if (key > m_data[mid])
+				first = mid + 1;  // repeat search in top half.
+			else if (key < m_data[mid])
+				last = mid - 1;  // repeat search in bottom half.
+			else
+				return mid;  // found it. return position /////
+		}
+		return size();  // failed to find key
+	}
+
+	int findLinearSearch(const T& key) const
+	{
+		int index = size();
+		int i;
+
+		for (i = 0; i < size(); i++)
+		{
+			if (m_data[i] == key)
+			{
+				index = i;
+				break;
+			}
+		}
+		return index;
+	}
+
+	int findLinearSearch2(const T& key) const
+	{
+		int index = -1;
+		int i;
+
+		for (i = 0; i < size(); i++)
+		{
+			if (m_data[i] == key)
+			{
+				index = i;
+				break;
+			}
+		}
+		return index;
+	}
+
+	void remove(const T& key)
+	{
+		int findIndex = findLinearSearch(key);
+		if (findIndex < size())
+		{
+			swap(findIndex, size() - 1);
+			pop_back();
+		}
+	}
+
+	//PCK: whole function
+	void initializeFromBuffer(void* buffer, int size, int capacity)
+	{
+		clear();
+		m_ownsMemory = false;
+		m_data = (T*)buffer;
+		m_size = size;
+		m_capacity = capacity;
+	}
+
+	void copyFromArray(const b3AlignedObjectArray& otherArray)
+	{
+		int otherSize = otherArray.size();
+		resize(otherSize);
+		otherArray.copy(0, otherSize, m_data);
+	}
+
+	void removeAtIndex(int index)
+	{
+		if (index < size())
+		{
+			swap(index, size() - 1);
+			pop_back();
+		}
+	}
+};
+
+#endif  //B3_OBJECT_ARRAY__

thirdparty/bullet/src/Bullet3Common/b3CommandLineArgs.h

@@ -0,0 +1,106 @@
+#ifndef COMMAND_LINE_ARGS_H
+#define COMMAND_LINE_ARGS_H
+
+/******************************************************************************
+ * Command-line parsing
+ ******************************************************************************/
+#include <map>
+#include <algorithm>
+#include <string>
+#include <cstring>
+#include <sstream>
+class b3CommandLineArgs
+{
+protected:
+	std::map<std::string, std::string> pairs;
+
+public:
+	// Constructor
+	b3CommandLineArgs(int argc, char **argv)
+	{
+		addArgs(argc, argv);
+	}
+
+	void addArgs(int argc, char **argv)
+	{
+		for (int i = 1; i < argc; i++)
+		{
+			std::string arg = argv[i];
+
+			if ((arg.length() < 2) || (arg[0] != '-') || (arg[1] != '-'))
+			{
+				continue;
+			}
+
+			std::string::size_type pos;
+			std::string key, val;
+			if ((pos = arg.find('=')) == std::string::npos)
+			{
+				key = std::string(arg, 2, arg.length() - 2);
+				val = "";
+			}
+			else
+			{
+				key = std::string(arg, 2, pos - 2);
+				val = std::string(arg, pos + 1, arg.length() - 1);
+			}
+
+			//only add new keys, don't replace existing
+			if (pairs.find(key) == pairs.end())
+			{
+				pairs[key] = val;
+			}
+		}
+	}
+
+	bool CheckCmdLineFlag(const char *arg_name)
+	{
+		std::map<std::string, std::string>::iterator itr;
+		if ((itr = pairs.find(arg_name)) != pairs.end())
+		{
+			return true;
+		}
+		return false;
+	}
+
+	template <typename T>
+	bool GetCmdLineArgument(const char *arg_name, T &val);
+
+	int ParsedArgc()
+	{
+		return pairs.size();
+	}
+};
+
+template <typename T>
+inline bool b3CommandLineArgs::GetCmdLineArgument(const char *arg_name, T &val)
+{
+	std::map<std::string, std::string>::iterator itr;
+	if ((itr = pairs.find(arg_name)) != pairs.end())
+	{
+		std::istringstream strstream(itr->second);
+		strstream >> val;
+		return true;
+	}
+	return false;
+}
+
+template <>
+inline bool b3CommandLineArgs::GetCmdLineArgument<char *>(const char *arg_name, char *&val)
+{
+	std::map<std::string, std::string>::iterator itr;
+	if ((itr = pairs.find(arg_name)) != pairs.end())
+	{
+		std::string s = itr->second;
+		val = (char *)malloc(sizeof(char) * (s.length() + 1));
+		std::strcpy(val, s.c_str());
+		return true;
+	}
+	else
+	{
+		val = NULL;
+	}
+	return false;
+}
+
+#endif  //COMMAND_LINE_ARGS_H

thirdparty/bullet/src/Bullet3Common/b3FileUtils.h

@@ -0,0 +1,133 @@
+#ifndef B3_FILE_UTILS_H
+#define B3_FILE_UTILS_H
+
+#include <stdio.h>
+#include "b3Scalar.h"
+#include <stddef.h>  //ptrdiff_h
+#include <string.h>
+
+struct b3FileUtils
+{
+	b3FileUtils()
+	{
+	}
+	virtual ~b3FileUtils()
+	{
+	}
+
+	static bool findFile(const char* orgFileName, char* relativeFileName, int maxRelativeFileNameMaxLen)
+	{
+		FILE* f = 0;
+		f = fopen(orgFileName, "rb");
+		if (f)
+		{
+			//printf("original file found: [%s]\n", orgFileName);
+			sprintf(relativeFileName, "%s", orgFileName);
+			fclose(f);
+			return true;
+		}
+
+		//printf("Trying various directories, relative to current working directory\n");
+		const char* prefix[] = {"./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/"};
+		int numPrefixes = sizeof(prefix) / sizeof(const char*);
+
+		f = 0;
+		bool fileFound = false;
+
+		for (int i = 0; !f && i < numPrefixes; i++)
+		{
+#ifdef _MSC_VER
+			sprintf_s(relativeFileName, maxRelativeFileNameMaxLen, "%s%s", prefix[i], orgFileName);
+#else
+			sprintf(relativeFileName, "%s%s", prefix[i], orgFileName);
+#endif
+			f = fopen(relativeFileName, "rb");
+			if (f)
+			{
+				fileFound = true;
+				break;
+			}
+		}
+		if (f)
+		{
+			fclose(f);
+		}
+
+		return fileFound;
+	}
+
+	static const char* strip2(const char* name, const char* pattern)
+	{
+		size_t const patlen = strlen(pattern);
+		size_t patcnt = 0;
+		const char* oriptr;
+		const char* patloc;
+		// find how many times the pattern occurs in the original string
+		for (oriptr = name; (patloc = strstr(oriptr, pattern)); oriptr = patloc + patlen)
+		{
+			patcnt++;
+		}
+		return oriptr;
+	}
+
+	static int extractPath(const char* fileName, char* path, int maxPathLength)
+	{
+		const char* stripped = strip2(fileName, "/");
+		stripped = strip2(stripped, "\\");
+
+		ptrdiff_t len = stripped - fileName;
+		b3Assert((len + 1) < maxPathLength);
+
+		if (len && ((len + 1) < maxPathLength))
+		{
+			for (int i = 0; i < len; i++)
+			{
+				path[i] = fileName[i];
+			}
+			path[len] = 0;
+		}
+		else
+		{
+			len = 0;
+			b3Assert(maxPathLength > 0);
+			if (maxPathLength > 0)
+			{
+				path[len] = 0;
+			}
+		}
+		return len;
+	}
+
+	static char toLowerChar(const char t)
+	{
+		if (t >= (char)'A' && t <= (char)'Z')
+			return t + ((char)'a' - (char)'A');
+		else
+			return t;
+	}
+
+	static void toLower(char* str)
+	{
+		int len = strlen(str);
+		for (int i = 0; i < len; i++)
+		{
+			str[i] = toLowerChar(str[i]);
+		}
+	}
+
+	/*static const char* strip2(const char* name, const char* pattern)
+	{
+		size_t const patlen = strlen(pattern);
+		size_t patcnt = 0;
+		const char * oriptr;
+		const char * patloc;
+		// find how many times the pattern occurs in the original string
+		for (oriptr = name; patloc = strstr(oriptr, pattern); oriptr = patloc + patlen)
+		{
+			patcnt++;
+		}
+		return oriptr;
+	}
+	*/
+};
+#endif  //B3_FILE_UTILS_H

thirdparty/bullet/src/Bullet3Common/b3HashMap.h

@@ -0,0 +1,462 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2013 Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_HASH_MAP_H
+#define B3_HASH_MAP_H
+
+#include "b3AlignedObjectArray.h"
+
+#include <string>
+
+///very basic hashable string implementation, compatible with b3HashMap
+struct b3HashString
+{
+	std::string m_string;
+	unsigned int m_hash;
+
+	B3_FORCE_INLINE unsigned int getHash() const
+	{
+		return m_hash;
+	}
+
+	b3HashString(const char* name)
+		: m_string(name)
+	{
+		/* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
+		static const unsigned int InitialFNV = 2166136261u;
+		static const unsigned int FNVMultiple = 16777619u;
+
+		/* Fowler / Noll / Vo (FNV) Hash */
+		unsigned int hash = InitialFNV;
+		int len = m_string.length();
+		for (int i = 0; i < len; i++)
+		{
+			hash = hash ^ (m_string[i]); /* xor  the low 8 bits */
+			hash = hash * FNVMultiple;   /* multiply by the magic number */
+		}
+		m_hash = hash;
+	}
+
+	int portableStringCompare(const char* src, const char* dst) const
+	{
+		int ret = 0;
+
+		while (!(ret = *(unsigned char*)src - *(unsigned char*)dst) && *dst)
+			++src, ++dst;
+
+		if (ret < 0)
+			ret = -1;
+		else if (ret > 0)
+			ret = 1;
+
+		return (ret);
+	}
+
+	bool equals(const b3HashString& other) const
+	{
+		return (m_string == other.m_string);
+	}
+};
+
+const int B3_HASH_NULL = 0xffffffff;
+
+class b3HashInt
+{
+	int m_uid;
+
+public:
+	b3HashInt(int uid) : m_uid(uid)
+	{
+	}
+
+	int getUid1() const
+	{
+		return m_uid;
+	}
+
+	void setUid1(int uid)
+	{
+		m_uid = uid;
+	}
+
+	bool equals(const b3HashInt& other) const
+	{
+		return getUid1() == other.getUid1();
+	}
+	//to our success
+	B3_FORCE_INLINE unsigned int getHash() const
+	{
+		int key = m_uid;
+		// Thomas Wang's hash
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
+		return key;
+	}
+};
+
+class b3HashPtr
+{
+	union {
+		const void* m_pointer;
+		int m_hashValues[2];
+	};
+
+public:
+	b3HashPtr(const void* ptr)
+		: m_pointer(ptr)
+	{
+	}
+
+	const void* getPointer() const
+	{
+		return m_pointer;
+	}
+
+	bool equals(const b3HashPtr& other) const
+	{
+		return getPointer() == other.getPointer();
+	}
+
+	//to our success
+	B3_FORCE_INLINE unsigned int getHash() const
+	{
+		const bool VOID_IS_8 = ((sizeof(void*) == 8));
+
+		int key = VOID_IS_8 ? m_hashValues[0] + m_hashValues[1] : m_hashValues[0];
+
+		// Thomas Wang's hash
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
+		return key;
+	}
+};
+
+template <class Value>
+class b3HashKeyPtr
+{
+	int m_uid;
+
+public:
+	b3HashKeyPtr(int uid) : m_uid(uid)
+	{
+	}
+
+	int getUid1() const
+	{
+		return m_uid;
+	}
+
+	bool equals(const b3HashKeyPtr<Value>& other) const
+	{
+		return getUid1() == other.getUid1();
+	}
+
+	//to our success
+	B3_FORCE_INLINE unsigned int getHash() const
+	{
+		int key = m_uid;
+		// Thomas Wang's hash
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
+		return key;
+	}
+};
+
+template <class Value>
+class b3HashKey
+{
+	int m_uid;
+
+public:
+	b3HashKey(int uid) : m_uid(uid)
+	{
+	}
+
+	int getUid1() const
+	{
+		return m_uid;
+	}
+
+	bool equals(const b3HashKey<Value>& other) const
+	{
+		return getUid1() == other.getUid1();
+	}
+	//to our success
+	B3_FORCE_INLINE unsigned int getHash() const
+	{
+		int key = m_uid;
+		// Thomas Wang's hash
+		key += ~(key << 15);
+		key ^= (key >> 10);
+		key += (key << 3);
+		key ^= (key >> 6);
+		key += ~(key << 11);
+		key ^= (key >> 16);
+		return key;
+	}
+};
+
+///The b3HashMap template class implements a generic and lightweight hashmap.
+///A basic sample of how to use b3HashMap is located in Demos\BasicDemo\main.cpp
+template <class Key, class Value>
+class b3HashMap
+{
+protected:
+	b3AlignedObjectArray<int> m_hashTable;
+	b3AlignedObjectArray<int> m_next;
+
+	b3AlignedObjectArray<Value> m_valueArray;
+	b3AlignedObjectArray<Key> m_keyArray;
+
+	void growTables(const Key& /*key*/)
+	{
+		int newCapacity = m_valueArray.capacity();
+
+		if (m_hashTable.size() < newCapacity)
+		{
+			//grow hashtable and next table
+			int curHashtableSize = m_hashTable.size();
+
+			m_hashTable.resize(newCapacity);
+			m_next.resize(newCapacity);
+
+			int i;
+
+			for (i = 0; i < newCapacity; ++i)
+			{
+				m_hashTable[i] = B3_HASH_NULL;
+			}
+			for (i = 0; i < newCapacity; ++i)
+			{
+				m_next[i] = B3_HASH_NULL;
+			}
+
+			for (i = 0; i < curHashtableSize; i++)
+			{
+				//const Value& value = m_valueArray[i];
+				//const Key& key = m_keyArray[i];
+
+				int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity() - 1);  // New hash value with new mask
+				m_next[i] = m_hashTable[hashValue];
+				m_hashTable[hashValue] = i;
+			}
+		}
+	}
+
+public:
+	void insert(const Key& key, const Value& value)
+	{
+		int hash = key.getHash() & (m_valueArray.capacity() - 1);
+
+		//replace value if the key is already there
+		int index = findIndex(key);
+		if (index != B3_HASH_NULL)
+		{
+			m_valueArray[index] = value;
+			return;
+		}
+
+		int count = m_valueArray.size();
+		int oldCapacity = m_valueArray.capacity();
+		m_valueArray.push_back(value);
+		m_keyArray.push_back(key);
+
+		int newCapacity = m_valueArray.capacity();
+		if (oldCapacity < newCapacity)
+		{
+			growTables(key);
+			//hash with new capacity
+			hash = key.getHash() & (m_valueArray.capacity() - 1);
+		}
+		m_next[count] = m_hashTable[hash];
+		m_hashTable[hash] = count;
+	}
+
+	void remove(const Key& key)
+	{
+		int hash = key.getHash() & (m_valueArray.capacity() - 1);
+
+		int pairIndex = findIndex(key);
+
+		if (pairIndex == B3_HASH_NULL)
+		{
+			return;
+		}
+
+		// Remove the pair from the hash table.
+		int index = m_hashTable[hash];
+		b3Assert(index != B3_HASH_NULL);
+
+		int previous = B3_HASH_NULL;
+		while (index != pairIndex)
+		{
+			previous = index;
+			index = m_next[index];
+		}
+
+		if (previous != B3_HASH_NULL)
+		{
+			b3Assert(m_next[previous] == pairIndex);
+			m_next[previous] = m_next[pairIndex];
+		}
+		else
+		{
+			m_hashTable[hash] = m_next[pairIndex];
+		}
+
+		// We now move the last pair into spot of the
+		// pair being removed. We need to fix the hash
+		// table indices to support the move.
+
+		int lastPairIndex = m_valueArray.size() - 1;
+
+		// If the removed pair is the last pair, we are done.
+		if (lastPairIndex == pairIndex)
+		{
+			m_valueArray.pop_back();
+			m_keyArray.pop_back();
+			return;
+		}
+
+		// Remove the last pair from the hash table.
+		int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity() - 1);
+
+		index = m_hashTable[lastHash];
+		b3Assert(index != B3_HASH_NULL);
+
+		previous = B3_HASH_NULL;
+		while (index != lastPairIndex)
+		{
+			previous = index;
+			index = m_next[index];
+		}
+
+		if (previous != B3_HASH_NULL)
+		{
+			b3Assert(m_next[previous] == lastPairIndex);
+			m_next[previous] = m_next[lastPairIndex];
+		}
+		else
+		{
+			m_hashTable[lastHash] = m_next[lastPairIndex];
+		}
+
+		// Copy the last pair into the remove pair's spot.
+		m_valueArray[pairIndex] = m_valueArray[lastPairIndex];
+		m_keyArray[pairIndex] = m_keyArray[lastPairIndex];
+
+		// Insert the last pair into the hash table
+		m_next[pairIndex] = m_hashTable[lastHash];
+		m_hashTable[lastHash] = pairIndex;
+
+		m_valueArray.pop_back();
+		m_keyArray.pop_back();
+	}
+
+	int size() const
+	{
+		return m_valueArray.size();
+	}
+
+	const Value* getAtIndex(int index) const
+	{
+		b3Assert(index < m_valueArray.size());
+
+		return &m_valueArray[index];
+	}
+
+	Value* getAtIndex(int index)
+	{
+		b3Assert(index < m_valueArray.size());
+
+		return &m_valueArray[index];
+	}
+
+	Key getKeyAtIndex(int index)
+	{
+		b3Assert(index < m_keyArray.size());
+		return m_keyArray[index];
+	}
+
+	const Key getKeyAtIndex(int index) const
+	{
+		b3Assert(index < m_keyArray.size());
+		return m_keyArray[index];
+	}
+
+	Value* operator[](const Key& key)
+	{
+		return find(key);
+	}
+
+	const Value* find(const Key& key) const
+	{
+		int index = findIndex(key);
+		if (index == B3_HASH_NULL)
+		{
+			return NULL;
+		}
+		return &m_valueArray[index];
+	}
+
+	Value* find(const Key& key)
+	{
+		int index = findIndex(key);
+		if (index == B3_HASH_NULL)
+		{
+			return NULL;
+		}
+		return &m_valueArray[index];
+	}
+
+	int findIndex(const Key& key) const
+	{
+		unsigned int hash = key.getHash() & (m_valueArray.capacity() - 1);
+
+		if (hash >= (unsigned int)m_hashTable.size())
+		{
+			return B3_HASH_NULL;
+		}
+
+		int index = m_hashTable[hash];
+		while ((index != B3_HASH_NULL) && key.equals(m_keyArray[index]) == false)
+		{
+			index = m_next[index];
+		}
+		return index;
+	}
+
+	void clear()
+	{
+		m_hashTable.clear();
+		m_next.clear();
+		m_valueArray.clear();
+		m_keyArray.clear();
+	}
+};
+
+#endif  //B3_HASH_MAP_H

thirdparty/bullet/src/Bullet3Common/b3Logging.cpp

@@ -0,0 +1,145 @@
+/*
+Copyright (c) 2013 Advanced Micro Devices, Inc.  
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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 Erwin Coumans
+
+#include "b3Logging.h"
+
+#include <stdio.h>
+#include <stdarg.h>
+
+#ifdef _WIN32
+#include <windows.h>
+#endif  //_WIN32
+
+void b3PrintfFuncDefault(const char* msg)
+{
+#ifdef _WIN32
+	OutputDebugStringA(msg);
+#endif
+	printf("%s", msg);
+	//is this portable?
+	fflush(stdout);
+}
+
+void b3WarningMessageFuncDefault(const char* msg)
+{
+#ifdef _WIN32
+	OutputDebugStringA(msg);
+#endif
+	printf("%s", msg);
+	//is this portable?
+	fflush(stdout);
+}
+
+void b3ErrorMessageFuncDefault(const char* msg)
+{
+#ifdef _WIN32
+	OutputDebugStringA(msg);
+#endif
+	printf("%s", msg);
+
+	//is this portable?
+	fflush(stdout);
+}
+
+static b3PrintfFunc* b3s_printfFunc = b3PrintfFuncDefault;
+static b3WarningMessageFunc* b3s_warningMessageFunc = b3WarningMessageFuncDefault;
+static b3ErrorMessageFunc* b3s_errorMessageFunc = b3ErrorMessageFuncDefault;
+
+///The developer can route b3Printf output using their own implementation
+void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc)
+{
+	b3s_printfFunc = printfFunc;
+}
+void b3SetCustomWarningMessageFunc(b3PrintfFunc* warningMessageFunc)
+{
+	b3s_warningMessageFunc = warningMessageFunc;
+}
+void b3SetCustomErrorMessageFunc(b3PrintfFunc* errorMessageFunc)
+{
+	b3s_errorMessageFunc = errorMessageFunc;
+}
+
+//#define B3_MAX_DEBUG_STRING_LENGTH 2048
+#define B3_MAX_DEBUG_STRING_LENGTH 32768
+
+void b3OutputPrintfVarArgsInternal(const char* str, ...)
+{
+	char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
+	va_list argList;
+	va_start(argList, str);
+#ifdef _MSC_VER
+	vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
+#else
+	vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
+#endif
+	(b3s_printfFunc)(strDebug);
+	va_end(argList);
+}
+void b3OutputWarningMessageVarArgsInternal(const char* str, ...)
+{
+	char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
+	va_list argList;
+	va_start(argList, str);
+#ifdef _MSC_VER
+	vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
+#else
+	vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
+#endif
+	(b3s_warningMessageFunc)(strDebug);
+	va_end(argList);
+}
+void b3OutputErrorMessageVarArgsInternal(const char* str, ...)
+{
+	char strDebug[B3_MAX_DEBUG_STRING_LENGTH] = {0};
+	va_list argList;
+	va_start(argList, str);
+#ifdef _MSC_VER
+	vsprintf_s(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
+#else
+	vsnprintf(strDebug, B3_MAX_DEBUG_STRING_LENGTH, str, argList);
+#endif
+	(b3s_errorMessageFunc)(strDebug);
+	va_end(argList);
+}
+
+void b3EnterProfileZoneDefault(const char* name)
+{
+}
+void b3LeaveProfileZoneDefault()
+{
+}
+static b3EnterProfileZoneFunc* b3s_enterFunc = b3EnterProfileZoneDefault;
+static b3LeaveProfileZoneFunc* b3s_leaveFunc = b3LeaveProfileZoneDefault;
+void b3EnterProfileZone(const char* name)
+{
+	(b3s_enterFunc)(name);
+}
+void b3LeaveProfileZone()
+{
+	(b3s_leaveFunc)();
+}
+
+void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc)
+{
+	b3s_enterFunc = enterFunc;
+}
+void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc)
+{
+	b3s_leaveFunc = leaveFunc;
+}
+
+#ifndef _MSC_VER
+#undef vsprintf_s
+#endif

thirdparty/bullet/src/Bullet3Common/b3Logging.h

@@ -0,0 +1,74 @@
+
+#ifndef B3_LOGGING_H
+#define B3_LOGGING_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+///We add the do/while so that the statement "if (condition) b3Printf("test"); else {...}" would fail
+///You can also customize the message by uncommenting out a different line below
+#define b3Printf(...) b3OutputPrintfVarArgsInternal(__VA_ARGS__)
+	//#define b3Printf(...) do {b3OutputPrintfVarArgsInternal("b3Printf[%s,%d]:",__FILE__,__LINE__);b3OutputPrintfVarArgsInternal(__VA_ARGS__); } while(0)
+	//#define b3Printf b3OutputPrintfVarArgsInternal
+	//#define b3Printf(...) printf(__VA_ARGS__)
+	//#define b3Printf(...)
+#define b3Warning(...) do{	b3OutputWarningMessageVarArgsInternal("b3Warning[%s,%d]:\n", __FILE__, __LINE__);b3OutputWarningMessageVarArgsInternal(__VA_ARGS__);} while (0)
+#define b3Error(...)do	{b3OutputErrorMessageVarArgsInternal("b3Error[%s,%d]:\n", __FILE__, __LINE__);b3OutputErrorMessageVarArgsInternal(__VA_ARGS__);} while (0)
+#ifndef B3_NO_PROFILE
+
+	void b3EnterProfileZone(const char* name);
+	void b3LeaveProfileZone();
+#ifdef __cplusplus
+
+	class b3ProfileZone
+	{
+	public:
+		b3ProfileZone(const char* name)
+		{
+			b3EnterProfileZone(name);
+		}
+
+		~b3ProfileZone()
+		{
+			b3LeaveProfileZone();
+		}
+	};
+
+#define B3_PROFILE(name) b3ProfileZone __profile(name)
+#endif
+
+#else  //B3_NO_PROFILE
+
+#define B3_PROFILE(name)
+#define b3StartProfile(a)
+#define b3StopProfile
+
+#endif  //#ifndef B3_NO_PROFILE
+
+	typedef void(b3PrintfFunc)(const char* msg);
+	typedef void(b3WarningMessageFunc)(const char* msg);
+	typedef void(b3ErrorMessageFunc)(const char* msg);
+	typedef void(b3EnterProfileZoneFunc)(const char* msg);
+	typedef void(b3LeaveProfileZoneFunc)();
+
+	///The developer can route b3Printf output using their own implementation
+	void b3SetCustomPrintfFunc(b3PrintfFunc* printfFunc);
+	void b3SetCustomWarningMessageFunc(b3WarningMessageFunc* warningMsgFunc);
+	void b3SetCustomErrorMessageFunc(b3ErrorMessageFunc* errorMsgFunc);
+
+	///Set custom profile zone functions (zones can be nested)
+	void b3SetCustomEnterProfileZoneFunc(b3EnterProfileZoneFunc* enterFunc);
+	void b3SetCustomLeaveProfileZoneFunc(b3LeaveProfileZoneFunc* leaveFunc);
+
+	///Don't use those internal functions directly, use the b3Printf or b3SetCustomPrintfFunc instead (or warning/error version)
+	void b3OutputPrintfVarArgsInternal(const char* str, ...);
+	void b3OutputWarningMessageVarArgsInternal(const char* str, ...);
+	void b3OutputErrorMessageVarArgsInternal(const char* str, ...);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  //B3_LOGGING_H

thirdparty/bullet/src/Bullet3Common/b3MinMax.h

@@ -0,0 +1,69 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_GEN_MINMAX_H
+#define B3_GEN_MINMAX_H
+
+#include "b3Scalar.h"
+
+template <class T>
+B3_FORCE_INLINE const T& b3Min(const T& a, const T& b)
+{
+	return a < b ? a : b;
+}
+
+template <class T>
+B3_FORCE_INLINE const T& b3Max(const T& a, const T& b)
+{
+	return a > b ? a : b;
+}
+
+template <class T>
+B3_FORCE_INLINE const T& b3Clamped(const T& a, const T& lb, const T& ub)
+{
+	return a < lb ? lb : (ub < a ? ub : a);
+}
+
+template <class T>
+B3_FORCE_INLINE void b3SetMin(T& a, const T& b)
+{
+	if (b < a)
+	{
+		a = b;
+	}
+}
+
+template <class T>
+B3_FORCE_INLINE void b3SetMax(T& a, const T& b)
+{
+	if (a < b)
+	{
+		a = b;
+	}
+}
+
+template <class T>
+B3_FORCE_INLINE void b3Clamp(T& a, const T& lb, const T& ub)
+{
+	if (a < lb)
+	{
+		a = lb;
+	}
+	else if (ub < a)
+	{
+		a = ub;
+	}
+}
+
+#endif  //B3_GEN_MINMAX_H

thirdparty/bullet/src/Bullet3Common/b3PoolAllocator.h

@@ -0,0 +1,121 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef _BT_POOL_ALLOCATOR_H
+#define _BT_POOL_ALLOCATOR_H
+
+#include "b3Scalar.h"
+#include "b3AlignedAllocator.h"
+
+///The b3PoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
+class b3PoolAllocator
+{
+	int m_elemSize;
+	int m_maxElements;
+	int m_freeCount;
+	void* m_firstFree;
+	unsigned char* m_pool;
+
+public:
+	b3PoolAllocator(int elemSize, int maxElements)
+		: m_elemSize(elemSize),
+		  m_maxElements(maxElements)
+	{
+		m_pool = (unsigned char*)b3AlignedAlloc(static_cast<unsigned int>(m_elemSize * m_maxElements), 16);
+
+		unsigned char* p = m_pool;
+		m_firstFree = p;
+		m_freeCount = m_maxElements;
+		int count = m_maxElements;
+		while (--count)
+		{
+			*(void**)p = (p + m_elemSize);
+			p += m_elemSize;
+		}
+		*(void**)p = 0;
+	}
+
+	~b3PoolAllocator()
+	{
+		b3AlignedFree(m_pool);
+	}
+
+	int getFreeCount() const
+	{
+		return m_freeCount;
+	}
+
+	int getUsedCount() const
+	{
+		return m_maxElements - m_freeCount;
+	}
+
+	int getMaxCount() const
+	{
+		return m_maxElements;
+	}
+
+	void* allocate(int size)
+	{
+		// release mode fix
+		(void)size;
+		b3Assert(!size || size <= m_elemSize);
+		b3Assert(m_freeCount > 0);
+		void* result = m_firstFree;
+		m_firstFree = *(void**)m_firstFree;
+		--m_freeCount;
+		return result;
+	}
+
+	bool validPtr(void* ptr)
+	{
+		if (ptr)
+		{
+			if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
+			{
+				return true;
+			}
+		}
+		return false;
+	}
+
+	void freeMemory(void* ptr)
+	{
+		if (ptr)
+		{
+			b3Assert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
+
+			*(void**)ptr = m_firstFree;
+			m_firstFree = ptr;
+			++m_freeCount;
+		}
+	}
+
+	int getElementSize() const
+	{
+		return m_elemSize;
+	}
+
+	unsigned char* getPoolAddress()
+	{
+		return m_pool;
+	}
+
+	const unsigned char* getPoolAddress() const
+	{
+		return m_pool;
+	}
+};
+
+#endif  //_BT_POOL_ALLOCATOR_H

thirdparty/bullet/src/Bullet3Common/b3QuadWord.h

@@ -0,0 +1,242 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_SIMD_QUADWORD_H
+#define B3_SIMD_QUADWORD_H
+
+#include "b3Scalar.h"
+#include "b3MinMax.h"
+
+#if defined(__CELLOS_LV2) && defined(__SPU__)
+#include <altivec.h>
+#endif
+
+/**@brief The b3QuadWord class is base class for b3Vector3 and b3Quaternion. 
+ * Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
+ */
+#ifndef USE_LIBSPE2
+B3_ATTRIBUTE_ALIGNED16(class)
+b3QuadWord
+#else
+class b3QuadWord
+#endif
+{
+protected:
+#if defined(__SPU__) && defined(__CELLOS_LV2__)
+	union {
+		vec_float4 mVec128;
+		b3Scalar m_floats[4];
+	};
+
+public:
+	vec_float4 get128() const
+	{
+		return mVec128;
+	}
+
+#else  //__CELLOS_LV2__ __SPU__
+
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
+public:
+	union {
+		b3SimdFloat4 mVec128;
+		b3Scalar m_floats[4];
+		struct
+		{
+			b3Scalar x, y, z, w;
+		};
+	};
+
+public:
+	B3_FORCE_INLINE b3SimdFloat4 get128() const
+	{
+		return mVec128;
+	}
+	B3_FORCE_INLINE void set128(b3SimdFloat4 v128)
+	{
+		mVec128 = v128;
+	}
+#else
+public:
+	union {
+		b3Scalar m_floats[4];
+		struct
+		{
+			b3Scalar x, y, z, w;
+		};
+	};
+#endif  // B3_USE_SSE
+
+#endif  //__CELLOS_LV2__ __SPU__
+
+public:
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
+
+	// Set Vector
+	B3_FORCE_INLINE b3QuadWord(const b3SimdFloat4 vec)
+	{
+		mVec128 = vec;
+	}
+
+	// Copy constructor
+	B3_FORCE_INLINE b3QuadWord(const b3QuadWord& rhs)
+	{
+		mVec128 = rhs.mVec128;
+	}
+
+	// Assignment Operator
+	B3_FORCE_INLINE b3QuadWord&
+	operator=(const b3QuadWord& v)
+	{
+		mVec128 = v.mVec128;
+
+		return *this;
+	}
+
+#endif
+
+	/**@brief Return the x value */
+	B3_FORCE_INLINE const b3Scalar& getX() const { return m_floats[0]; }
+	/**@brief Return the y value */
+	B3_FORCE_INLINE const b3Scalar& getY() const { return m_floats[1]; }
+	/**@brief Return the z value */
+	B3_FORCE_INLINE const b3Scalar& getZ() const { return m_floats[2]; }
+	/**@brief Set the x value */
+	B3_FORCE_INLINE void setX(b3Scalar _x) { m_floats[0] = _x; };
+	/**@brief Set the y value */
+	B3_FORCE_INLINE void setY(b3Scalar _y) { m_floats[1] = _y; };
+	/**@brief Set the z value */
+	B3_FORCE_INLINE void setZ(b3Scalar _z) { m_floats[2] = _z; };
+	/**@brief Set the w value */
+	B3_FORCE_INLINE void setW(b3Scalar _w) { m_floats[3] = _w; };
+	/**@brief Return the x value */
+
+	//B3_FORCE_INLINE b3Scalar&       operator[](int i)       { return (&m_floats[0])[i];	}
+	//B3_FORCE_INLINE const b3Scalar& operator[](int i) const { return (&m_floats[0])[i]; }
+	///operator b3Scalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
+	B3_FORCE_INLINE operator b3Scalar*() { return &m_floats[0]; }
+	B3_FORCE_INLINE operator const b3Scalar*() const { return &m_floats[0]; }
+
+	B3_FORCE_INLINE bool operator==(const b3QuadWord& other) const
+	{
+#ifdef B3_USE_SSE
+		return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
+#else
+		return ((m_floats[3] == other.m_floats[3]) &&
+				(m_floats[2] == other.m_floats[2]) &&
+				(m_floats[1] == other.m_floats[1]) &&
+				(m_floats[0] == other.m_floats[0]));
+#endif
+	}
+
+	B3_FORCE_INLINE bool operator!=(const b3QuadWord& other) const
+	{
+		return !(*this == other);
+	}
+
+	/**@brief Set x,y,z and zero w 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   */
+	B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
+	{
+		m_floats[0] = _x;
+		m_floats[1] = _y;
+		m_floats[2] = _z;
+		m_floats[3] = 0.f;
+	}
+
+	/*		void getValue(b3Scalar *m) const 
+		{
+			m[0] = m_floats[0];
+			m[1] = m_floats[1];
+			m[2] = m_floats[2];
+		}
+*/
+	/**@brief Set the values 
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   * @param w Value of w
+   */
+	B3_FORCE_INLINE void setValue(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
+	{
+		m_floats[0] = _x;
+		m_floats[1] = _y;
+		m_floats[2] = _z;
+		m_floats[3] = _w;
+	}
+	/**@brief No initialization constructor */
+	B3_FORCE_INLINE b3QuadWord()
+	//	:m_floats[0](b3Scalar(0.)),m_floats[1](b3Scalar(0.)),m_floats[2](b3Scalar(0.)),m_floats[3](b3Scalar(0.))
+	{
+	}
+
+	/**@brief Three argument constructor (zeros w)
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   */
+	B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z)
+	{
+		m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
+	}
+
+	/**@brief Initializing constructor
+   * @param x Value of x
+   * @param y Value of y
+   * @param z Value of z
+   * @param w Value of w
+   */
+	B3_FORCE_INLINE b3QuadWord(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
+	{
+		m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
+	}
+
+	/**@brief Set each element to the max of the current values and the values of another b3QuadWord
+   * @param other The other b3QuadWord to compare with 
+   */
+	B3_FORCE_INLINE void setMax(const b3QuadWord& other)
+	{
+#ifdef B3_USE_SSE
+		mVec128 = _mm_max_ps(mVec128, other.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vmaxq_f32(mVec128, other.mVec128);
+#else
+		b3SetMax(m_floats[0], other.m_floats[0]);
+		b3SetMax(m_floats[1], other.m_floats[1]);
+		b3SetMax(m_floats[2], other.m_floats[2]);
+		b3SetMax(m_floats[3], other.m_floats[3]);
+#endif
+	}
+	/**@brief Set each element to the min of the current values and the values of another b3QuadWord
+   * @param other The other b3QuadWord to compare with 
+   */
+	B3_FORCE_INLINE void setMin(const b3QuadWord& other)
+	{
+#ifdef B3_USE_SSE
+		mVec128 = _mm_min_ps(mVec128, other.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vminq_f32(mVec128, other.mVec128);
+#else
+		b3SetMin(m_floats[0], other.m_floats[0]);
+		b3SetMin(m_floats[1], other.m_floats[1]);
+		b3SetMin(m_floats[2], other.m_floats[2]);
+		b3SetMin(m_floats[3], other.m_floats[3]);
+#endif
+	}
+};
+
+#endif  //B3_SIMD_QUADWORD_H

thirdparty/bullet/src/Bullet3Common/b3Quaternion.h

@@ -0,0 +1,908 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_SIMD__QUATERNION_H_
+#define B3_SIMD__QUATERNION_H_
+
+#include "b3Vector3.h"
+#include "b3QuadWord.h"
+
+#ifdef B3_USE_SSE
+
+const __m128 B3_ATTRIBUTE_ALIGNED16(b3vOnes) = {1.0f, 1.0f, 1.0f, 1.0f};
+
+#endif
+
+#if defined(B3_USE_SSE) || defined(B3_USE_NEON)
+
+const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f};
+const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f};
+
+#endif
+
+/**@brief The b3Quaternion implements quaternion to perform linear algebra rotations in combination with b3Matrix3x3, b3Vector3 and b3Transform. */
+class b3Quaternion : public b3QuadWord
+{
+public:
+	/**@brief No initialization constructor */
+	b3Quaternion() {}
+
+#if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
+	// Set Vector
+	B3_FORCE_INLINE b3Quaternion(const b3SimdFloat4 vec)
+	{
+		mVec128 = vec;
+	}
+
+	// Copy constructor
+	B3_FORCE_INLINE b3Quaternion(const b3Quaternion& rhs)
+	{
+		mVec128 = rhs.mVec128;
+	}
+
+	// Assignment Operator
+	B3_FORCE_INLINE b3Quaternion&
+	operator=(const b3Quaternion& v)
+	{
+		mVec128 = v.mVec128;
+
+		return *this;
+	}
+
+#endif
+
+	//		template <typename b3Scalar>
+	//		explicit Quaternion(const b3Scalar *v) : Tuple4<b3Scalar>(v) {}
+	/**@brief Constructor from scalars */
+	b3Quaternion(const b3Scalar& _x, const b3Scalar& _y, const b3Scalar& _z, const b3Scalar& _w)
+		: b3QuadWord(_x, _y, _z, _w)
+	{
+		//b3Assert(!((_x==1.f) && (_y==0.f) && (_z==0.f) && (_w==0.f)));
+	}
+	/**@brief Axis angle Constructor
+   * @param axis The axis which the rotation is around
+   * @param angle The magnitude of the rotation around the angle (Radians) */
+	b3Quaternion(const b3Vector3& _axis, const b3Scalar& _angle)
+	{
+		setRotation(_axis, _angle);
+	}
+	/**@brief Constructor from Euler angles
+   * @param yaw Angle around Y unless B3_EULER_DEFAULT_ZYX defined then Z
+   * @param pitch Angle around X unless B3_EULER_DEFAULT_ZYX defined then Y
+   * @param roll Angle around Z unless B3_EULER_DEFAULT_ZYX defined then X */
+	b3Quaternion(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
+	{
+#ifndef B3_EULER_DEFAULT_ZYX
+		setEuler(yaw, pitch, roll);
+#else
+		setEulerZYX(yaw, pitch, roll);
+#endif
+	}
+	/**@brief Set the rotation using axis angle notation 
+   * @param axis The axis around which to rotate
+   * @param angle The magnitude of the rotation in Radians */
+	void setRotation(const b3Vector3& axis1, const b3Scalar& _angle)
+	{
+		b3Vector3 axis = axis1;
+		axis.safeNormalize();
+		
+		b3Scalar d = axis.length();
+		b3Assert(d != b3Scalar(0.0));
+		if (d < B3_EPSILON)
+		{
+			setValue(0, 0, 0, 1);
+		}
+		else
+		{
+			b3Scalar s = b3Sin(_angle * b3Scalar(0.5)) / d;
+			setValue(axis.getX() * s, axis.getY() * s, axis.getZ() * s,
+				b3Cos(_angle * b3Scalar(0.5)));
+		}
+	}
+	/**@brief Set the quaternion using Euler angles
+   * @param yaw Angle around Y
+   * @param pitch Angle around X
+   * @param roll Angle around Z */
+	void setEuler(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
+	{
+		b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5);
+		b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5);
+		b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5);
+		b3Scalar cosYaw = b3Cos(halfYaw);
+		b3Scalar sinYaw = b3Sin(halfYaw);
+		b3Scalar cosPitch = b3Cos(halfPitch);
+		b3Scalar sinPitch = b3Sin(halfPitch);
+		b3Scalar cosRoll = b3Cos(halfRoll);
+		b3Scalar sinRoll = b3Sin(halfRoll);
+		setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
+				 cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
+				 sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
+				 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
+	}
+
+	/**@brief Set the quaternion using euler angles 
+   * @param yaw Angle around Z
+   * @param pitch Angle around Y
+   * @param roll Angle around X */
+	void setEulerZYX(const b3Scalar& yawZ, const b3Scalar& pitchY, const b3Scalar& rollX)
+	{
+		b3Scalar halfYaw = b3Scalar(yawZ) * b3Scalar(0.5);
+		b3Scalar halfPitch = b3Scalar(pitchY) * b3Scalar(0.5);
+		b3Scalar halfRoll = b3Scalar(rollX) * b3Scalar(0.5);
+		b3Scalar cosYaw = b3Cos(halfYaw);
+		b3Scalar sinYaw = b3Sin(halfYaw);
+		b3Scalar cosPitch = b3Cos(halfPitch);
+		b3Scalar sinPitch = b3Sin(halfPitch);
+		b3Scalar cosRoll = b3Cos(halfRoll);
+		b3Scalar sinRoll = b3Sin(halfRoll);
+		setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,   //x
+				 cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,   //y
+				 cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,   //z
+				 cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);  //formerly yzx
+		normalize();
+	}
+
+	/**@brief Get the euler angles from this quaternion
+	   * @param yaw Angle around Z
+	   * @param pitch Angle around Y
+	   * @param roll Angle around X */
+	void getEulerZYX(b3Scalar& yawZ, b3Scalar& pitchY, b3Scalar& rollX) const
+	{
+		b3Scalar squ;
+		b3Scalar sqx;
+		b3Scalar sqy;
+		b3Scalar sqz;
+		b3Scalar sarg;
+		sqx = m_floats[0] * m_floats[0];
+		sqy = m_floats[1] * m_floats[1];
+		sqz = m_floats[2] * m_floats[2];
+		squ = m_floats[3] * m_floats[3];
+		rollX = b3Atan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz);
+		sarg = b3Scalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]);
+		pitchY = sarg <= b3Scalar(-1.0) ? b3Scalar(-0.5) * B3_PI : (sarg >= b3Scalar(1.0) ? b3Scalar(0.5) * B3_PI : b3Asin(sarg));
+		yawZ = b3Atan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz);
+	}
+
+	/**@brief Add two quaternions
+   * @param q The quaternion to add to this one */
+	B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		mVec128 = _mm_add_ps(mVec128, q.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vaddq_f32(mVec128, q.mVec128);
+#else
+		m_floats[0] += q.getX();
+		m_floats[1] += q.getY();
+		m_floats[2] += q.getZ();
+		m_floats[3] += q.m_floats[3];
+#endif
+		return *this;
+	}
+
+	/**@brief Subtract out a quaternion
+   * @param q The quaternion to subtract from this one */
+	b3Quaternion& operator-=(const b3Quaternion& q)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		mVec128 = _mm_sub_ps(mVec128, q.mVec128);
+#elif defined(B3_USE_NEON)
+		mVec128 = vsubq_f32(mVec128, q.mVec128);
+#else
+		m_floats[0] -= q.getX();
+		m_floats[1] -= q.getY();
+		m_floats[2] -= q.getZ();
+		m_floats[3] -= q.m_floats[3];
+#endif
+		return *this;
+	}
+
+	/**@brief Scale this quaternion
+   * @param s The scalar to scale by */
+	b3Quaternion& operator*=(const b3Scalar& s)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vs = _mm_load_ss(&s);  //	(S 0 0 0)
+		vs = b3_pshufd_ps(vs, 0);     //	(S S S S)
+		mVec128 = _mm_mul_ps(mVec128, vs);
+#elif defined(B3_USE_NEON)
+		mVec128 = vmulq_n_f32(mVec128, s);
+#else
+		m_floats[0] *= s;
+		m_floats[1] *= s;
+		m_floats[2] *= s;
+		m_floats[3] *= s;
+#endif
+		return *this;
+	}
+
+	/**@brief Multiply this quaternion by q on the right
+   * @param q The other quaternion 
+   * Equivilant to this = this * q */
+	b3Quaternion& operator*=(const b3Quaternion& q)
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vQ2 = q.get128();
+
+		__m128 A1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(0, 1, 2, 0));
+		__m128 B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3, 3, 3, 0));
+
+		A1 = A1 * B1;
+
+		__m128 A2 = b3_pshufd_ps(mVec128, B3_SHUFFLE(1, 2, 0, 1));
+		__m128 B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));
+
+		A2 = A2 * B2;
+
+		B1 = b3_pshufd_ps(mVec128, B3_SHUFFLE(2, 0, 1, 2));
+		B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));
+
+		B1 = B1 * B2;  //	A3 *= B3
+
+		mVec128 = b3_splat_ps(mVec128, 3);  //	A0
+		mVec128 = mVec128 * vQ2;            //	A0 * B0
+
+		A1 = A1 + A2;                  //	AB12
+		mVec128 = mVec128 - B1;        //	AB03 = AB0 - AB3
+		A1 = _mm_xor_ps(A1, b3vPPPM);  //	change sign of the last element
+		mVec128 = mVec128 + A1;        //	AB03 + AB12
+
+#elif defined(B3_USE_NEON)
+
+		float32x4_t vQ1 = mVec128;
+		float32x4_t vQ2 = q.get128();
+		float32x4_t A0, A1, B1, A2, B2, A3, B3;
+		float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
+
+		{
+			float32x2x2_t tmp;
+			tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1));  // {z x}, {w y}
+			vQ1zx = tmp.val[0];
+
+			tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2));  // {z x}, {w y}
+			vQ2zx = tmp.val[0];
+		}
+		vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
+
+		vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
+
+		vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
+		vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
+
+		A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                     // X Y  z x
+		B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx);  // W W  W X
+
+		A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
+		B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
+
+		A3 = vcombine_f32(vQ1zx, vQ1yz);  // Z X Y Z
+		B3 = vcombine_f32(vQ2yz, vQ2xz);  // Y Z x z
+
+		A1 = vmulq_f32(A1, B1);
+		A2 = vmulq_f32(A2, B2);
+		A3 = vmulq_f32(A3, B3);                           //	A3 *= B3
+		A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1);  //	A0 * B0
+
+		A1 = vaddq_f32(A1, A2);  //	AB12 = AB1 + AB2
+		A0 = vsubq_f32(A0, A3);  //	AB03 = AB0 - AB3
+
+		//	change the sign of the last element
+		A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
+		A0 = vaddq_f32(A0, A1);  //	AB03 + AB12
+
+		mVec128 = A0;
+#else
+		setValue(
+			m_floats[3] * q.getX() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.getZ() - m_floats[2] * q.getY(),
+			m_floats[3] * q.getY() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.getX() - m_floats[0] * q.getZ(),
+			m_floats[3] * q.getZ() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.getY() - m_floats[1] * q.getX(),
+			m_floats[3] * q.m_floats[3] - m_floats[0] * q.getX() - m_floats[1] * q.getY() - m_floats[2] * q.getZ());
+#endif
+		return *this;
+	}
+	/**@brief Return the dot product between this quaternion and another
+   * @param q The other quaternion */
+	b3Scalar dot(const b3Quaternion& q) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vd;
+
+		vd = _mm_mul_ps(mVec128, q.mVec128);
+
+		__m128 t = _mm_movehl_ps(vd, vd);
+		vd = _mm_add_ps(vd, t);
+		t = _mm_shuffle_ps(vd, vd, 0x55);
+		vd = _mm_add_ss(vd, t);
+
+		return _mm_cvtss_f32(vd);
+#elif defined(B3_USE_NEON)
+		float32x4_t vd = vmulq_f32(mVec128, q.mVec128);
+		float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd));
+		x = vpadd_f32(x, x);
+		return vget_lane_f32(x, 0);
+#else
+		return m_floats[0] * q.getX() +
+			   m_floats[1] * q.getY() +
+			   m_floats[2] * q.getZ() +
+			   m_floats[3] * q.m_floats[3];
+#endif
+	}
+
+	/**@brief Return the length squared of the quaternion */
+	b3Scalar length2() const
+	{
+		return dot(*this);
+	}
+
+	/**@brief Return the length of the quaternion */
+	b3Scalar length() const
+	{
+		return b3Sqrt(length2());
+	}
+
+	/**@brief Normalize the quaternion 
+   * Such that x^2 + y^2 + z^2 +w^2 = 1 */
+	b3Quaternion& normalize()
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vd;
+
+		vd = _mm_mul_ps(mVec128, mVec128);
+
+		__m128 t = _mm_movehl_ps(vd, vd);
+		vd = _mm_add_ps(vd, t);
+		t = _mm_shuffle_ps(vd, vd, 0x55);
+		vd = _mm_add_ss(vd, t);
+
+		vd = _mm_sqrt_ss(vd);
+		vd = _mm_div_ss(b3vOnes, vd);
+		vd = b3_pshufd_ps(vd, 0);  // splat
+		mVec128 = _mm_mul_ps(mVec128, vd);
+
+		return *this;
+#else
+		return *this /= length();
+#endif
+	}
+
+	/**@brief Return a scaled version of this quaternion
+   * @param s The scale factor */
+	B3_FORCE_INLINE b3Quaternion
+	operator*(const b3Scalar& s) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		__m128 vs = _mm_load_ss(&s);  //	(S 0 0 0)
+		vs = b3_pshufd_ps(vs, 0x00);  //	(S S S S)
+
+		return b3Quaternion(_mm_mul_ps(mVec128, vs));
+#elif defined(B3_USE_NEON)
+		return b3Quaternion(vmulq_n_f32(mVec128, s));
+#else
+		return b3Quaternion(getX() * s, getY() * s, getZ() * s, m_floats[3] * s);
+#endif
+	}
+
+	/**@brief Return an inversely scaled versionof this quaternion
+   * @param s The inverse scale factor */
+	b3Quaternion operator/(const b3Scalar& s) const
+	{
+		b3Assert(s != b3Scalar(0.0));
+		return *this * (b3Scalar(1.0) / s);
+	}
+
+	/**@brief Inversely scale this quaternion
+   * @param s The scale factor */
+	b3Quaternion& operator/=(const b3Scalar& s)
+	{
+		b3Assert(s != b3Scalar(0.0));
+		return *this *= b3Scalar(1.0) / s;
+	}
+
+	/**@brief Return a normalized version of this quaternion */
+	b3Quaternion normalized() const
+	{
+		return *this / length();
+	}
+	/**@brief Return the angle between this quaternion and the other 
+   * @param q The other quaternion */
+	b3Scalar angle(const b3Quaternion& q) const
+	{
+		b3Scalar s = b3Sqrt(length2() * q.length2());
+		b3Assert(s != b3Scalar(0.0));
+		return b3Acos(dot(q) / s);
+	}
+	/**@brief Return the angle of rotation represented by this quaternion */
+	b3Scalar getAngle() const
+	{
+		b3Scalar s = b3Scalar(2.) * b3Acos(m_floats[3]);
+		return s;
+	}
+
+	/**@brief Return the axis of the rotation represented by this quaternion */
+	b3Vector3 getAxis() const
+	{
+		b3Scalar s_squared = 1.f - m_floats[3] * m_floats[3];
+
+		if (s_squared < b3Scalar(10.) * B3_EPSILON)  //Check for divide by zero
+			return b3MakeVector3(1.0, 0.0, 0.0);     // Arbitrary
+		b3Scalar s = 1.f / b3Sqrt(s_squared);
+		return b3MakeVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
+	}
+
+	/**@brief Return the inverse of this quaternion */
+	b3Quaternion inverse() const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		return b3Quaternion(_mm_xor_ps(mVec128, b3vQInv));
+#elif defined(B3_USE_NEON)
+		return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)b3vQInv));
+#else
+		return b3Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
+#endif
+	}
+
+	/**@brief Return the sum of this quaternion and the other 
+   * @param q2 The other quaternion */
+	B3_FORCE_INLINE b3Quaternion
+	operator+(const b3Quaternion& q2) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		return b3Quaternion(_mm_add_ps(mVec128, q2.mVec128));
+#elif defined(B3_USE_NEON)
+		return b3Quaternion(vaddq_f32(mVec128, q2.mVec128));
+#else
+		const b3Quaternion& q1 = *this;
+		return b3Quaternion(q1.getX() + q2.getX(), q1.getY() + q2.getY(), q1.getZ() + q2.getZ(), q1.m_floats[3] + q2.m_floats[3]);
+#endif
+	}
+
+	/**@brief Return the difference between this quaternion and the other 
+   * @param q2 The other quaternion */
+	B3_FORCE_INLINE b3Quaternion
+	operator-(const b3Quaternion& q2) const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		return b3Quaternion(_mm_sub_ps(mVec128, q2.mVec128));
+#elif defined(B3_USE_NEON)
+		return b3Quaternion(vsubq_f32(mVec128, q2.mVec128));
+#else
+		const b3Quaternion& q1 = *this;
+		return b3Quaternion(q1.getX() - q2.getX(), q1.getY() - q2.getY(), q1.getZ() - q2.getZ(), q1.m_floats[3] - q2.m_floats[3]);
+#endif
+	}
+
+	/**@brief Return the negative of this quaternion 
+   * This simply negates each element */
+	B3_FORCE_INLINE b3Quaternion operator-() const
+	{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+		return b3Quaternion(_mm_xor_ps(mVec128, b3vMzeroMask));
+#elif defined(B3_USE_NEON)
+		return b3Quaternion((b3SimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)b3vMzeroMask));
+#else
+		const b3Quaternion& q2 = *this;
+		return b3Quaternion(-q2.getX(), -q2.getY(), -q2.getZ(), -q2.m_floats[3]);
+#endif
+	}
+	/**@todo document this and it's use */
+	B3_FORCE_INLINE b3Quaternion farthest(const b3Quaternion& qd) const
+	{
+		b3Quaternion diff, sum;
+		diff = *this - qd;
+		sum = *this + qd;
+		if (diff.dot(diff) > sum.dot(sum))
+			return qd;
+		return (-qd);
+	}
+
+	/**@todo document this and it's use */
+	B3_FORCE_INLINE b3Quaternion nearest(const b3Quaternion& qd) const
+	{
+		b3Quaternion diff, sum;
+		diff = *this - qd;
+		sum = *this + qd;
+		if (diff.dot(diff) < sum.dot(sum))
+			return qd;
+		return (-qd);
+	}
+
+	/**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
+   * @param q The other quaternion to interpolate with 
+   * @param t The ratio between this and q to interpolate.  If t = 0 the result is this, if t=1 the result is q.
+   * Slerp interpolates assuming constant velocity.  */
+	b3Quaternion slerp(const b3Quaternion& q, const b3Scalar& t) const
+	{
+		b3Scalar magnitude = b3Sqrt(length2() * q.length2());
+		b3Assert(magnitude > b3Scalar(0));
+
+		b3Scalar product = dot(q) / magnitude;
+		if (b3Fabs(product) < b3Scalar(1))
+		{
+			// Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
+			const b3Scalar sign = (product < 0) ? b3Scalar(-1) : b3Scalar(1);
+
+			const b3Scalar theta = b3Acos(sign * product);
+			const b3Scalar s1 = b3Sin(sign * t * theta);
+			const b3Scalar d = b3Scalar(1.0) / b3Sin(theta);
+			const b3Scalar s0 = b3Sin((b3Scalar(1.0) - t) * theta);
+
+			return b3Quaternion(
+				(m_floats[0] * s0 + q.getX() * s1) * d,
+				(m_floats[1] * s0 + q.getY() * s1) * d,
+				(m_floats[2] * s0 + q.getZ() * s1) * d,
+				(m_floats[3] * s0 + q.m_floats[3] * s1) * d);
+		}
+		else
+		{
+			return *this;
+		}
+	}
+
+	static const b3Quaternion& getIdentity()
+	{
+		static const b3Quaternion identityQuat(b3Scalar(0.), b3Scalar(0.), b3Scalar(0.), b3Scalar(1.));
+		return identityQuat;
+	}
+
+	B3_FORCE_INLINE const b3Scalar& getW() const { return m_floats[3]; }
+};
+
+/**@brief Return the product of two quaternions */
+B3_FORCE_INLINE b3Quaternion
+operator*(const b3Quaternion& q1, const b3Quaternion& q2)
+{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+	__m128 vQ1 = q1.get128();
+	__m128 vQ2 = q2.get128();
+	__m128 A0, A1, B1, A2, B2;
+
+	A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0, 1, 2, 0));  // X Y  z x     //      vtrn
+	B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3, 3, 3, 0));  // W W  W X     // vdup vext
+
+	A1 = A1 * B1;
+
+	A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1, 2, 0, 1));  // Y Z  X Y     // vext
+	B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));  // z x  Y Y     // vtrn vdup
+
+	A2 = A2 * B2;
+
+	B1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2, 0, 1, 2));  // z x Y Z      // vtrn vext
+	B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));  // Y Z x z      // vext vtrn
+
+	B1 = B1 * B2;  //	A3 *= B3
+
+	A0 = b3_splat_ps(vQ1, 3);  //	A0
+	A0 = A0 * vQ2;             //	A0 * B0
+
+	A1 = A1 + A2;  //	AB12
+	A0 = A0 - B1;  //	AB03 = AB0 - AB3
+
+	A1 = _mm_xor_ps(A1, b3vPPPM);  //	change sign of the last element
+	A0 = A0 + A1;                  //	AB03 + AB12
+
+	return b3Quaternion(A0);
+
+#elif defined(B3_USE_NEON)
+
+	float32x4_t vQ1 = q1.get128();
+	float32x4_t vQ2 = q2.get128();
+	float32x4_t A0, A1, B1, A2, B2, A3, B3;
+	float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
+
+	{
+		float32x2x2_t tmp;
+		tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1));  // {z x}, {w y}
+		vQ1zx = tmp.val[0];
+
+		tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2));  // {z x}, {w y}
+		vQ2zx = tmp.val[0];
+	}
+	vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
+
+	vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
+
+	vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
+	vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
+
+	A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                     // X Y  z x
+	B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx);  // W W  W X
+
+	A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
+	B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
+
+	A3 = vcombine_f32(vQ1zx, vQ1yz);  // Z X Y Z
+	B3 = vcombine_f32(vQ2yz, vQ2xz);  // Y Z x z
+
+	A1 = vmulq_f32(A1, B1);
+	A2 = vmulq_f32(A2, B2);
+	A3 = vmulq_f32(A3, B3);                           //	A3 *= B3
+	A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1);  //	A0 * B0
+
+	A1 = vaddq_f32(A1, A2);  //	AB12 = AB1 + AB2
+	A0 = vsubq_f32(A0, A3);  //	AB03 = AB0 - AB3
+
+	//	change the sign of the last element
+	A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
+	A0 = vaddq_f32(A0, A1);  //	AB03 + AB12
+
+	return b3Quaternion(A0);
+
+#else
+	return b3Quaternion(
+		q1.getW() * q2.getX() + q1.getX() * q2.getW() + q1.getY() * q2.getZ() - q1.getZ() * q2.getY(),
+		q1.getW() * q2.getY() + q1.getY() * q2.getW() + q1.getZ() * q2.getX() - q1.getX() * q2.getZ(),
+		q1.getW() * q2.getZ() + q1.getZ() * q2.getW() + q1.getX() * q2.getY() - q1.getY() * q2.getX(),
+		q1.getW() * q2.getW() - q1.getX() * q2.getX() - q1.getY() * q2.getY() - q1.getZ() * q2.getZ());
+#endif
+}
+
+B3_FORCE_INLINE b3Quaternion
+operator*(const b3Quaternion& q, const b3Vector3& w)
+{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+	__m128 vQ1 = q.get128();
+	__m128 vQ2 = w.get128();
+	__m128 A1, B1, A2, B2, A3, B3;
+
+	A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(3, 3, 3, 0));
+	B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(0, 1, 2, 0));
+
+	A1 = A1 * B1;
+
+	A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1, 2, 0, 1));
+	B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));
+
+	A2 = A2 * B2;
+
+	A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2, 0, 1, 2));
+	B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));
+
+	A3 = A3 * B3;  //	A3 *= B3
+
+	A1 = A1 + A2;                  //	AB12
+	A1 = _mm_xor_ps(A1, b3vPPPM);  //	change sign of the last element
+	A1 = A1 - A3;                  //	AB123 = AB12 - AB3
+
+	return b3Quaternion(A1);
+
+#elif defined(B3_USE_NEON)
+
+	float32x4_t vQ1 = q.get128();
+	float32x4_t vQ2 = w.get128();
+	float32x4_t A1, B1, A2, B2, A3, B3;
+	float32x2_t vQ1wx, vQ2zx, vQ1yz, vQ2yz, vQ1zx, vQ2xz;
+
+	vQ1wx = vext_f32(vget_high_f32(vQ1), vget_low_f32(vQ1), 1);
+	{
+		float32x2x2_t tmp;
+
+		tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2));  // {z x}, {w y}
+		vQ2zx = tmp.val[0];
+
+		tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1));  // {z x}, {w y}
+		vQ1zx = tmp.val[0];
+	}
+
+	vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
+
+	vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
+	vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
+
+	A1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ1), 1), vQ1wx);  // W W  W X
+	B1 = vcombine_f32(vget_low_f32(vQ2), vQ2zx);                     // X Y  z x
+
+	A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
+	B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
+
+	A3 = vcombine_f32(vQ1zx, vQ1yz);  // Z X Y Z
+	B3 = vcombine_f32(vQ2yz, vQ2xz);  // Y Z x z
+
+	A1 = vmulq_f32(A1, B1);
+	A2 = vmulq_f32(A2, B2);
+	A3 = vmulq_f32(A3, B3);  //	A3 *= B3
+
+	A1 = vaddq_f32(A1, A2);  //	AB12 = AB1 + AB2
+
+	//	change the sign of the last element
+	A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
+
+	A1 = vsubq_f32(A1, A3);  //	AB123 = AB12 - AB3
+
+	return b3Quaternion(A1);
+
+#else
+	return b3Quaternion(
+		q.getW() * w.getX() + q.getY() * w.getZ() - q.getZ() * w.getY(),
+		q.getW() * w.getY() + q.getZ() * w.getX() - q.getX() * w.getZ(),
+		q.getW() * w.getZ() + q.getX() * w.getY() - q.getY() * w.getX(),
+		-q.getX() * w.getX() - q.getY() * w.getY() - q.getZ() * w.getZ());
+#endif
+}
+
+B3_FORCE_INLINE b3Quaternion
+operator*(const b3Vector3& w, const b3Quaternion& q)
+{
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+	__m128 vQ1 = w.get128();
+	__m128 vQ2 = q.get128();
+	__m128 A1, B1, A2, B2, A3, B3;
+
+	A1 = b3_pshufd_ps(vQ1, B3_SHUFFLE(0, 1, 2, 0));  // X Y  z x
+	B1 = b3_pshufd_ps(vQ2, B3_SHUFFLE(3, 3, 3, 0));  // W W  W X
+
+	A1 = A1 * B1;
+
+	A2 = b3_pshufd_ps(vQ1, B3_SHUFFLE(1, 2, 0, 1));
+	B2 = b3_pshufd_ps(vQ2, B3_SHUFFLE(2, 0, 1, 1));
+
+	A2 = A2 * B2;
+
+	A3 = b3_pshufd_ps(vQ1, B3_SHUFFLE(2, 0, 1, 2));
+	B3 = b3_pshufd_ps(vQ2, B3_SHUFFLE(1, 2, 0, 2));
+
+	A3 = A3 * B3;  //	A3 *= B3
+
+	A1 = A1 + A2;                  //	AB12
+	A1 = _mm_xor_ps(A1, b3vPPPM);  //	change sign of the last element
+	A1 = A1 - A3;                  //	AB123 = AB12 - AB3
+
+	return b3Quaternion(A1);
+
+#elif defined(B3_USE_NEON)
+
+	float32x4_t vQ1 = w.get128();
+	float32x4_t vQ2 = q.get128();
+	float32x4_t A1, B1, A2, B2, A3, B3;
+	float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
+
+	{
+		float32x2x2_t tmp;
+
+		tmp = vtrn_f32(vget_high_f32(vQ1), vget_low_f32(vQ1));  // {z x}, {w y}
+		vQ1zx = tmp.val[0];
+
+		tmp = vtrn_f32(vget_high_f32(vQ2), vget_low_f32(vQ2));  // {z x}, {w y}
+		vQ2zx = tmp.val[0];
+	}
+	vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1);
+
+	vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
+
+	vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
+	vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
+
+	A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                     // X Y  z x
+	B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx);  // W W  W X
+
+	A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
+	B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
+
+	A3 = vcombine_f32(vQ1zx, vQ1yz);  // Z X Y Z
+	B3 = vcombine_f32(vQ2yz, vQ2xz);  // Y Z x z
+
+	A1 = vmulq_f32(A1, B1);
+	A2 = vmulq_f32(A2, B2);
+	A3 = vmulq_f32(A3, B3);  //	A3 *= B3
+
+	A1 = vaddq_f32(A1, A2);  //	AB12 = AB1 + AB2
+
+	//	change the sign of the last element
+	A1 = (b3SimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)b3vPPPM);
+
+	A1 = vsubq_f32(A1, A3);  //	AB123 = AB12 - AB3
+
+	return b3Quaternion(A1);
+
+#else
+	return b3Quaternion(
+		+w.getX() * q.getW() + w.getY() * q.getZ() - w.getZ() * q.getY(),
+		+w.getY() * q.getW() + w.getZ() * q.getX() - w.getX() * q.getZ(),
+		+w.getZ() * q.getW() + w.getX() * q.getY() - w.getY() * q.getX(),
+		-w.getX() * q.getX() - w.getY() * q.getY() - w.getZ() * q.getZ());
+#endif
+}
+
+/**@brief Calculate the dot product between two quaternions */
+B3_FORCE_INLINE b3Scalar
+b3Dot(const b3Quaternion& q1, const b3Quaternion& q2)
+{
+	return q1.dot(q2);
+}
+
+/**@brief Return the length of a quaternion */
+B3_FORCE_INLINE b3Scalar
+b3Length(const b3Quaternion& q)
+{
+	return q.length();
+}
+
+/**@brief Return the angle between two quaternions*/
+B3_FORCE_INLINE b3Scalar
+b3Angle(const b3Quaternion& q1, const b3Quaternion& q2)
+{
+	return q1.angle(q2);
+}
+
+/**@brief Return the inverse of a quaternion*/
+B3_FORCE_INLINE b3Quaternion
+b3Inverse(const b3Quaternion& q)
+{
+	return q.inverse();
+}
+
+/**@brief Return the result of spherical linear interpolation betwen two quaternions 
+ * @param q1 The first quaternion
+ * @param q2 The second quaternion 
+ * @param t The ration between q1 and q2.  t = 0 return q1, t=1 returns q2 
+ * Slerp assumes constant velocity between positions. */
+B3_FORCE_INLINE b3Quaternion
+b3Slerp(const b3Quaternion& q1, const b3Quaternion& q2, const b3Scalar& t)
+{
+	return q1.slerp(q2, t);
+}
+
+B3_FORCE_INLINE b3Quaternion
+b3QuatMul(const b3Quaternion& rot0, const b3Quaternion& rot1)
+{
+	return rot0 * rot1;
+}
+
+B3_FORCE_INLINE b3Quaternion
+b3QuatNormalized(const b3Quaternion& orn)
+{
+	return orn.normalized();
+}
+
+B3_FORCE_INLINE b3Vector3
+b3QuatRotate(const b3Quaternion& rotation, const b3Vector3& v)
+{
+	b3Quaternion q = rotation * v;
+	q *= rotation.inverse();
+#if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
+	return b3MakeVector3(_mm_and_ps(q.get128(), b3vFFF0fMask));
+#elif defined(B3_USE_NEON)
+	return b3MakeVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), b3vFFF0Mask));
+#else
+	return b3MakeVector3(q.getX(), q.getY(), q.getZ());
+#endif
+}
+
+B3_FORCE_INLINE b3Quaternion
+b3ShortestArcQuat(const b3Vector3& v0, const b3Vector3& v1)  // Game Programming Gems 2.10. make sure v0,v1 are normalized
+{
+	b3Vector3 c = v0.cross(v1);
+	b3Scalar d = v0.dot(v1);
+
+	if (d < -1.0 + B3_EPSILON)
+	{
+		b3Vector3 n, unused;
+		b3PlaneSpace1(v0, n, unused);
+		return b3Quaternion(n.getX(), n.getY(), n.getZ(), 0.0f);  // just pick any vector that is orthogonal to v0
+	}
+
+	b3Scalar s = b3Sqrt((1.0f + d) * 2.0f);
+	b3Scalar rs = 1.0f / s;
+
+	return b3Quaternion(c.getX() * rs, c.getY() * rs, c.getZ() * rs, s * 0.5f);
+}
+
+B3_FORCE_INLINE b3Quaternion
+b3ShortestArcQuatNormalize2(b3Vector3& v0, b3Vector3& v1)
+{
+	v0.normalize();
+	v1.normalize();
+	return b3ShortestArcQuat(v0, v1);
+}
+
+#endif  //B3_SIMD__QUATERNION_H_

thirdparty/bullet/src/Bullet3Common/b3Random.h

@@ -0,0 +1,46 @@
+/*
+Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef B3_GEN_RANDOM_H
+#define B3_GEN_RANDOM_H
+
+#include "b3Scalar.h"
+
+#ifdef MT19937
+
+#include <limits.h>
+#include <mt19937.h>
+
+#define B3_RAND_MAX UINT_MAX
+
+B3_FORCE_INLINE void b3Srand(unsigned int seed) { init_genrand(seed); }
+B3_FORCE_INLINE unsigned int b3rand() { return genrand_int32(); }
+
+#else
+
+#include <stdlib.h>
+
+#define B3_RAND_MAX RAND_MAX
+
+B3_FORCE_INLINE void b3Srand(unsigned int seed) { srand(seed); }
+B3_FORCE_INLINE unsigned int b3rand() { return rand(); }
+
+#endif
+
+inline b3Scalar b3RandRange(b3Scalar minRange, b3Scalar maxRange)
+{
+	return (b3rand() / (b3Scalar(B3_RAND_MAX) + b3Scalar(1.0))) * (maxRange - minRange) + minRange;
+}
+
+#endif  //B3_GEN_RANDOM_H

thirdparty/bullet/src/Bullet3Common/b3ResizablePool.h

@@ -0,0 +1,171 @@
+
+#ifndef B3_RESIZABLE_POOL_H
+#define B3_RESIZABLE_POOL_H
+
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+enum
+{
+	B3_POOL_HANDLE_TERMINAL_FREE = -1,
+	B3_POOL_HANDLE_TERMINAL_USED = -2
+};
+
+template <typename U>
+struct b3PoolBodyHandle : public U
+{
+	B3_DECLARE_ALIGNED_ALLOCATOR();
+
+	int m_nextFreeHandle;
+	void setNextFree(int next)
+	{
+		m_nextFreeHandle = next;
+	}
+	int getNextFree() const
+	{
+		return m_nextFreeHandle;
+	}
+};
+
+template <typename T>
+class b3ResizablePool
+{
+protected:
+	b3AlignedObjectArray<T> m_bodyHandles;
+	int m_numUsedHandles;   // number of active handles
+	int m_firstFreeHandle;  // free handles list
+
+	T* getHandleInternal(int handle)
+	{
+		return &m_bodyHandles[handle];
+	}
+	const T* getHandleInternal(int handle) const
+	{
+		return &m_bodyHandles[handle];
+	}
+
+public:
+	b3ResizablePool()
+	{
+		initHandles();
+	}
+
+	virtual ~b3ResizablePool()
+	{
+		exitHandles();
+	}
+	///handle management
+
+	int getNumHandles() const
+	{
+		return m_bodyHandles.size();
+	}
+
+	void getUsedHandles(b3AlignedObjectArray<int>& usedHandles) const
+	{
+		for (int i = 0; i < m_bodyHandles.size(); i++)
+		{
+			if (m_bodyHandles[i].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
+			{
+				usedHandles.push_back(i);
+			}
+		}
+	}
+
+	T* getHandle(int handle)
+	{
+		b3Assert(handle >= 0);
+		b3Assert(handle < m_bodyHandles.size());
+		if ((handle < 0) || (handle >= m_bodyHandles.size()))
+		{
+			return 0;
+		}
+
+		if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
+		{
+			return &m_bodyHandles[handle];
+		}
+		return 0;
+	}
+	const T* getHandle(int handle) const
+	{
+		b3Assert(handle >= 0);
+		b3Assert(handle < m_bodyHandles.size());
+		if ((handle < 0) || (handle >= m_bodyHandles.size()))
+		{
+			return 0;
+		}
+
+		if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
+		{
+			return &m_bodyHandles[handle];
+		}
+		return 0;
+	}
+
+	void increaseHandleCapacity(int extraCapacity)
+	{
+		int curCapacity = m_bodyHandles.size();
+		//b3Assert(curCapacity == m_numUsedHandles);
+		int newCapacity = curCapacity + extraCapacity;
+		m_bodyHandles.resize(newCapacity);
+
+		{
+			for (int i = curCapacity; i < newCapacity; i++)
+				m_bodyHandles[i].setNextFree(i + 1);
+
+			m_bodyHandles[newCapacity - 1].setNextFree(-1);
+		}
+		m_firstFreeHandle = curCapacity;
+	}
+	void initHandles()
+	{
+		m_numUsedHandles = 0;
+		m_firstFreeHandle = -1;
+
+		increaseHandleCapacity(1);
+	}
+
+	void exitHandles()
+	{
+		m_bodyHandles.resize(0);
+		m_firstFreeHandle = -1;
+		m_numUsedHandles = 0;
+	}
+
+	int allocHandle()
+	{
+		b3Assert(m_firstFreeHandle >= 0);
+
+		int handle = m_firstFreeHandle;
+		m_firstFreeHandle = getHandleInternal(handle)->getNextFree();
+		m_numUsedHandles++;
+
+		if (m_firstFreeHandle < 0)
+		{
+			//int curCapacity = m_bodyHandles.size();
+			int additionalCapacity = m_bodyHandles.size();
+			increaseHandleCapacity(additionalCapacity);
+
+			getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
+		}
+		getHandleInternal(handle)->setNextFree(B3_POOL_HANDLE_TERMINAL_USED);
+		getHandleInternal(handle)->clear();
+		return handle;
+	}
+
+	void freeHandle(int handle)
+	{
+		b3Assert(handle >= 0);
+
+		if (m_bodyHandles[handle].getNextFree() == B3_POOL_HANDLE_TERMINAL_USED)
+		{
+			getHandleInternal(handle)->clear();
+			getHandleInternal(handle)->setNextFree(m_firstFreeHandle);
+			m_firstFreeHandle = handle;
+			m_numUsedHandles--;
+		}
+	}
+};
+///end handle management
+
+#endif  //B3_RESIZABLE_POOL_H