ruby-bullet 0.0.2-x86-mingw32

data/deps/include/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h

@@ -0,0 +1,346 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+
+Written by: Marcus Hennix
+*/
+
+
+
+/*
+Overview:
+
+btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc).
+It is a fixed translation, 3 degree-of-freedom (DOF) rotational "joint".
+It divides the 3 rotational DOFs into swing (movement within a cone) and twist.
+Swing is divided into swing1 and swing2 which can have different limits, giving an elliptical shape.
+(Note: the cone's base isn't flat, so this ellipse is "embedded" on the surface of a sphere.)
+
+In the contraint's frame of reference:
+twist is along the x-axis,
+and swing 1 and 2 are along the z and y axes respectively.
+*/
+
+
+
+#ifndef BT_CONETWISTCONSTRAINT_H
+#define BT_CONETWISTCONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+
+class btRigidBody;
+
+enum btConeTwistFlags
+{
+	BT_CONETWIST_FLAGS_LIN_CFM = 1,
+	BT_CONETWIST_FLAGS_LIN_ERP = 2,
+	BT_CONETWIST_FLAGS_ANG_CFM = 4
+};
+
+///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc)
+class btConeTwistConstraint : public btTypedConstraint
+{
+#ifdef IN_PARALLELL_SOLVER
+public:
+#endif
+	btJacobianEntry	m_jac[3]; //3 orthogonal linear constraints
+
+	btTransform m_rbAFrame; 
+	btTransform m_rbBFrame;
+
+	btScalar	m_limitSoftness;
+	btScalar	m_biasFactor;
+	btScalar	m_relaxationFactor;
+
+	btScalar	m_damping;
+
+	btScalar	m_swingSpan1;
+	btScalar	m_swingSpan2;
+	btScalar	m_twistSpan;
+
+	btScalar	m_fixThresh;
+
+	btVector3   m_swingAxis;
+	btVector3	m_twistAxis;
+
+	btScalar	m_kSwing;
+	btScalar	m_kTwist;
+
+	btScalar	m_twistLimitSign;
+	btScalar	m_swingCorrection;
+	btScalar	m_twistCorrection;
+
+	btScalar	m_twistAngle;
+
+	btScalar	m_accSwingLimitImpulse;
+	btScalar	m_accTwistLimitImpulse;
+
+	bool		m_angularOnly;
+	bool		m_solveTwistLimit;
+	bool		m_solveSwingLimit;
+
+	bool	m_useSolveConstraintObsolete;
+
+	// not yet used...
+	btScalar	m_swingLimitRatio;
+	btScalar	m_twistLimitRatio;
+	btVector3   m_twistAxisA;
+
+	// motor
+	bool		 m_bMotorEnabled;
+	bool		 m_bNormalizedMotorStrength;
+	btQuaternion m_qTarget;
+	btScalar	 m_maxMotorImpulse;
+	btVector3	 m_accMotorImpulse;
+	
+	// parameters
+	int			m_flags;
+	btScalar	m_linCFM;
+	btScalar	m_linERP;
+	btScalar	m_angCFM;
+	
+protected:
+
+	void init();
+
+	void computeConeLimitInfo(const btQuaternion& qCone, // in
+		btScalar& swingAngle, btVector3& vSwingAxis, btScalar& swingLimit); // all outs
+
+	void computeTwistLimitInfo(const btQuaternion& qTwist, // in
+		btScalar& twistAngle, btVector3& vTwistAxis); // all outs
+
+	void adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const;
+
+
+public:
+
+	btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame);
+	
+	btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame);
+
+	virtual void	buildJacobian();
+
+	virtual void getInfo1 (btConstraintInfo1* info);
+
+	void	getInfo1NonVirtual(btConstraintInfo1* info);
+	
+	virtual void getInfo2 (btConstraintInfo2* info);
+	
+	void	getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
+
+	virtual	void	solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar	timeStep);
+
+	void	updateRHS(btScalar	timeStep);
+
+
+	const btRigidBody& getRigidBodyA() const
+	{
+		return m_rbA;
+	}
+	const btRigidBody& getRigidBodyB() const
+	{
+		return m_rbB;
+	}
+
+	void	setAngularOnly(bool angularOnly)
+	{
+		m_angularOnly = angularOnly;
+	}
+
+	void	setLimit(int limitIndex,btScalar limitValue)
+	{
+		switch (limitIndex)
+		{
+		case 3:
+			{
+				m_twistSpan = limitValue;
+				break;
+			}
+		case 4:
+			{
+				m_swingSpan2 = limitValue;
+				break;
+			}
+		case 5:
+			{
+				m_swingSpan1 = limitValue;
+				break;
+			}
+		default:
+			{
+			}
+		};
+	}
+
+	// setLimit(), a few notes:
+	// _softness:
+	//		0->1, recommend ~0.8->1.
+	//		describes % of limits where movement is free.
+	//		beyond this softness %, the limit is gradually enforced until the "hard" (1.0) limit is reached.
+	// _biasFactor:
+	//		0->1?, recommend 0.3 +/-0.3 or so.
+	//		strength with which constraint resists zeroth order (angular, not angular velocity) limit violation.
+	// __relaxationFactor:
+	//		0->1, recommend to stay near 1.
+	//		the lower the value, the less the constraint will fight velocities which violate the angular limits.
+	void	setLimit(btScalar _swingSpan1,btScalar _swingSpan2,btScalar _twistSpan, btScalar _softness = 1.f, btScalar _biasFactor = 0.3f, btScalar _relaxationFactor = 1.0f)
+	{
+		m_swingSpan1 = _swingSpan1;
+		m_swingSpan2 = _swingSpan2;
+		m_twistSpan  = _twistSpan;
+
+		m_limitSoftness =  _softness;
+		m_biasFactor = _biasFactor;
+		m_relaxationFactor = _relaxationFactor;
+	}
+
+	const btTransform& getAFrame() { return m_rbAFrame; };	
+	const btTransform& getBFrame() { return m_rbBFrame; };
+
+	inline int getSolveTwistLimit()
+	{
+		return m_solveTwistLimit;
+	}
+
+	inline int getSolveSwingLimit()
+	{
+		return m_solveTwistLimit;
+	}
+
+	inline btScalar getTwistLimitSign()
+	{
+		return m_twistLimitSign;
+	}
+
+	void calcAngleInfo();
+	void calcAngleInfo2(const btTransform& transA, const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB);
+
+	inline btScalar getSwingSpan1()
+	{
+		return m_swingSpan1;
+	}
+	inline btScalar getSwingSpan2()
+	{
+		return m_swingSpan2;
+	}
+	inline btScalar getTwistSpan()
+	{
+		return m_twistSpan;
+	}
+	inline btScalar getTwistAngle()
+	{
+		return m_twistAngle;
+	}
+	bool isPastSwingLimit() { return m_solveSwingLimit; }
+
+	void setDamping(btScalar damping) { m_damping = damping; }
+
+	void enableMotor(bool b) { m_bMotorEnabled = b; }
+	void setMaxMotorImpulse(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = false; }
+	void setMaxMotorImpulseNormalized(btScalar maxMotorImpulse) { m_maxMotorImpulse = maxMotorImpulse; m_bNormalizedMotorStrength = true; }
+
+	btScalar getFixThresh() { return m_fixThresh; }
+	void setFixThresh(btScalar fixThresh) { m_fixThresh = fixThresh; }
+
+	// setMotorTarget:
+	// q: the desired rotation of bodyA wrt bodyB.
+	// note: if q violates the joint limits, the internal target is clamped to avoid conflicting impulses (very bad for stability)
+	// note: don't forget to enableMotor()
+	void setMotorTarget(const btQuaternion &q);
+
+	// same as above, but q is the desired rotation of frameA wrt frameB in constraint space
+	void setMotorTargetInConstraintSpace(const btQuaternion &q);
+
+	btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const;
+
+	///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, btScalar value, int axis = -1);
+
+	virtual void setFrames(const btTransform& frameA, const btTransform& frameB);
+
+	const btTransform& getFrameOffsetA() const
+	{
+		return m_rbAFrame;
+	}
+
+	const btTransform& getFrameOffsetB() const
+	{
+		return m_rbBFrame;
+	}
+
+
+	///return the local value of parameter
+	virtual	btScalar getParam(int num, int axis = -1) const;
+
+	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;
+
+};
+
+///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
+struct	btConeTwistConstraintData
+{
+	btTypedConstraintData	m_typeConstraintData;
+	btTransformFloatData m_rbAFrame;
+	btTransformFloatData m_rbBFrame;
+
+	//limits
+	float	m_swingSpan1;
+	float	m_swingSpan2;
+	float	m_twistSpan;
+	float	m_limitSoftness;
+	float	m_biasFactor;
+	float	m_relaxationFactor;
+
+	float	m_damping;
+		
+	char m_pad[4];
+
+};
+	
+
+
+SIMD_FORCE_INLINE int	btConeTwistConstraint::calculateSerializeBufferSize() const
+{
+	return sizeof(btConeTwistConstraintData);
+
+}
+
+
+	///fills the dataBuffer and returns the struct name (and 0 on failure)
+SIMD_FORCE_INLINE const char*	btConeTwistConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btConeTwistConstraintData* cone = (btConeTwistConstraintData*) dataBuffer;
+	btTypedConstraint::serialize(&cone->m_typeConstraintData,serializer);
+
+	m_rbAFrame.serializeFloat(cone->m_rbAFrame);
+	m_rbBFrame.serializeFloat(cone->m_rbBFrame);
+	
+	cone->m_swingSpan1 = float(m_swingSpan1);
+	cone->m_swingSpan2 = float(m_swingSpan2);
+	cone->m_twistSpan = float(m_twistSpan);
+	cone->m_limitSoftness = float(m_limitSoftness);
+	cone->m_biasFactor = float(m_biasFactor);
+	cone->m_relaxationFactor = float(m_relaxationFactor);
+	cone->m_damping = float(m_damping);
+
+	return "btConeTwistConstraintData";
+}
+
+
+#endif //BT_CONETWISTCONSTRAINT_H

