1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 
16 #include "btRigidBody.h"
17 #include "BulletCollision/CollisionShapes/btConvexShape.h"
18 #include "LinearMath/btMinMax.h"
19 #include "LinearMath/btTransformUtil.h"
20 #include "LinearMath/btMotionState.h"
21 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
22 #include "LinearMath/btSerializer.h"
23 
24 //'temporarily' global variables
25 btScalar gDeactivationTime = btScalar(2.);
26 bool gDisableDeactivation = false;
27 static int uniqueId = 0;
28 
btRigidBody(const btRigidBody::btRigidBodyConstructionInfo & constructionInfo)29 btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
30 {
31 	setupRigidBody(constructionInfo);
32 }
33 
btRigidBody(btScalar mass,btMotionState * motionState,btCollisionShape * collisionShape,const btVector3 & localInertia)34 btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia)
35 {
36 	btRigidBodyConstructionInfo cinfo(mass, motionState, collisionShape, localInertia);
37 	setupRigidBody(cinfo);
38 }
39 
setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo & constructionInfo)40 void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
41 {
42 	m_internalType = CO_RIGID_BODY;
43 
44 	m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
45 	m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
46 	m_angularFactor.setValue(1, 1, 1);
47 	m_linearFactor.setValue(1, 1, 1);
48 	m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
49 	m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
50 	m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
51 	m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
52 		setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
53 
54 	m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
55 	m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
56 	m_optionalMotionState = constructionInfo.m_motionState;
57 	m_contactSolverType = 0;
58 	m_frictionSolverType = 0;
59 	m_additionalDamping = constructionInfo.m_additionalDamping;
60 	m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
61 	m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
62 	m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
63 	m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
64 
65 	if (m_optionalMotionState)
66 	{
67 		m_optionalMotionState->getWorldTransform(m_worldTransform);
68 	}
69 	else
70 	{
71 		m_worldTransform = constructionInfo.m_startWorldTransform;
72 	}
73 
74 	m_interpolationWorldTransform = m_worldTransform;
75 	m_interpolationLinearVelocity.setValue(0, 0, 0);
76 	m_interpolationAngularVelocity.setValue(0, 0, 0);
77 
78 	//moved to btCollisionObject
79 	m_friction = constructionInfo.m_friction;
80 	m_rollingFriction = constructionInfo.m_rollingFriction;
81 	m_spinningFriction = constructionInfo.m_spinningFriction;
82 
83 	m_restitution = constructionInfo.m_restitution;
84 
85 	setCollisionShape(constructionInfo.m_collisionShape);
86 	m_debugBodyId = uniqueId++;
87 
88 	setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
89 	updateInertiaTensor();
90 
91 	m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY;
92 
93 	m_deltaLinearVelocity.setZero();
94 	m_deltaAngularVelocity.setZero();
95 	m_invMass = m_inverseMass * m_linearFactor;
96 	m_pushVelocity.setZero();
97 	m_turnVelocity.setZero();
98 }
99 
predictIntegratedTransform(btScalar timeStep,btTransform & predictedTransform)100 void btRigidBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform)
101 {
102 	btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform);
103 }
104 
saveKinematicState(btScalar timeStep)105 void btRigidBody::saveKinematicState(btScalar timeStep)
106 {
107 	//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
108 	if (timeStep != btScalar(0.))
109 	{
110 		//if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
111 		if (getMotionState())
112 			getMotionState()->getWorldTransform(m_worldTransform);
113 		btVector3 linVel, angVel;
114 
115 		btTransformUtil::calculateVelocity(m_interpolationWorldTransform, m_worldTransform, timeStep, m_linearVelocity, m_angularVelocity);
116 		m_interpolationLinearVelocity = m_linearVelocity;
117 		m_interpolationAngularVelocity = m_angularVelocity;
118 		m_interpolationWorldTransform = m_worldTransform;
119 		//printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
120 	}
121 }
122 
getAabb(btVector3 & aabbMin,btVector3 & aabbMax) const123 void btRigidBody::getAabb(btVector3& aabbMin, btVector3& aabbMax) const
124 {
125 	getCollisionShape()->getAabb(m_worldTransform, aabbMin, aabbMax);
126 }
127 
setGravity(const btVector3 & acceleration)128 void btRigidBody::setGravity(const btVector3& acceleration)
129 {
130 	if (m_inverseMass != btScalar(0.0))
131 	{
132 		m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
133 	}
134 	m_gravity_acceleration = acceleration;
135 }
136 
setDamping(btScalar lin_damping,btScalar ang_damping)137 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
138 {
139 #ifdef BT_USE_OLD_DAMPING_METHOD
140 	m_linearDamping = btMax(lin_damping, btScalar(0.0));
141 	m_angularDamping = btMax(ang_damping, btScalar(0.0));
142 #else
143 	m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0));
144 	m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0));
145 #endif
146 }
147 
148 ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
applyDamping(btScalar timeStep)149 void btRigidBody::applyDamping(btScalar timeStep)
150 {
151 	//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
152 	//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
153 
154 #ifdef BT_USE_OLD_DAMPING_METHOD
155 	m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0));
156 	m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.0));
157 #else
158 	m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep);
159 	m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep);
160 #endif
161 
162 	if (m_additionalDamping)
163 	{
164 		//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
165 		//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
166 		if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
167 			(m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
168 		{
169 			m_angularVelocity *= m_additionalDampingFactor;
170 			m_linearVelocity *= m_additionalDampingFactor;
171 		}
172 
173 		btScalar speed = m_linearVelocity.length();
174 		if (speed < m_linearDamping)
175 		{
176 			btScalar dampVel = btScalar(0.005);
177 			if (speed > dampVel)
178 			{
179 				btVector3 dir = m_linearVelocity.normalized();
180 				m_linearVelocity -= dir * dampVel;
181 			}
182 			else
183 			{
184 				m_linearVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
185 			}
186 		}
187 
188 		btScalar angSpeed = m_angularVelocity.length();
189 		if (angSpeed < m_angularDamping)
190 		{
191 			btScalar angDampVel = btScalar(0.005);
192 			if (angSpeed > angDampVel)
193 			{
194 				btVector3 dir = m_angularVelocity.normalized();
195 				m_angularVelocity -= dir * angDampVel;
196 			}
197 			else
198 			{
199 				m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
200 			}
201 		}
202 	}
203 }
204 
applyGravity()205 void btRigidBody::applyGravity()
206 {
207 	if (isStaticOrKinematicObject())
208 		return;
209 
210 	applyCentralForce(m_gravity);
211 }
212 
clearGravity()213 void btRigidBody::clearGravity()
214 {
215     if (isStaticOrKinematicObject())
216         return;
217 
218     applyCentralForce(-m_gravity);
219 }
220 
proceedToTransform(const btTransform & newTrans)221 void btRigidBody::proceedToTransform(const btTransform& newTrans)
222 {
223 	setCenterOfMassTransform(newTrans);
224 }
225 
setMassProps(btScalar mass,const btVector3 & inertia)226 void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
227 {
228 	if (mass == btScalar(0.))
229 	{
230 		m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
231 		m_inverseMass = btScalar(0.);
232 	}
233 	else
234 	{
235 		m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
236 		m_inverseMass = btScalar(1.0) / mass;
237 	}
238 
239 	//Fg = m * a
240 	m_gravity = mass * m_gravity_acceleration;
241 
242 	m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
243 							   inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
244 							   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
245 
246 	m_invMass = m_linearFactor * m_inverseMass;
247 }
248 
updateInertiaTensor()249 void btRigidBody::updateInertiaTensor()
250 {
251 	m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
252 }
253 
getLocalInertia() const254 btVector3 btRigidBody::getLocalInertia() const
255 {
256 	btVector3 inertiaLocal;
257 	const btVector3 inertia = m_invInertiaLocal;
258 	inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
259 						  inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
260 						  inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
261 	return inertiaLocal;
262 }
263 
evalEulerEqn(const btVector3 & w1,const btVector3 & w0,const btVector3 & T,const btScalar dt,const btMatrix3x3 & I)264 inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt,
265 							  const btMatrix3x3& I)
266 {
267 	const btVector3 w2 = I * w1 + w1.cross(I * w1) * dt - (T * dt + I * w0);
268 	return w2;
269 }
270 
evalEulerEqnDeriv(const btVector3 & w1,const btVector3 & w0,const btScalar dt,const btMatrix3x3 & I)271 inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt,
272 									 const btMatrix3x3& I)
273 {
274 	btMatrix3x3 w1x, Iw1x;
275 	const btVector3 Iwi = (I * w1);
276 	w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]);
277 	Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]);
278 
279 	const btMatrix3x3 dfw1 = I + (w1x * I - Iw1x) * dt;
280 	return dfw1;
281 }
282 
computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const283 btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
284 {
285 	btVector3 inertiaLocal = getLocalInertia();
286 	btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
287 	btVector3 tmp = inertiaTensorWorld * getAngularVelocity();
288 	btVector3 gf = getAngularVelocity().cross(tmp);
289 	btScalar l2 = gf.length2();
290 	if (l2 > maxGyroscopicForce * maxGyroscopicForce)
291 	{
292 		gf *= btScalar(1.) / btSqrt(l2) * maxGyroscopicForce;
293 	}
294 	return gf;
295 }
296 
computeGyroscopicImpulseImplicit_Body(btScalar step) const297 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const
298 {
299 	btVector3 idl = getLocalInertia();
300 	btVector3 omega1 = getAngularVelocity();
301 	btQuaternion q = getWorldTransform().getRotation();
302 
303 	// Convert to body coordinates
304 	btVector3 omegab = quatRotate(q.inverse(), omega1);
305 	btMatrix3x3 Ib;
306 	Ib.setValue(idl.x(), 0, 0,
307 				0, idl.y(), 0,
308 				0, 0, idl.z());
309 
310 	btVector3 ibo = Ib * omegab;
311 
312 	// Residual vector
313 	btVector3 f = step * omegab.cross(ibo);
314 
315 	btMatrix3x3 skew0;
316 	omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]);
317 	btVector3 om = Ib * omegab;
318 	btMatrix3x3 skew1;
319 	om.getSkewSymmetricMatrix(&skew1[0], &skew1[1], &skew1[2]);
320 
321 	// Jacobian
322 	btMatrix3x3 J = Ib + (skew0 * Ib - skew1) * step;
323 
324 	//	btMatrix3x3 Jinv = J.inverse();
325 	//	btVector3 omega_div = Jinv*f;
326 	btVector3 omega_div = J.solve33(f);
327 
328 	// Single Newton-Raphson update
329 	omegab = omegab - omega_div;  //Solve33(J, f);
330 	// Back to world coordinates
331 	btVector3 omega2 = quatRotate(q, omegab);
332 	btVector3 gf = omega2 - omega1;
333 	return gf;
334 }
335 
computeGyroscopicImpulseImplicit_World(btScalar step) const336 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const
337 {
338 	// use full newton-euler equations.  common practice to drop the wxIw term. want it for better tumbling behavior.
339 	// calculate using implicit euler step so it's stable.
340 
341 	const btVector3 inertiaLocal = getLocalInertia();
342 	const btVector3 w0 = getAngularVelocity();
343 
344 	btMatrix3x3 I;
345 
346 	I = m_worldTransform.getBasis().scaled(inertiaLocal) *
347 		m_worldTransform.getBasis().transpose();
348 
349 	// use newtons method to find implicit solution for new angular velocity (w')
350 	// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0
351 	// df/dw' = I + 1xIw'*step + w'xI*step
352 
353 	btVector3 w1 = w0;
354 
355 	// one step of newton's method
356 	{
357 		const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
358 		const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);
359 
360 		btVector3 dw;
361 		dw = dfw.solve33(fw);
362 		//const btMatrix3x3 dfw_inv = dfw.inverse();
363 		//dw = dfw_inv*fw;
364 
365 		w1 -= dw;
366 	}
367 
368 	btVector3 gf = (w1 - w0);
369 	return gf;
370 }
371 
integrateVelocities(btScalar step)372 void btRigidBody::integrateVelocities(btScalar step)
373 {
374 	if (isStaticOrKinematicObject())
375 		return;
376 
377 	m_linearVelocity += m_totalForce * (m_inverseMass * step);
378 	m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
379 
380 #define MAX_ANGVEL SIMD_HALF_PI
381 	/// clamp angular velocity. collision calculations will fail on higher angular velocities
382 	btScalar angvel = m_angularVelocity.length();
383 	if (angvel * step > MAX_ANGVEL)
384 	{
385 		m_angularVelocity *= (MAX_ANGVEL / step) / angvel;
386 	}
387 	#if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
388 	clampVelocity(m_angularVelocity);
389 	#endif
390 }
391 
getOrientation() const392 btQuaternion btRigidBody::getOrientation() const
393 {
394 	btQuaternion orn;
395 	m_worldTransform.getBasis().getRotation(orn);
396 	return orn;
397 }
398 
setCenterOfMassTransform(const btTransform & xform)399 void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
400 {
401 	if (isKinematicObject())
402 	{
403 		m_interpolationWorldTransform = m_worldTransform;
404 	}
405 	else
406 	{
407 		m_interpolationWorldTransform = xform;
408 	}
409 	m_interpolationLinearVelocity = getLinearVelocity();
410 	m_interpolationAngularVelocity = getAngularVelocity();
411 	m_worldTransform = xform;
412 	updateInertiaTensor();
413 }
414 
addConstraintRef(btTypedConstraint * c)415 void btRigidBody::addConstraintRef(btTypedConstraint* c)
416 {
417 	///disable collision with the 'other' body
418 
419 	int index = m_constraintRefs.findLinearSearch(c);
420 	//don't add constraints that are already referenced
421 	//btAssert(index == m_constraintRefs.size());
422 	if (index == m_constraintRefs.size())
423 	{
424 		m_constraintRefs.push_back(c);
425 		btCollisionObject* colObjA = &c->getRigidBodyA();
426 		btCollisionObject* colObjB = &c->getRigidBodyB();
427 		if (colObjA == this)
428 		{
429 			colObjA->setIgnoreCollisionCheck(colObjB, true);
430 		}
431 		else
432 		{
433 			colObjB->setIgnoreCollisionCheck(colObjA, true);
434 		}
435 	}
436 }
437 
removeConstraintRef(btTypedConstraint * c)438 void btRigidBody::removeConstraintRef(btTypedConstraint* c)
439 {
440 	int index = m_constraintRefs.findLinearSearch(c);
441 	//don't remove constraints that are not referenced
442 	if (index < m_constraintRefs.size())
443 	{
444 		m_constraintRefs.remove(c);
445 		btCollisionObject* colObjA = &c->getRigidBodyA();
446 		btCollisionObject* colObjB = &c->getRigidBodyB();
447 		if (colObjA == this)
448 		{
449 			colObjA->setIgnoreCollisionCheck(colObjB, false);
450 		}
451 		else
452 		{
453 			colObjB->setIgnoreCollisionCheck(colObjA, false);
454 		}
455 	}
456 }
457 
calculateSerializeBufferSize() const458 int btRigidBody::calculateSerializeBufferSize() const
459 {
460 	int sz = sizeof(btRigidBodyData);
461 	return sz;
462 }
463 
464 ///fills the dataBuffer and returns the struct name (and 0 on failure)
serialize(void * dataBuffer,class btSerializer * serializer) const465 const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
466 {
467 	btRigidBodyData* rbd = (btRigidBodyData*)dataBuffer;
468 
469 	btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
470 
471 	m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld);
472 	m_linearVelocity.serialize(rbd->m_linearVelocity);
473 	m_angularVelocity.serialize(rbd->m_angularVelocity);
474 	rbd->m_inverseMass = m_inverseMass;
475 	m_angularFactor.serialize(rbd->m_angularFactor);
476 	m_linearFactor.serialize(rbd->m_linearFactor);
477 	m_gravity.serialize(rbd->m_gravity);
478 	m_gravity_acceleration.serialize(rbd->m_gravity_acceleration);
479 	m_invInertiaLocal.serialize(rbd->m_invInertiaLocal);
480 	m_totalForce.serialize(rbd->m_totalForce);
481 	m_totalTorque.serialize(rbd->m_totalTorque);
482 	rbd->m_linearDamping = m_linearDamping;
483 	rbd->m_angularDamping = m_angularDamping;
484 	rbd->m_additionalDamping = m_additionalDamping;
485 	rbd->m_additionalDampingFactor = m_additionalDampingFactor;
486 	rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
487 	rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
488 	rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
489 	rbd->m_linearSleepingThreshold = m_linearSleepingThreshold;
490 	rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
491 
492 	// Fill padding with zeros to appease msan.
493 #ifdef BT_USE_DOUBLE_PRECISION
494 	memset(rbd->m_padding, 0, sizeof(rbd->m_padding));
495 #endif
496 
497 	return btRigidBodyDataName;
498 }
499 
serializeSingleObject(class btSerializer * serializer) const500 void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
501 {
502 	btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(), 1);
503 	const char* structType = serialize(chunk->m_oldPtr, serializer);
504 	serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, (void*)this);
505 }
506