Merge branch 'master' of https://bitbucket.org/erwincoumans/bullet3_experiments

build/premake4.lua

@@ -109,6 +109,10 @@
 		include "../btgui/OpenGLTrueTypeFont"
 --		include "../btgui/OpenGLWindow"
 --		include "../demo/ObjLoader"
-		
+		include "../src/Bullet3Dynamics"
+		include "../src/Bullet3Common"
+		include "../src/Bullet3Geometry"
+		include "../src/Bullet3Collision"
+			
 		
 	end

demo/gpudemo/GpuDemo.h

@@ -38,9 +38,9 @@ public:
                     :useOpenCL(true),
                     preferredOpenCLPlatformIndex(-1),
                     preferredOpenCLDeviceIndex(-1),
-					arraySizeX(30),
+					arraySizeX(10),
 		arraySizeY(30),
-		arraySizeZ(30),
+		arraySizeZ(10),
 		m_useConcaveMesh(false),
 		gapX(14.3),
 		gapY(14.0),

demo/gpudemo/premake4.lua

@@ -25,7 +25,10 @@ function createProject(vendor)
 		}
 		
 		links {
-			"gwen"
+			"gwen",
+			"Bullet3Common",
+			"Bullet3Geometry",
+			"Bullet3Dynamics"
 		}
 		
 		files {
@@ -41,12 +44,6 @@ function createProject(vendor)
 			"../ObjLoader/list.cpp",
 			"../ObjLoader/list.h",
 			
-			"../../src/Bullet3Common/b3AlignedAllocator.cpp",
-			"../../src/Bullet3Common/b3AlignedAllocator.h",
-			"../../src/Bullet3Common/b3Quickprof.cpp",
-			"../../src/Bullet3Common/b3Quickprof.h",
-			"../../src/Bullet3Geometry/b3ConvexHullComputer.cpp",
-			"../../src/Bullet3Geometry/b3ConvexHullComputer.h",
 			
 			"../../btgui/OpenGLWindow/GLInstancingRenderer.cpp",
 			"../../btgui/OpenGLWindow/GLInstancingRenderer.h",

opencl/gpu_narrowphase/host/b3ConvexHullContact.h

@@ -3,12 +3,12 @@
 #define _CONVEX_HULL_CONTACT_H
 
 #include "parallel_primitives/host/btOpenCLArray.h"
-#include "b3RigidBodyCL.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
 #include "Bullet3Common/b3AlignedObjectArray.h"
 #include "b3ConvexUtility.h"
 #include "b3ConvexPolyhedronCL.h"
 #include "b3Collidable.h"
-#include "b3Contact4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
 #include "parallel_primitives/host/btInt2.h"
 #include "parallel_primitives/host/btInt4.h"
 #include "b3OptimizedBvh.h"

opencl/gpu_rigidbody/host/b3GpuBatchingPgsSolver.h

@@ -4,8 +4,8 @@
 
 #include "../../basic_initialize/b3OpenCLInclude.h"
 #include "../../parallel_primitives/host/btOpenCLArray.h"
-#include "../../gpu_narrowphase/host/b3RigidBodyCL.h"
-#include "../../gpu_narrowphase/host/b3Contact4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
 #include "b3GpuConstraint4.h"
 
 class b3GpuBatchingPgsSolver

opencl/gpu_rigidbody/host/b3GpuRigidBodyPipeline.cpp

@@ -9,14 +9,18 @@
 #include "../../gpu_broadphase/host/b3SapAabb.h"
 #include "../../gpu_broadphase/host/b3GpuSapBroadphase.h"
 #include "parallel_primitives/host/btLauncherCL.h"
+#include "Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h"
+
+
 //#define TEST_OTHER_GPU_SOLVER
+
 #ifdef TEST_OTHER_GPU_SOLVER
 #include "btGpuJacobiSolver.h"
-#include "btPgsJacobiSolver.h"
+#include "b3PgsJacobiSolver.h"
 #endif //TEST_OTHER_GPU_SOLVER
 
-#include "../../gpu_narrowphase/host/b3RigidBodyCL.h"
-#include "../../gpu_narrowphase/host/b3Contact4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
 #include "b3GpuBatchingPgsSolver.h"
 #include "b3Solver.h"
 
@@ -31,8 +35,11 @@ b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id devic
 	m_data->m_context = ctx;
 	m_data->m_device = device;
 	m_data->m_queue = q;
+
+	m_data->m_solver = new b3PgsJacobiSolver();
+
+	
 #ifdef TEST_OTHER_GPU_SOLVER
-	m_data->m_solver = new btPgsJacobiSolver();
 	m_data->m_solver3 = new btGpuJacobiSolver(ctx,device,q,config.m_maxBroadphasePairs);	
 #endif //	TEST_OTHER_GPU_SOLVER
 	b3Config config;
@@ -66,9 +73,10 @@ b3GpuRigidBodyPipeline::b3GpuRigidBodyPipeline(cl_context ctx,cl_device_id devic
 b3GpuRigidBodyPipeline::~b3GpuRigidBodyPipeline()
 {
 	clReleaseKernel(m_data->m_integrateTransformsKernel);
-	
-#ifdef TEST_OTHER_GPU_SOLVER
+
 	delete m_data->m_solver;
+
+#ifdef TEST_OTHER_GPU_SOLVER
 	delete m_data->m_solver3;
 #endif //TEST_OTHER_GPU_SOLVER
 	
@@ -141,7 +149,20 @@ void	b3GpuRigidBodyPipeline::stepSimulation(float deltaTime)
 		btOpenCLArray<b3Contact4> gpuContacts(m_data->m_context,m_data->m_queue,0,true);
 		gpuContacts.setFromOpenCLBuffer(m_data->m_narrowphase->getContactsGpu(),m_data->m_narrowphase->getNumContactsGpu());
 
-		bool useJacobi = false;
+		bool useBullet2CpuSolver = false;
+		if (useBullet2CpuSolver)
+		{
+			b3AlignedObjectArray<b3RigidBodyCL> hostBodies;
+			gpuBodies.copyToHost(hostBodies);
+			b3AlignedObjectArray<btInertiaCL> hostInertias;
+			gpuInertias.copyToHost(hostInertias);
+			b3AlignedObjectArray<b3Contact4> hostContacts;
+			gpuContacts.copyToHost(hostContacts);
+			{
+				m_data->m_solver->solveContacts(m_data->m_narrowphase->getNumBodiesGpu(),&hostBodies[0],&hostInertias[0],numContacts,&hostContacts[0]);
+			}
+			gpuBodies.copyFromHost(hostBodies);
+		} else
 #ifdef TEST_OTHER_GPU_SOLVER
 		if (useJacobi)
 		{

opencl/gpu_rigidbody/host/b3GpuRigidBodyPipelineInternalData.h

@@ -18,7 +18,7 @@ struct b3GpuRigidBodyPipelineInternalData
 	cl_kernel	m_integrateTransformsKernel;
 	cl_kernel	m_updateAabbsKernel;
 	
-	class btPgsJacobiSolver* m_solver;
+	class b3PgsJacobiSolver* m_solver;
 	class b3GpuBatchingPgsSolver* m_solver2;
 	class btGpuJacobiSolver* m_solver3;
 	

opencl/gpu_rigidbody/host/b3Solver.h

@@ -19,8 +19,8 @@ subject to the following restrictions:
 
 #include "../../parallel_primitives/host/btOpenCLArray.h"
 #include "../host/b3GpuConstraint4.h"
-#include "../../gpu_narrowphase/host/b3RigidBodyCL.h"
-#include "../../gpu_narrowphase/host/b3Contact4.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
+#include "Bullet3Collision/NarrowPhaseCollision/b3Contact4.h"
 
 #include "../host/b3GpuConstraint4.h"
 #include "../../parallel_primitives/host/btPrefixScanCL.h"

src/Bullet3Collision/premake4.lua

@@ -0,0 +1,13 @@
+	project "Bullet3Collision"
+
+	language "C++"
+				
+	kind "StaticLib"
+		
+	includedirs {".."}
+	targetdir "../../bin"
+
+	files {
+		"**.cpp",
+		"**.h"
+	}

src/Bullet3Dynamics/ConstraintSolver/b3ContactSolverInfo.h

@@ -0,0 +1,159 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_CONTACT_SOLVER_INFO
+#define BT_CONTACT_SOLVER_INFO
+
+#include "Bullet3Common/b3Scalar.h"
+
+enum	btSolverMode
+{
+	SOLVER_RANDMIZE_ORDER = 1,
+	SOLVER_FRICTION_SEPARATE = 2,
+	SOLVER_USE_WARMSTARTING = 4,
+	SOLVER_USE_2_FRICTION_DIRECTIONS = 16,
+	SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32,
+	SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64,
+	SOLVER_CACHE_FRIENDLY = 128,
+	SOLVER_SIMD = 256,
+	SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512,
+	SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024
+};
+
+struct btContactSolverInfoData
+{
+	
+
+	b3Scalar	m_tau;
+	b3Scalar	m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	b3Scalar	m_friction;
+	b3Scalar	m_timeStep;
+	b3Scalar	m_restitution;
+	int		m_numIterations;
+	b3Scalar	m_maxErrorReduction;
+	b3Scalar	m_sor;
+	b3Scalar	m_erp;//used as Baumgarte factor
+	b3Scalar	m_erp2;//used in Split Impulse
+	b3Scalar	m_globalCfm;//constraint force mixing
+	int			m_splitImpulse;
+	b3Scalar	m_splitImpulsePenetrationThreshold;
+	b3Scalar	m_splitImpulseTurnErp;
+	b3Scalar	m_linearSlop;
+	b3Scalar	m_warmstartingFactor;
+
+	int			m_solverMode;
+	int	m_restingContactRestitutionThreshold;
+	int			m_minimumSolverBatchSize;
+	b3Scalar	m_maxGyroscopicForce;
+	b3Scalar	m_singleAxisRollingFrictionThreshold;
+
+
+};
+
+struct b3ContactSolverInfo : public btContactSolverInfoData
+{
+
+	
+
+	inline b3ContactSolverInfo()
+	{
+		m_tau = b3Scalar(0.6);
+		m_damping = b3Scalar(1.0);
+		m_friction = b3Scalar(0.3);
+		m_timeStep = b3Scalar(1.f/60.f);
+		m_restitution = b3Scalar(0.);
+		m_maxErrorReduction = b3Scalar(20.);
+		m_numIterations = 10;
+		m_erp = b3Scalar(0.2);
+		m_erp2 = b3Scalar(0.8);
+		m_globalCfm = b3Scalar(0.);
+		m_sor = b3Scalar(1.);
+		m_splitImpulse = true;
+		m_splitImpulsePenetrationThreshold = -.04f;
+		m_splitImpulseTurnErp = 0.1f;
+		m_linearSlop = b3Scalar(0.0);
+		m_warmstartingFactor=b3Scalar(0.85);
+		//m_solverMode =  SOLVER_USE_WARMSTARTING |  SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER;
+		m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
+		m_restingContactRestitutionThreshold = 2;//unused as of 2.81
+		m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
+		m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag)
+		m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
+	}
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btContactSolverInfoDoubleData
+{
+	double		m_tau;
+	double		m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	double		m_friction;
+	double		m_timeStep;
+	double		m_restitution;
+	double		m_maxErrorReduction;
+	double		m_sor;
+	double		m_erp;//used as Baumgarte factor
+	double		m_erp2;//used in Split Impulse
+	double		m_globalCfm;//constraint force mixing
+	double		m_splitImpulsePenetrationThreshold;
+	double		m_splitImpulseTurnErp;
+	double		m_linearSlop;
+	double		m_warmstartingFactor;
+	double		m_maxGyroscopicForce;
+	double		m_singleAxisRollingFrictionThreshold;
+
+	int			m_numIterations;
+	int			m_solverMode;
+	int			m_restingContactRestitutionThreshold;
+	int			m_minimumSolverBatchSize;
+	int			m_splitImpulse;
+	char		m_padding[4];
+
+};
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct btContactSolverInfoFloatData
+{
+	float		m_tau;
+	float		m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'.
+	float		m_friction;
+	float		m_timeStep;
+
+	float		m_restitution;
+	float		m_maxErrorReduction;
+	float		m_sor;
+	float		m_erp;//used as Baumgarte factor
+
+	float		m_erp2;//used in Split Impulse
+	float		m_globalCfm;//constraint force mixing
+	float		m_splitImpulsePenetrationThreshold;
+	float		m_splitImpulseTurnErp;
+
+	float		m_linearSlop;
+	float		m_warmstartingFactor;
+	float		m_maxGyroscopicForce;
+	float		m_singleAxisRollingFrictionThreshold;
+
+	int			m_numIterations;
+	int			m_solverMode;
+	int			m_restingContactRestitutionThreshold;
+	int			m_minimumSolverBatchSize;
+
+	int			m_splitImpulse;
+	char		m_padding[4];
+};
+
+
+
+#endif //BT_CONTACT_SOLVER_INFO

src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.h

@@ -0,0 +1,145 @@
+#ifndef BT_PGS_JACOBI_SOLVER
+#define BT_PGS_JACOBI_SOLVER
+
+
+struct b3Contact4;
+struct btContactPoint;
+
+
+class btDispatcher;
+
+#include "b3TypedConstraint.h"
+#include "b3ContactSolverInfo.h"
+#include "b3SolverBody.h"
+#include "b3SolverConstraint.h"
+
+struct b3RigidBodyCL;
+struct btInertiaCL;
+
+class b3PgsJacobiSolver
+{
+
+protected:
+	b3AlignedObjectArray<b3SolverBody>      m_tmpSolverBodyPool;
+	btConstraintArray			m_tmpSolverContactConstraintPool;
+	btConstraintArray			m_tmpSolverNonContactConstraintPool;
+	btConstraintArray			m_tmpSolverContactFrictionConstraintPool;
+	btConstraintArray			m_tmpSolverContactRollingFrictionConstraintPool;
+
+	b3AlignedObjectArray<int>	m_orderTmpConstraintPool;
+	b3AlignedObjectArray<int>	m_orderNonContactConstraintPool;
+	b3AlignedObjectArray<int>	m_orderFrictionConstraintPool;
+	b3AlignedObjectArray<b3TypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
+	
+	b3AlignedObjectArray<int>		m_bodyCount;
+	b3AlignedObjectArray<int>		m_bodyCountCheck;
+	
+	b3AlignedObjectArray<b3Vector3>	m_deltaLinearVelocities;
+	b3AlignedObjectArray<b3Vector3>	m_deltaAngularVelocities;
+
+	bool						m_usePgs;
+	void						averageVelocities();
+
+	int							m_maxOverrideNumSolverIterations;
+	b3Scalar	getContactProcessingThreshold(b3Contact4* contact)
+	{
+		return 0.02f;
+	}
+	void setupFrictionConstraint(	b3RigidBodyCL* bodies,btInertiaCL* inertias, b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
+									btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
+									b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, 
+									b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
+
+	void setupRollingFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias,	b3SolverConstraint& solverConstraint, const b3Vector3& normalAxis,int solverBodyIdA,int  solverBodyIdB,
+									btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,
+									b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, 
+									b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
+
+	b3SolverConstraint&	addFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0., b3Scalar cfmSlip=0.);
+	b3SolverConstraint&	addRollingFrictionConstraint(b3RigidBodyCL* bodies,btInertiaCL* inertias,const b3Vector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btContactPoint& cp,const b3Vector3& rel_pos1,const b3Vector3& rel_pos2,b3RigidBodyCL* colObj0,b3RigidBodyCL* colObj1, b3Scalar relaxation, b3Scalar desiredVelocity=0, b3Scalar cfmSlip=0.f);
+
+
+	void setupContactConstraint(b3RigidBodyCL* bodies, btInertiaCL* inertias,
+								b3SolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btContactPoint& cp, 
+								const b3ContactSolverInfo& infoGlobal, b3Vector3& vel, b3Scalar& rel_vel, b3Scalar& relaxation, 
+								b3Vector3& rel_pos1, b3Vector3& rel_pos2);
+
+	void setFrictionConstraintImpulse( b3RigidBodyCL* bodies, btInertiaCL* inertias,b3SolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, 
+										 btContactPoint& cp, const b3ContactSolverInfo& infoGlobal);
+
+	///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
+	unsigned long	m_btSeed2;
+
+	
+	b3Scalar restitutionCurve(b3Scalar rel_vel, b3Scalar restitution);
+
+	void	convertContact(b3RigidBodyCL* bodies, btInertiaCL* inertias,b3Contact4* manifold,const b3ContactSolverInfo& infoGlobal);
+
+
+	void	resolveSplitPenetrationSIMD(
+     b3SolverBody& bodyA,b3SolverBody& bodyB,
+        const b3SolverConstraint& contactConstraint);
+
+	void	resolveSplitPenetrationImpulseCacheFriendly(
+       b3SolverBody& bodyA,b3SolverBody& bodyB,
+        const b3SolverConstraint& contactConstraint);
+
+	//internal method
+	int		getOrInitSolverBody(int bodyIndex, b3RigidBodyCL* bodies,btInertiaCL* inertias);
+	void	initSolverBody(int bodyIndex, b3SolverBody* solverBody, b3RigidBodyCL* collisionObject);
+
+	void	resolveSingleConstraintRowGeneric(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
+
+	void	resolveSingleConstraintRowGenericSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
+	
+	void	resolveSingleConstraintRowLowerLimit(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
+	
+	void	resolveSingleConstraintRowLowerLimitSIMD(b3SolverBody& bodyA,b3SolverBody& bodyB,const b3SolverConstraint& contactConstraint);
+		
+protected:
+
+	virtual b3Scalar solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies, btInertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
+
+
+	virtual b3Scalar solveGroupCacheFriendlyIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
+	virtual void solveGroupCacheFriendlySplitImpulseIterations(b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
+	b3Scalar solveSingleIteration(int iteration, b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
+
+
+	virtual b3Scalar solveGroupCacheFriendlyFinish(b3RigidBodyCL* bodies, btInertiaCL* inertias,int numBodies,const b3ContactSolverInfo& infoGlobal);
+
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	
+	b3PgsJacobiSolver();
+	virtual ~b3PgsJacobiSolver();
+
+	void	solveContacts(int numBodies, b3RigidBodyCL* bodies, btInertiaCL* inertias, int numContacts, b3Contact4* contacts);
+	
+	b3Scalar solveGroup(b3RigidBodyCL* bodies,btInertiaCL* inertias,int numBodies,b3Contact4* manifoldPtr, int numManifolds,b3TypedConstraint** constraints,int numConstraints,const b3ContactSolverInfo& infoGlobal);
+
+	///clear internal cached data and reset random seed
+	virtual	void	reset();
+	
+	unsigned long btRand2();
+
+	int btRandInt2 (int n);
+
+	void	setRandSeed(unsigned long seed)
+	{
+		m_btSeed2 = seed;
+	}
+	unsigned long	getRandSeed() const
+	{
+		return m_btSeed2;
+	}
+
+
+
+
+};
+
+#endif //BT_PGS_JACOBI_SOLVER
+

src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h

@@ -0,0 +1,299 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_SOLVER_BODY_H
+#define BT_SOLVER_BODY_H
+
+class	btRigidBody;
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Matrix3x3.h"
+
+#include "Bullet3Common/b3AlignedAllocator.h"
+#include "Bullet3Common/b3TransformUtil.h"
+
+///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
+#ifdef BT_USE_SSE
+#define USE_SIMD 1
+#endif //
+
+
+#ifdef USE_SIMD
+
+struct	btSimdScalar
+{
+	SIMD_FORCE_INLINE	btSimdScalar()
+	{
+
+	}
+
+	SIMD_FORCE_INLINE	btSimdScalar(float	fl)
+	:m_vec128 (_mm_set1_ps(fl))
+	{
+	}
+
+	SIMD_FORCE_INLINE	btSimdScalar(__m128 v128)
+		:m_vec128(v128)
+	{
+	}
+	union
+	{
+		__m128		m_vec128;
+		float		m_floats[4];
+		int			m_ints[4];
+		b3Scalar	m_unusedPadding;
+	};
+	SIMD_FORCE_INLINE	__m128	get128()
+	{
+		return m_vec128;
+	}
+
+	SIMD_FORCE_INLINE	const __m128	get128() const
+	{
+		return m_vec128;
+	}
+
+	SIMD_FORCE_INLINE	void	set128(__m128 v128)
+	{
+		m_vec128 = v128;
+	}
+
+	SIMD_FORCE_INLINE	operator       __m128()       
+	{ 
+		return m_vec128; 
+	}
+	SIMD_FORCE_INLINE	operator const __m128() const 
+	{ 
+		return m_vec128; 
+	}
+	
+	SIMD_FORCE_INLINE	operator float() const 
+	{ 
+		return m_floats[0]; 
+	}
+
+};
+
+///@brief Return the elementwise product of two btSimdScalar
+SIMD_FORCE_INLINE btSimdScalar 
+operator*(const btSimdScalar& v1, const btSimdScalar& v2) 
+{
+	return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
+}
+
+///@brief Return the elementwise product of two btSimdScalar
+SIMD_FORCE_INLINE btSimdScalar 
+operator+(const btSimdScalar& v1, const btSimdScalar& v2) 
+{
+	return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
+}
+
+
+#else
+#define btSimdScalar b3Scalar
+#endif
+
+///The b3SolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
+ATTRIBUTE_ALIGNED64 (struct)	b3SolverBody
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+	b3Transform		m_worldTransform;
+	b3Vector3		m_deltaLinearVelocity;
+	b3Vector3		m_deltaAngularVelocity;
+	b3Vector3		m_angularFactor;
+	b3Vector3		m_linearFactor;
+	b3Vector3		m_invMass;
+	b3Vector3		m_pushVelocity;
+	b3Vector3		m_turnVelocity;
+	b3Vector3		m_linearVelocity;
+	b3Vector3		m_angularVelocity;
+
+	union 
+	{
+		void*	m_originalBody;
+		int		m_originalBodyIndex;
+	};
+
+
+	void	setWorldTransform(const b3Transform& worldTransform)
+	{
+		m_worldTransform = worldTransform;
+	}
+
+	const b3Transform& getWorldTransform() const
+	{
+		return m_worldTransform;
+	}
+	
+	SIMD_FORCE_INLINE void	getVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
+	{
+		if (m_originalBody)
+			velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
+		else
+			velocity.setValue(0,0,0);
+	}
+
+	SIMD_FORCE_INLINE void	getAngularVelocity(b3Vector3& angVel) const
+	{
+		if (m_originalBody)
+			angVel =m_angularVelocity+m_deltaAngularVelocity;
+		else
+			angVel.setValue(0,0,0);
+	}
+
+
+	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+	SIMD_FORCE_INLINE void applyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
+	{
+		if (m_originalBody)
+		{
+			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
+			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+
+	SIMD_FORCE_INLINE void internalApplyPushImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,b3Scalar impulseMagnitude)
+	{
+		if (m_originalBody)
+		{
+			m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
+			m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+
+
+
+	const b3Vector3& getDeltaLinearVelocity() const
+	{
+		return m_deltaLinearVelocity;
+	}
+
+	const b3Vector3& getDeltaAngularVelocity() const
+	{
+		return m_deltaAngularVelocity;
+	}
+
+	const b3Vector3& getPushVelocity() const 
+	{
+		return m_pushVelocity;
+	}
+
+	const b3Vector3& getTurnVelocity() const 
+	{
+		return m_turnVelocity;
+	}
+
+
+	////////////////////////////////////////////////
+	///some internal methods, don't use them
+		
+	b3Vector3& internalGetDeltaLinearVelocity()
+	{
+		return m_deltaLinearVelocity;
+	}
+
+	b3Vector3& internalGetDeltaAngularVelocity()
+	{
+		return m_deltaAngularVelocity;
+	}
+
+	const b3Vector3& internalGetAngularFactor() const
+	{
+		return m_angularFactor;
+	}
+
+	const b3Vector3& internalGetInvMass() const
+	{
+		return m_invMass;
+	}
+
+	void internalSetInvMass(const b3Vector3& invMass)
+	{
+		m_invMass = invMass;
+	}
+	
+	b3Vector3& internalGetPushVelocity()
+	{
+		return m_pushVelocity;
+	}
+
+	b3Vector3& internalGetTurnVelocity()
+	{
+		return m_turnVelocity;
+	}
+
+	SIMD_FORCE_INLINE void	internalGetVelocityInLocalPointObsolete(const b3Vector3& rel_pos, b3Vector3& velocity ) const
+	{
+		velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
+	}
+
+	SIMD_FORCE_INLINE void	internalGetAngularVelocity(b3Vector3& angVel) const
+	{
+		angVel = m_angularVelocity+m_deltaAngularVelocity;
+	}
+
+
+	//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+	SIMD_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
+	{
+		if (m_originalBody)
+		{
+			m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
+			m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+		}
+	}
+		
+	
+	
+
+	void	writebackVelocity()
+	{
+		if (m_originalBody)
+		{
+			m_linearVelocity +=m_deltaLinearVelocity;
+			m_angularVelocity += m_deltaAngularVelocity;
+			
+			//m_originalBody->setCompanionId(-1);
+		}
+	}
+
+
+	void	writebackVelocityAndTransform(b3Scalar timeStep, b3Scalar splitImpulseTurnErp)
+	{
+        (void) timeStep;
+		if (m_originalBody)
+		{
+			m_linearVelocity += m_deltaLinearVelocity;
+			m_angularVelocity += m_deltaAngularVelocity;
+			
+			//correct the position/orientation based on push/turn recovery
+			b3Transform newTransform;
+			if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
+			{
+			//	btQuaternion orn = m_worldTransform.getRotation();
+				b3TransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
+				m_worldTransform = newTransform;
+			}
+			//m_worldTransform.setRotation(orn);
+			//m_originalBody->setCompanionId(-1);
+		}
+	}
+	
+
+
+};
+
+#endif //BT_SOLVER_BODY_H
+
+

src/Bullet3Dynamics/ConstraintSolver/b3SolverConstraint.h

@@ -0,0 +1,79 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_SOLVER_CONSTRAINT_H
+#define BT_SOLVER_CONSTRAINT_H
+
+class	btRigidBody;
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Matrix3x3.h"
+//#include "btJacobianEntry.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+
+//#define NO_FRICTION_TANGENTIALS 1
+#include "b3SolverBody.h"
+
+
+///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
+ATTRIBUTE_ALIGNED16 (struct)	b3SolverConstraint
+{
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	b3Vector3		m_relpos1CrossNormal;
+	b3Vector3		m_contactNormal;
+
+	b3Vector3		m_relpos2CrossNormal;
+	//b3Vector3		m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal
+
+	b3Vector3		m_angularComponentA;
+	b3Vector3		m_angularComponentB;
+	
+	mutable btSimdScalar	m_appliedPushImpulse;
+	mutable btSimdScalar	m_appliedImpulse;
+
+	b3Scalar	m_friction;
+	b3Scalar	m_jacDiagABInv;
+	b3Scalar		m_rhs;
+	b3Scalar		m_cfm;
+	
+    b3Scalar		m_lowerLimit;
+	b3Scalar		m_upperLimit;
+	b3Scalar		m_rhsPenetration;
+    union
+	{
+		void*		m_originalContactPoint;
+		b3Scalar	m_unusedPadding4;
+	};
+
+	int	m_overrideNumSolverIterations;
+    int			m_frictionIndex;
+	int m_solverBodyIdA;
+	int m_solverBodyIdB;
+
+    
+	enum		btSolverConstraintType
+	{
+		BT_SOLVER_CONTACT_1D = 0,
+		BT_SOLVER_FRICTION_1D
+	};
+};
+
+typedef b3AlignedObjectArray<b3SolverConstraint>	btConstraintArray;
+
+
+#endif //BT_SOLVER_CONSTRAINT_H
+
+
+

src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.cpp

@@ -0,0 +1,161 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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 "b3TypedConstraint.h"
+//#include "Bullet3Common/btSerializer.h"
+
+
+#define DEFAULT_DEBUGDRAW_SIZE b3Scalar(0.3f)
+
+
+
+b3TypedConstraint::b3TypedConstraint(btTypedConstraintType type, int rbA,int rbB)
+:btTypedObject(type),
+m_userConstraintType(-1),
+m_userConstraintId(-1),
+m_breakingImpulseThreshold(SIMD_INFINITY),
+m_isEnabled(true),
+m_needsFeedback(false),
+m_overrideNumSolverIterations(-1),
+m_rbA(rbA),
+m_rbB(rbB),
+m_appliedImpulse(b3Scalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
+m_jointFeedback(0)
+{
+}
+
+
+
+
+b3Scalar b3TypedConstraint::getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact)
+{
+	if(lowLim > uppLim)
+	{
+		return b3Scalar(1.0f);
+	}
+	else if(lowLim == uppLim)
+	{
+		return b3Scalar(0.0f);
+	}
+	b3Scalar lim_fact = b3Scalar(1.0f);
+	b3Scalar delta_max = vel / timeFact;
+	if(delta_max < b3Scalar(0.0f))
+	{
+		if((pos >= lowLim) && (pos < (lowLim - delta_max)))
+		{
+			lim_fact = (lowLim - pos) / delta_max;
+		}
+		else if(pos  < lowLim)
+		{
+			lim_fact = b3Scalar(0.0f);
+		}
+		else
+		{
+			lim_fact = b3Scalar(1.0f);
+		}
+	}
+	else if(delta_max > b3Scalar(0.0f))
+	{
+		if((pos <= uppLim) && (pos > (uppLim - delta_max)))
+		{
+			lim_fact = (uppLim - pos) / delta_max;
+		}
+		else if(pos  > uppLim)
+		{
+			lim_fact = b3Scalar(0.0f);
+		}
+		else
+		{
+			lim_fact = b3Scalar(1.0f);
+		}
+	}
+	else
+	{
+			lim_fact = b3Scalar(0.0f);
+	}
+	return lim_fact;
+}
+
+
+
+void btAngularLimit::set(b3Scalar low, b3Scalar high, b3Scalar _softness, b3Scalar _biasFactor, b3Scalar _relaxationFactor)
+{
+	m_halfRange = (high - low) / 2.0f;
+	m_center = btNormalizeAngle(low + m_halfRange);
+	m_softness =  _softness;
+	m_biasFactor = _biasFactor;
+	m_relaxationFactor = _relaxationFactor;
+}
+
+void btAngularLimit::test(const b3Scalar angle)
+{
+	m_correction = 0.0f;
+	m_sign = 0.0f;
+	m_solveLimit = false;
+
+	if (m_halfRange >= 0.0f)
+	{
+		b3Scalar deviation = btNormalizeAngle(angle - m_center);
+		if (deviation < -m_halfRange)
+		{
+			m_solveLimit = true;
+			m_correction = - (deviation + m_halfRange);
+			m_sign = +1.0f;
+		}
+		else if (deviation > m_halfRange)
+		{
+			m_solveLimit = true;
+			m_correction = m_halfRange - deviation;
+			m_sign = -1.0f;
+		}
+	}
+}
+
+
+b3Scalar btAngularLimit::getError() const
+{
+	return m_correction * m_sign;
+}
+
+void btAngularLimit::fit(b3Scalar& angle) const
+{
+	if (m_halfRange > 0.0f)
+	{
+		b3Scalar relativeAngle = btNormalizeAngle(angle - m_center);
+		if (!btEqual(relativeAngle, m_halfRange))
+		{
+			if (relativeAngle > 0.0f)
+			{
+				angle = getHigh();
+			}
+			else
+			{
+				angle = getLow();
+			}
+		}
+	}
+}
+
+b3Scalar btAngularLimit::getLow() const
+{
+	return btNormalizeAngle(m_center - m_halfRange);
+}
+
+b3Scalar btAngularLimit::getHigh() const
+{
+	return btNormalizeAngle(m_center + m_halfRange);
+}

src/Bullet3Dynamics/ConstraintSolver/b3TypedConstraint.h

@@ -0,0 +1,481 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2003-2010 Erwin Coumans  http://continuousphysics.com/Bullet/
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as 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_TYPED_CONSTRAINT_H
+#define BT_TYPED_CONSTRAINT_H
+
+
+#include "Bullet3Common/b3Scalar.h"
+#include "b3SolverConstraint.h"
+
+class btSerializer;
+
+//Don't change any of the existing enum values, so add enum types at the end for serialization compatibility
+enum btTypedConstraintType
+{
+	POINT2POINT_CONSTRAINT_TYPE=3,
+	HINGE_CONSTRAINT_TYPE,
+	CONETWIST_CONSTRAINT_TYPE,
+	D6_CONSTRAINT_TYPE,
+	SLIDER_CONSTRAINT_TYPE,
+	CONTACT_CONSTRAINT_TYPE,
+	D6_SPRING_CONSTRAINT_TYPE,
+	GEAR_CONSTRAINT_TYPE,
+	MAX_CONSTRAINT_TYPE
+};
+
+
+enum btConstraintParams
+{
+	BT_CONSTRAINT_ERP=1,
+	BT_CONSTRAINT_STOP_ERP,
+	BT_CONSTRAINT_CFM,
+	BT_CONSTRAINT_STOP_CFM
+};
+
+#if 1
+	#define btAssertConstrParams(_par) btAssert(_par) 
+#else
+	#define btAssertConstrParams(_par)
+#endif
+
+
+ATTRIBUTE_ALIGNED16(struct)	btJointFeedback
+{
+	b3Vector3	m_appliedForceBodyA;
+	b3Vector3	m_appliedTorqueBodyA;
+	b3Vector3	m_appliedForceBodyB;
+	b3Vector3	m_appliedTorqueBodyB;
+};
+
+
+///TypedConstraint is the baseclass for Bullet constraints and vehicles
+ATTRIBUTE_ALIGNED16(class) b3TypedConstraint : public btTypedObject
+{
+	int	m_userConstraintType;
+
+	union
+	{
+		int	m_userConstraintId;
+		void* m_userConstraintPtr;
+	};
+
+	b3Scalar	m_breakingImpulseThreshold;
+	bool		m_isEnabled;
+	bool		m_needsFeedback;
+	int			m_overrideNumSolverIterations;
+
+
+	b3TypedConstraint&	operator=(b3TypedConstraint&	other)
+	{
+		btAssert(0);
+		(void) other;
+		return *this;
+	}
+
+protected:
+	int				m_rbA;
+	int				m_rbB;
+	b3Scalar	m_appliedImpulse;
+	b3Scalar	m_dbgDrawSize;
+	btJointFeedback*	m_jointFeedback;
+
+	///internal method used by the constraint solver, don't use them directly
+	b3Scalar getMotorFactor(b3Scalar pos, b3Scalar lowLim, b3Scalar uppLim, b3Scalar vel, b3Scalar timeFact);
+	
+
+public:
+
+	BT_DECLARE_ALIGNED_ALLOCATOR();
+
+	virtual ~b3TypedConstraint() {};
+	b3TypedConstraint(btTypedConstraintType type, int bodyA,int bodyB);
+
+	struct btConstraintInfo1 {
+		int m_numConstraintRows,nub;
+	};
+
+	static btRigidBody& getFixedBody();
+
+	struct btConstraintInfo2 {
+		// integrator parameters: frames per second (1/stepsize), default error
+		// reduction parameter (0..1).
+		b3Scalar fps,erp;
+
+		// for the first and second body, pointers to two (linear and angular)
+		// n*3 jacobian sub matrices, stored by rows. these matrices will have
+		// been initialized to 0 on entry. if the second body is zero then the
+		// J2xx pointers may be 0.
+		b3Scalar *m_J1linearAxis,*m_J1angularAxis,*m_J2linearAxis,*m_J2angularAxis;
+
+		// elements to jump from one row to the next in J's
+		int rowskip;
+
+		// right hand sides of the equation J*v = c + cfm * lambda. cfm is the
+		// "constraint force mixing" vector. c is set to zero on entry, cfm is
+		// set to a constant value (typically very small or zero) value on entry.
+		b3Scalar *m_constraintError,*cfm;
+
+		// lo and hi limits for variables (set to -/+ infinity on entry).
+		b3Scalar *m_lowerLimit,*m_upperLimit;
+
+		// findex vector for variables. see the LCP solver interface for a
+		// description of what this does. this is set to -1 on entry.
+		// note that the returned indexes are relative to the first index of
+		// the constraint.
+		int *findex;
+		// number of solver iterations
+		int m_numIterations;
+
+		//damping of the velocity
+		b3Scalar	m_damping;
+	};
+
+	int	getOverrideNumSolverIterations() const
+	{
+		return m_overrideNumSolverIterations;
+	}
+
+	///override the number of constraint solver iterations used to solve this constraint
+	///-1 will use the default number of iterations, as specified in SolverInfo.m_numIterations
+	void setOverrideNumSolverIterations(int overideNumIterations)
+	{
+		m_overrideNumSolverIterations = overideNumIterations;
+	}
+
+	///internal method used by the constraint solver, don't use them directly
+	virtual void	buildJacobian() {};
+
+	///internal method used by the constraint solver, don't use them directly
+	virtual	void	setupSolverConstraint(btConstraintArray& ca, int solverBodyA,int solverBodyB, b3Scalar timeStep)
+	{
+        (void)ca;
+        (void)solverBodyA;
+        (void)solverBodyB;
+        (void)timeStep;
+	}
+	
+	///internal method used by the constraint solver, don't use them directly
+	virtual void getInfo1 (btConstraintInfo1* info)=0;
+
+	///internal method used by the constraint solver, don't use them directly
+	virtual void getInfo2 (btConstraintInfo2* info)=0;
+
+	///internal method used by the constraint solver, don't use them directly
+	void	internalSetAppliedImpulse(b3Scalar appliedImpulse)
+	{
+		m_appliedImpulse = appliedImpulse;
+	}
+	///internal method used by the constraint solver, don't use them directly
+	b3Scalar	internalGetAppliedImpulse()
+	{
+		return m_appliedImpulse;
+	}
+
+
+	b3Scalar	getBreakingImpulseThreshold() const
+	{
+		return 	m_breakingImpulseThreshold;
+	}
+
+	void	setBreakingImpulseThreshold(b3Scalar threshold)
+	{
+		m_breakingImpulseThreshold = threshold;
+	}
+
+	bool	isEnabled() const
+	{
+		return m_isEnabled;
+	}
+
+	void	setEnabled(bool enabled)
+	{
+		m_isEnabled=enabled;
+	}
+
+
+	///internal method used by the constraint solver, don't use them directly
+	virtual	void	solveConstraintObsolete(b3SolverBody& /*bodyA*/,b3SolverBody& /*bodyB*/,b3Scalar	/*timeStep*/) {};
+
+	
+	int getRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	int getRigidBodyB() const
+	{
+		return m_rbB;
+	}
+
+
+	int getRigidBodyA() 
+	{
+		return m_rbA;
+	}
+	int getRigidBodyB()
+	{
+		return m_rbB;
+	}
+
+	int getUserConstraintType() const
+	{
+		return m_userConstraintType ;
+	}
+
+	void	setUserConstraintType(int userConstraintType)
+	{
+		m_userConstraintType = userConstraintType;
+	};
+
+	void	setUserConstraintId(int uid)
+	{
+		m_userConstraintId = uid;
+	}
+
+	int getUserConstraintId() const
+	{
+		return m_userConstraintId;
+	}
+
+	void	setUserConstraintPtr(void* ptr)
+	{
+		m_userConstraintPtr = ptr;
+	}
+
+	void*	getUserConstraintPtr()
+	{
+		return m_userConstraintPtr;
+	}
+
+	void	setJointFeedback(btJointFeedback* jointFeedback)
+	{
+		m_jointFeedback = jointFeedback;
+	}
+
+	const btJointFeedback* getJointFeedback() const
+	{
+		return m_jointFeedback;
+	}
+
+	btJointFeedback* getJointFeedback()
+	{
+		return m_jointFeedback;
+	}
+
+
+	int getUid() const
+	{
+		return m_userConstraintId;   
+	} 
+
+	bool	needsFeedback() const
+	{
+		return m_needsFeedback;
+	}
+
+	///enableFeedback will allow to read the applied linear and angular impulse
+	///use getAppliedImpulse, getAppliedLinearImpulse and getAppliedAngularImpulse to read feedback information
+	void	enableFeedback(bool needsFeedback)
+	{
+		m_needsFeedback = needsFeedback;
+	}
+
+	///getAppliedImpulse is an estimated total applied impulse. 
+	///This feedback could be used to determine breaking constraints or playing sounds.
+	b3Scalar	getAppliedImpulse() const
+	{
+		btAssert(m_needsFeedback);
+		return m_appliedImpulse;
+	}
+
+	btTypedConstraintType getConstraintType () const
+	{
+		return btTypedConstraintType(m_objectType);
+	}
+	
+	void setDbgDrawSize(b3Scalar dbgDrawSize)
+	{
+		m_dbgDrawSize = dbgDrawSize;
+	}
+	b3Scalar getDbgDrawSize()
+	{
+		return m_dbgDrawSize;
+	}
+
+	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5). 
+	///If no axis is provided, it uses the default axis for this constraint.
+	virtual	void	setParam(int num, b3Scalar value, int axis = -1) = 0;
+
+	///return the local value of parameter
+	virtual	b3Scalar getParam(int num, int axis = -1) const = 0;
+	
+	virtual	int	calculateSerializeBufferSize() const;
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+	virtual	const char*	serialize(void* dataBuffer, btSerializer* serializer) const;
+
+};
+
+// returns angle in range [-SIMD_2_PI, SIMD_2_PI], closest to one of the limits 
+// all arguments should be normalized angles (i.e. in range [-SIMD_PI, SIMD_PI])
+SIMD_FORCE_INLINE b3Scalar btAdjustAngleToLimits(b3Scalar angleInRadians, b3Scalar angleLowerLimitInRadians, b3Scalar angleUpperLimitInRadians)
+{
+	if(angleLowerLimitInRadians >= angleUpperLimitInRadians)
+	{
+		return angleInRadians;
+	}
+	else if(angleInRadians < angleLowerLimitInRadians)
+	{
+		b3Scalar diffLo = btFabs(btNormalizeAngle(angleLowerLimitInRadians - angleInRadians));
+		b3Scalar diffHi = btFabs(btNormalizeAngle(angleUpperLimitInRadians - angleInRadians));
+		return (diffLo < diffHi) ? angleInRadians : (angleInRadians + SIMD_2_PI);
+	}
+	else if(angleInRadians > angleUpperLimitInRadians)
+	{
+		b3Scalar diffHi = btFabs(btNormalizeAngle(angleInRadians - angleUpperLimitInRadians));
+		b3Scalar diffLo = btFabs(btNormalizeAngle(angleInRadians - angleLowerLimitInRadians));
+		return (diffLo < diffHi) ? (angleInRadians - SIMD_2_PI) : angleInRadians;
+	}
+	else
+	{
+		return angleInRadians;
+	}
+}
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btTypedConstraintData
+{
+	int		m_bodyA;
+	int		m_bodyB;
+	char	*m_name;
+
+	int	m_objectType;
+	int	m_userConstraintType;
+	int	m_userConstraintId;
+	int	m_needsFeedback;
+
+	float	m_appliedImpulse;
+	float	m_dbgDrawSize;
+
+	int	m_disableCollisionsBetweenLinkedBodies;
+	int	m_overrideNumSolverIterations;
+
+	float	m_breakingImpulseThreshold;
+	int		m_isEnabled;
+	
+};
+
+SIMD_FORCE_INLINE	int	b3TypedConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btTypedConstraintData);
+}
+
+
+
+class btAngularLimit
+{
+private:
+	b3Scalar 
+		m_center,
+		m_halfRange,
+		m_softness,
+		m_biasFactor,
+		m_relaxationFactor,
+		m_correction,
+		m_sign;
+
+	bool
+		m_solveLimit;
+
+public:
+	/// Default constructor initializes limit as inactive, allowing free constraint movement
+	btAngularLimit()
+		:m_center(0.0f),
+		m_halfRange(-1.0f),
+		m_softness(0.9f),
+		m_biasFactor(0.3f),
+		m_relaxationFactor(1.0f),
+		m_correction(0.0f),
+		m_sign(0.0f),
+		m_solveLimit(false)
+	{}
+
+	/// Sets all limit's parameters.
+	/// When low > high limit becomes inactive.
+	/// When high - low > 2PI limit is ineffective too becouse no angle can exceed the limit
+	void set(b3Scalar low, b3Scalar high, b3Scalar _softness = 0.9f, b3Scalar _biasFactor = 0.3f, b3Scalar _relaxationFactor = 1.0f);
+
+	/// Checks conastaint angle against limit. If limit is active and the angle violates the limit
+	/// correction is calculated.
+	void test(const b3Scalar angle);
+
+	/// Returns limit's softness
+	inline b3Scalar getSoftness() const
+	{
+		return m_softness;
+	}
+
+	/// Returns limit's bias factor
+	inline b3Scalar getBiasFactor() const
+	{
+		return m_biasFactor;
+	}
+
+	/// Returns limit's relaxation factor
+	inline b3Scalar getRelaxationFactor() const
+	{
+		return m_relaxationFactor;
+	}
+
+	/// Returns correction value evaluated when test() was invoked 
+	inline b3Scalar getCorrection() const
+	{
+		return m_correction;
+	}
+
+	/// Returns sign value evaluated when test() was invoked 
+	inline b3Scalar getSign() const
+	{
+		return m_sign;
+	}
+
+	/// Gives half of the distance between min and max limit angle
+	inline b3Scalar getHalfRange() const
+	{
+		return m_halfRange;
+	}
+
+	/// Returns true when the last test() invocation recognized limit violation
+	inline bool isLimit() const
+	{
+		return m_solveLimit;
+	}
+
+	/// Checks given angle against limit. If limit is active and angle doesn't fit it, the angle
+	/// returned is modified so it equals to the limit closest to given angle.
+	void fit(b3Scalar& angle) const;
+
+	/// Returns correction value multiplied by sign value
+	b3Scalar getError() const;
+
+	b3Scalar getLow() const;
+
+	b3Scalar getHigh() const;
+
+};
+
+
+
+#endif //BT_TYPED_CONSTRAINT_H

src/Bullet3Dynamics/premake4.lua

@@ -0,0 +1,15 @@
+	project "Bullet3Dynamics"
+
+	language "C++"
+				
+	kind "StaticLib"
+
+	includedirs {
+		".."
+	}		
+	targetdir "../../bin"
+
+	files {
+		"**.cpp",
+		"**.h"
+	}

src/Bullet3Geometry/premake4.lua

@@ -0,0 +1,13 @@
+	project "Bullet3Geometry"
+
+	language "C++"
+				
+	kind "StaticLib"
+		
+	includedirs {".."}
+	targetdir "../../bin"
+
+	files {
+		"**.cpp",
+		"**.h"
+	}