data/deps/include/bullet/BulletDynamics/ConstraintSolver/btConstraintSolver.h

@@ -0,0 +1,52 @@
+/*
+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_CONSTRAINT_SOLVER_H
+#define BT_CONSTRAINT_SOLVER_H
+
+#include "LinearMath/btScalar.h"
+
+class btPersistentManifold;
+class btRigidBody;
+class btCollisionObject;
+class btTypedConstraint;
+struct btContactSolverInfo;
+struct btBroadphaseProxy;
+class btIDebugDraw;
+class btStackAlloc;
+class	btDispatcher;
+/// btConstraintSolver provides solver interface
+class btConstraintSolver
+{
+
+public:
+
+	virtual ~btConstraintSolver() {}
+	
+	virtual void prepareSolve (int /* numBodies */, int /* numManifolds */) {;}
+
+	///solve a group of constraints
+	virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints, const btContactSolverInfo& info,class btIDebugDraw* debugDrawer, btStackAlloc* stackAlloc,btDispatcher* dispatcher) = 0;
+
+	virtual void allSolved (const btContactSolverInfo& /* info */,class btIDebugDraw* /* debugDrawer */, btStackAlloc* /* stackAlloc */) {;}
+
+	///clear internal cached data and reset random seed
+	virtual	void	reset() = 0;
+};
+
+
+
+
+#endif //BT_CONSTRAINT_SOLVER_H

data/deps/include/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.cpp

@@ -0,0 +1,178 @@
+/*
+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 "btContactConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btContactSolverInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+
+
+
+btContactConstraint::btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB)
+:btTypedConstraint(CONTACT_CONSTRAINT_TYPE,rbA,rbB),
+	m_contactManifold(*contactManifold)
+{
+
+}
+
+btContactConstraint::~btContactConstraint()
+{
+
+}
+
+void	btContactConstraint::setContactManifold(btPersistentManifold* contactManifold)
+{
+	m_contactManifold = *contactManifold;
+}
+
+void btContactConstraint::getInfo1 (btConstraintInfo1* info)
+{
+
+}
+
+void btContactConstraint::getInfo2 (btConstraintInfo2* info)
+{
+
+}
+
+void	btContactConstraint::buildJacobian()
+{
+
+}
+
+
+
+
+
+#include "btContactConstraint.h"
+#include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btContactSolverInfo.h"
+#include "LinearMath/btMinMax.h"
+#include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h"
+
+
+
+//response  between two dynamic objects without friction, assuming 0 penetration depth
+btScalar resolveSingleCollision(
+        btRigidBody* body1,
+        btCollisionObject* colObj2,
+		const btVector3& contactPositionWorld,
+		const btVector3& contactNormalOnB,
+        const btContactSolverInfo& solverInfo,
+		btScalar distance)
+{
+	btRigidBody* body2 = btRigidBody::upcast(colObj2);
+    
+	
+    const btVector3& normal = contactNormalOnB;
+
+    btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin(); 
+    btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin();
+    
+    btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1);
+	btVector3 vel2 = body2? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0);
+    btVector3 vel = vel1 - vel2;
+    btScalar rel_vel;
+    rel_vel = normal.dot(vel);
+    
+    btScalar combinedRestitution = body1->getRestitution() * colObj2->getRestitution();
+    btScalar restitution = combinedRestitution* -rel_vel;
+
+    btScalar positionalError = solverInfo.m_erp *-distance /solverInfo.m_timeStep ;
+    btScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping;
+	btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld,normal);
+	btScalar denom1 = body2? body2->computeImpulseDenominator(contactPositionWorld,normal) : 0.f;
+	btScalar relaxation = 1.f;
+	btScalar jacDiagABInv = relaxation/(denom0+denom1);
+
+    btScalar penetrationImpulse = positionalError * jacDiagABInv;
+    btScalar velocityImpulse = velocityError * jacDiagABInv;
+
+    btScalar normalImpulse = penetrationImpulse+velocityImpulse;
+    normalImpulse = 0.f > normalImpulse ? 0.f: normalImpulse;
+
+	body1->applyImpulse(normal*(normalImpulse), rel_pos1);
+    if (body2)
+		body2->applyImpulse(-normal*(normalImpulse), rel_pos2);
+    
+    return normalImpulse;
+}
+
+
+//bilateral constraint between two dynamic objects
+void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
+                      btRigidBody& body2, const btVector3& pos2,
+                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep)
+{
+	(void)timeStep;
+	(void)distance;
+
+
+	btScalar normalLenSqr = normal.length2();
+	btAssert(btFabs(normalLenSqr) < btScalar(1.1));
+	if (normalLenSqr > btScalar(1.1))
+	{
+		impulse = btScalar(0.);
+		return;
+	}
+	btVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
+	btVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
+	//this jacobian entry could be re-used for all iterations
+	
+	btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
+	btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
+	btVector3 vel = vel1 - vel2;
+	
+
+	   btJacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
+		body2.getCenterOfMassTransform().getBasis().transpose(),
+		rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
+		body2.getInvInertiaDiagLocal(),body2.getInvMass());
+
+	btScalar jacDiagAB = jac.getDiagonal();
+	btScalar jacDiagABInv = btScalar(1.) / jacDiagAB;
+	
+	  btScalar rel_vel = jac.getRelativeVelocity(
+		body1.getLinearVelocity(),
+		body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
+		body2.getLinearVelocity(),
+		body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
+	btScalar a;
+	a=jacDiagABInv;
+
+
+	rel_vel = normal.dot(vel);
+	
+	//todo: move this into proper structure
+	btScalar contactDamping = btScalar(0.2);
+
+#ifdef ONLY_USE_LINEAR_MASS
+	btScalar massTerm = btScalar(1.) / (body1.getInvMass() + body2.getInvMass());
+	impulse = - contactDamping * rel_vel * massTerm;
+#else	
+	btScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
+	impulse = velocityImpulse;
+#endif
+}
+
+
+
+

data/deps/include/bullet/BulletDynamics/ConstraintSolver/btContactConstraint.h

@@ -0,0 +1,71 @@
+/*
+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_CONSTRAINT_H
+#define BT_CONTACT_CONSTRAINT_H
+
+#include "LinearMath/btVector3.h"
+#include "btJacobianEntry.h"
+#include "btTypedConstraint.h"
+#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
+
+///btContactConstraint can be automatically created to solve contact constraints using the unified btTypedConstraint interface
+ATTRIBUTE_ALIGNED16(class) btContactConstraint : public btTypedConstraint
+{
+protected:
+
+	btPersistentManifold m_contactManifold;
+
+public:
+
+
+	btContactConstraint(btPersistentManifold* contactManifold,btRigidBody& rbA,btRigidBody& rbB);
+
+	void	setContactManifold(btPersistentManifold* contactManifold);
+
+	btPersistentManifold* getContactManifold()
+	{
+		return &m_contactManifold;
+	}
+
+	const btPersistentManifold* getContactManifold() const
+	{
+		return &m_contactManifold;
+	}
+
+	virtual ~btContactConstraint();
+
+	virtual void getInfo1 (btConstraintInfo1* info);
+
+	virtual void getInfo2 (btConstraintInfo2* info);
+
+	///obsolete methods
+	virtual void	buildJacobian();
+
+
+};
+
+///very basic collision resolution without friction
+btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld,const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo,btScalar distance);
+
+
+///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects
+void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1,
+                      btRigidBody& body2, const btVector3& pos2,
+                      btScalar distance, const btVector3& normal,btScalar& impulse ,btScalar timeStep);
+
+
+
+#endif //BT_CONTACT_CONSTRAINT_H