Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/leveljoemeHS14/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @ 10087

Last change on this file since 10087 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

Property svn:eol-style set to native

File size: 31.6 KB

Line
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	2007-09-09
17	Refactored by Francisco Le?n
18	email: projectileman@yahoo.com
19	http://gimpact.sf.net
20	*/
21
22	#include "btGeneric6DofConstraint.h"
23	#include "BulletDynamics/Dynamics/btRigidBody.h"
24	#include "LinearMath/btTransformUtil.h"
25	#include "LinearMath/btTransformUtil.h"
26	#include <new>
27
28
29
30	#define D6_USE_OBSOLETE_METHOD false
31	#define D6_USE_FRAME_OFFSET true
32
33
34
35
36
37
38	btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
39	: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
40	, m_frameInA(frameInA)
41	, m_frameInB(frameInB),
42	m_useLinearReferenceFrameA(useLinearReferenceFrameA),
43	m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
44	m_flags(0),
45	m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
46	{
47	calculateTransforms();
48	}
49
50
51
52	btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
53	: btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
54	m_frameInB(frameInB),
55	m_useLinearReferenceFrameA(useLinearReferenceFrameB),
56	m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
57	m_flags(0),
58	m_useSolveConstraintObsolete(false)
59	{
60	///not providing rigidbody A means implicitly using worldspace for body A
61	m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
62	calculateTransforms();
63	}
64
65
66
67
68	#define GENERIC_D6_DISABLE_WARMSTARTING 1
69
70
71
72	btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
73	btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
74	{
75	int i = index%3;
76	int j = index/3;
77	return mat[i][j];
78	}
79
80
81
82	///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
83	bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
84	bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
85	{
86	// // rot = cycz -cysz sy
87	// // czsxsy+cxsz cxcz-sxsysz -cy*sx
88	// // -cxczsy+sxsz czsx+cxsysz cx*cy
89	//
90
91	btScalar fi = btGetMatrixElem(mat,2);
92	if (fi < btScalar(1.0f))
93	{
94	if (fi > btScalar(-1.0f))
95	{
96	xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
97	xyz[1] = btAsin(btGetMatrixElem(mat,2));
98	xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
99	return true;
100	}
101	else
102	{
103	// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
104	xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
105	xyz[1] = -SIMD_HALF_PI;
106	xyz[2] = btScalar(0.0);
107	return false;
108	}
109	}
110	else
111	{
112	// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
113	xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
114	xyz[1] = SIMD_HALF_PI;
115	xyz[2] = 0.0;
116	}
117	return false;
118	}
119
120	//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
121
122	int btRotationalLimitMotor::testLimitValue(btScalar test_value)
123	{
124	if(m_loLimit>m_hiLimit)
125	{
126	m_currentLimit = 0;//Free from violation
127	return 0;
128	}
129	if (test_value < m_loLimit)
130	{
131	m_currentLimit = 1;//low limit violation
132	m_currentLimitError = test_value - m_loLimit;
133	return 1;
134	}
135	else if (test_value> m_hiLimit)
136	{
137	m_currentLimit = 2;//High limit violation
138	m_currentLimitError = test_value - m_hiLimit;
139	return 2;
140	};
141
142	m_currentLimit = 0;//Free from violation
143	return 0;
144
145	}
146
147
148
149	btScalar btRotationalLimitMotor::solveAngularLimits(
150	btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
151	btRigidBody * body0, btRigidBody * body1 )
152	{
153	if (needApplyTorques()==false) return 0.0f;
154
155	btScalar target_velocity = m_targetVelocity;
156	btScalar maxMotorForce = m_maxMotorForce;
157
158	//current error correction
159	if (m_currentLimit!=0)
160	{
161	target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
162	maxMotorForce = m_maxLimitForce;
163	}
164
165	maxMotorForce *= timeStep;
166
167	// current velocity difference
168
169	btVector3 angVelA;
170	body0->internalGetAngularVelocity(angVelA);
171	btVector3 angVelB;
172	body1->internalGetAngularVelocity(angVelB);
173
174	btVector3 vel_diff;
175	vel_diff = angVelA-angVelB;
176
177
178
179	btScalar rel_vel = axis.dot(vel_diff);
180
181	// correction velocity
182	btScalar motor_relvel = m_limitSoftness(target_velocity - m_dampingrel_vel);
183
184
185	if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON )
186	{
187	return 0.0f;//no need for applying force
188	}
189
190
191	// correction impulse
192	btScalar unclippedMotorImpulse = (1+m_bounce)motor_relveljacDiagABInv;
193
194	// clip correction impulse
195	btScalar clippedMotorImpulse;
196
197	///@todo: should clip against accumulated impulse
198	if (unclippedMotorImpulse>0.0f)
199	{
200	clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
201	}
202	else
203	{
204	clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
205	}
206
207
208	// sort with accumulated impulses
209	btScalar lo = btScalar(-BT_LARGE_FLOAT);
210	btScalar hi = btScalar(BT_LARGE_FLOAT);
211
212	btScalar oldaccumImpulse = m_accumulatedImpulse;
213	btScalar sum = oldaccumImpulse + clippedMotorImpulse;
214	m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
215
216	clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
217
218	btVector3 motorImp = clippedMotorImpulse * axis;
219
220	//body0->applyTorqueImpulse(motorImp);
221	//body1->applyTorqueImpulse(-motorImp);
222
223	body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
224	body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
225
226
227	return clippedMotorImpulse;
228
229
230	}
231
232	//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
233
234
235
236
237	//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
238
239
240	int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
241	{
242	btScalar loLimit = m_lowerLimit[limitIndex];
243	btScalar hiLimit = m_upperLimit[limitIndex];
244	if(loLimit > hiLimit)
245	{
246	m_currentLimit[limitIndex] = 0;//Free from violation
247	m_currentLimitError[limitIndex] = btScalar(0.f);
248	return 0;
249	}
250
251	if (test_value < loLimit)
252	{
253	m_currentLimit[limitIndex] = 2;//low limit violation
254	m_currentLimitError[limitIndex] = test_value - loLimit;
255	return 2;
256	}
257	else if (test_value> hiLimit)
258	{
259	m_currentLimit[limitIndex] = 1;//High limit violation
260	m_currentLimitError[limitIndex] = test_value - hiLimit;
261	return 1;
262	};
263
264	m_currentLimit[limitIndex] = 0;//Free from violation
265	m_currentLimitError[limitIndex] = btScalar(0.f);
266	return 0;
267	}
268
269
270
271	btScalar btTranslationalLimitMotor::solveLinearAxis(
272	btScalar timeStep,
273	btScalar jacDiagABInv,
274	btRigidBody& body1,const btVector3 &pointInA,
275	btRigidBody& body2,const btVector3 &pointInB,
276	int limit_index,
277	const btVector3 & axis_normal_on_a,
278	const btVector3 & anchorPos)
279	{
280
281	///find relative velocity
282	// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
283	// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
284	btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
285	btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
286
287	btVector3 vel1;
288	body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
289	btVector3 vel2;
290	body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
291	btVector3 vel = vel1 - vel2;
292
293	btScalar rel_vel = axis_normal_on_a.dot(vel);
294
295
296
297	/// apply displacement correction
298
299	//positional error (zeroth order error)
300	btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
301	btScalar lo = btScalar(-BT_LARGE_FLOAT);
302	btScalar hi = btScalar(BT_LARGE_FLOAT);
303
304	btScalar minLimit = m_lowerLimit[limit_index];
305	btScalar maxLimit = m_upperLimit[limit_index];
306
307	//handle the limits
308	if (minLimit < maxLimit)
309	{
310	{
311	if (depth > maxLimit)
312	{
313	depth -= maxLimit;
314	lo = btScalar(0.);
315
316	}
317	else
318	{
319	if (depth < minLimit)
320	{
321	depth -= minLimit;
322	hi = btScalar(0.);
323	}
324	else
325	{
326	return 0.0f;
327	}
328	}
329	}
330	}
331
332	btScalar normalImpulse= m_limitSoftness(m_restitutiondepth/timeStep - m_dampingrel_vel) jacDiagABInv;
333
334
335
336
337	btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
338	btScalar sum = oldNormalImpulse + normalImpulse;
339	m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
340	normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
341
342	btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
343	//body1.applyImpulse( impulse_vector, rel_pos1);
344	//body2.applyImpulse(-impulse_vector, rel_pos2);
345
346	btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
347	btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
348	body1.internalApplyImpulse(axis_normal_on_abody1.getInvMass(), body1.getInvInertiaTensorWorld()ftorqueAxis1,normalImpulse);
349	body2.internalApplyImpulse(axis_normal_on_abody2.getInvMass(), body2.getInvInertiaTensorWorld()ftorqueAxis2,-normalImpulse);
350
351
352
353
354	return normalImpulse;
355	}
356
357	//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
358
359	void btGeneric6DofConstraint::calculateAngleInfo()
360	{
361	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
362	matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
363	// in euler angle mode we do not actually constrain the angular velocity
364	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
365	//
366	// to get constrain w2-w1 along ...not
367	// ------ --------------------- ------
368	// d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
369	// d(angle[1])/dt = 0 ax[1]
370	// d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
371	//
372	// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
373	// to prove the result for angle[0], write the expression for angle[0] from
374	// GetInfo1 then take the derivative. to prove this for angle[2] it is
375	// easier to take the euler rate expression for d(angle[2])/dt with respect
376	// to the components of w and set that to 0.
377	btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
378	btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
379
380	m_calculatedAxis[1] = axis2.cross(axis0);
381	m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
382	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
383
384	m_calculatedAxis[0].normalize();
385	m_calculatedAxis[1].normalize();
386	m_calculatedAxis[2].normalize();
387
388	}
389
390	void btGeneric6DofConstraint::calculateTransforms()
391	{
392	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
393	}
394
395	void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
396	{
397	m_calculatedTransformA = transA * m_frameInA;
398	m_calculatedTransformB = transB * m_frameInB;
399	calculateLinearInfo();
400	calculateAngleInfo();
401	if(m_useOffsetForConstraintFrame)
402	{ // get weight factors depending on masses
403	btScalar miA = getRigidBodyA().getInvMass();
404	btScalar miB = getRigidBodyB().getInvMass();
405	m_hasStaticBody = (miA < SIMD_EPSILON) \|\| (miB < SIMD_EPSILON);
406	btScalar miS = miA + miB;
407	if(miS > btScalar(0.f))
408	{
409	m_factA = miB / miS;
410	}
411	else
412	{
413	m_factA = btScalar(0.5f);
414	}
415	m_factB = btScalar(1.0f) - m_factA;
416	}
417	}
418
419
420
421	void btGeneric6DofConstraint::buildLinearJacobian(
422	btJacobianEntry & jacLinear,const btVector3 & normalWorld,
423	const btVector3 & pivotAInW,const btVector3 & pivotBInW)
424	{
425	new (&jacLinear) btJacobianEntry(
426	m_rbA.getCenterOfMassTransform().getBasis().transpose(),
427	m_rbB.getCenterOfMassTransform().getBasis().transpose(),
428	pivotAInW - m_rbA.getCenterOfMassPosition(),
429	pivotBInW - m_rbB.getCenterOfMassPosition(),
430	normalWorld,
431	m_rbA.getInvInertiaDiagLocal(),
432	m_rbA.getInvMass(),
433	m_rbB.getInvInertiaDiagLocal(),
434	m_rbB.getInvMass());
435	}
436
437
438
439	void btGeneric6DofConstraint::buildAngularJacobian(
440	btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
441	{
442	new (&jacAngular) btJacobianEntry(jointAxisW,
443	m_rbA.getCenterOfMassTransform().getBasis().transpose(),
444	m_rbB.getCenterOfMassTransform().getBasis().transpose(),
445	m_rbA.getInvInertiaDiagLocal(),
446	m_rbB.getInvInertiaDiagLocal());
447
448	}
449
450
451
452	bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
453	{
454	btScalar angle = m_calculatedAxisAngleDiff[axis_index];
455	angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
456	m_angularLimits[axis_index].m_currentPosition = angle;
457	//test limits
458	m_angularLimits[axis_index].testLimitValue(angle);
459	return m_angularLimits[axis_index].needApplyTorques();
460	}
461
462
463
464	void btGeneric6DofConstraint::buildJacobian()
465	{
466	#ifndef __SPU__
467	if (m_useSolveConstraintObsolete)
468	{
469
470	// Clear accumulated impulses for the next simulation step
471	m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
472	int i;
473	for(i = 0; i < 3; i++)
474	{
475	m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
476	}
477	//calculates transform
478	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
479
480	// const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
481	// const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
482	calcAnchorPos();
483	btVector3 pivotAInW = m_AnchorPos;
484	btVector3 pivotBInW = m_AnchorPos;
485
486	// not used here
487	// btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
488	// btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
489
490	btVector3 normalWorld;
491	//linear part
492	for (i=0;i<3;i++)
493	{
494	if (m_linearLimits.isLimited(i))
495	{
496	if (m_useLinearReferenceFrameA)
497	normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
498	else
499	normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
500
501	buildLinearJacobian(
502	m_jacLinear[i],normalWorld ,
503	pivotAInW,pivotBInW);
504
505	}
506	}
507
508	// angular part
509	for (i=0;i<3;i++)
510	{
511	//calculates error angle
512	if (testAngularLimitMotor(i))
513	{
514	normalWorld = this->getAxis(i);
515	// Create angular atom
516	buildAngularJacobian(m_jacAng[i],normalWorld);
517	}
518	}
519
520	}
521	#endif //__SPU__
522
523	}
524
525
526	void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
527	{
528	if (m_useSolveConstraintObsolete)
529	{
530	info->m_numConstraintRows = 0;
531	info->nub = 0;
532	} else
533	{
534	//prepare constraint
535	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
536	info->m_numConstraintRows = 0;
537	info->nub = 6;
538	int i;
539	//test linear limits
540	for(i = 0; i < 3; i++)
541	{
542	if(m_linearLimits.needApplyForce(i))
543	{
544	info->m_numConstraintRows++;
545	info->nub--;
546	}
547	}
548	//test angular limits
549	for (i=0;i<3 ;i++ )
550	{
551	if(testAngularLimitMotor(i))
552	{
553	info->m_numConstraintRows++;
554	info->nub--;
555	}
556	}
557	}
558	}
559
560	void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
561	{
562	if (m_useSolveConstraintObsolete)
563	{
564	info->m_numConstraintRows = 0;
565	info->nub = 0;
566	} else
567	{
568	//pre-allocate all 6
569	info->m_numConstraintRows = 6;
570	info->nub = 0;
571	}
572	}
573
574
575	void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
576	{
577	btAssert(!m_useSolveConstraintObsolete);
578
579	const btTransform& transA = m_rbA.getCenterOfMassTransform();
580	const btTransform& transB = m_rbB.getCenterOfMassTransform();
581	const btVector3& linVelA = m_rbA.getLinearVelocity();
582	const btVector3& linVelB = m_rbB.getLinearVelocity();
583	const btVector3& angVelA = m_rbA.getAngularVelocity();
584	const btVector3& angVelB = m_rbB.getAngularVelocity();
585
586	if(m_useOffsetForConstraintFrame)
587	{ // for stability better to solve angular limits first
588	int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
589	setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
590	}
591	else
592	{ // leave old version for compatibility
593	int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
594	setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
595	}
596
597	}
598
599
600	void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
601	{
602
603	btAssert(!m_useSolveConstraintObsolete);
604	//prepare constraint
605	calculateTransforms(transA,transB);
606
607	int i;
608	for (i=0;i<3 ;i++ )
609	{
610	testAngularLimitMotor(i);
611	}
612
613	if(m_useOffsetForConstraintFrame)
614	{ // for stability better to solve angular limits first
615	int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
616	setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
617	}
618	else
619	{ // leave old version for compatibility
620	int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
621	setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
622	}
623	}
624
625
626
627	int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
628	{
629	// int row = 0;
630	//solve linear limits
631	btRotationalLimitMotor limot;
632	for (int i=0;i<3 ;i++ )
633	{
634	if(m_linearLimits.needApplyForce(i))
635	{ // re-use rotational motor code
636	limot.m_bounce = btScalar(0.f);
637	limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
638	limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
639	limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
640	limot.m_damping = m_linearLimits.m_damping;
641	limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
642	limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
643	limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
644	limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
645	limot.m_maxLimitForce = btScalar(0.f);
646	limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
647	limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
648	btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
649	int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
650	limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
651	limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
652	limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
653	if(m_useOffsetForConstraintFrame)
654	{
655	int indx1 = (i + 1) % 3;
656	int indx2 = (i + 2) % 3;
657	int rotAllowed = 1; // rotations around orthos to current axis
658	if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
659	{
660	rotAllowed = 0;
661	}
662	row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
663	}
664	else
665	{
666	row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
667	}
668	}
669	}
670	return row;
671	}
672
673
674
675	int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
676	{
677	btGeneric6DofConstraint * d6constraint = this;
678	int row = row_offset;
679	//solve angular limits
680	for (int i=0;i<3 ;i++ )
681	{
682	if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
683	{
684	btVector3 axis = d6constraint->getAxis(i);
685	int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
686	if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
687	{
688	m_angularLimits[i].m_normalCFM = info->cfm[0];
689	}
690	if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
691	{
692	m_angularLimits[i].m_stopCFM = info->cfm[0];
693	}
694	if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
695	{
696	m_angularLimits[i].m_stopERP = info->erp;
697	}
698	row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
699	transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
700	}
701	}
702
703	return row;
704	}
705
706
707
708
709	void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
710	{
711	(void)timeStep;
712
713	}
714
715
716	void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
717	{
718	m_frameInA = frameA;
719	m_frameInB = frameB;
720	buildJacobian();
721	calculateTransforms();
722	}
723
724
725
726	btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
727	{
728	return m_calculatedAxis[axis_index];
729	}
730
731
732	btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
733	{
734	return m_calculatedLinearDiff[axisIndex];
735	}
736
737
738	btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
739	{
740	return m_calculatedAxisAngleDiff[axisIndex];
741	}
742
743
744
745	void btGeneric6DofConstraint::calcAnchorPos(void)
746	{
747	btScalar imA = m_rbA.getInvMass();
748	btScalar imB = m_rbB.getInvMass();
749	btScalar weight;
750	if(imB == btScalar(0.0))
751	{
752	weight = btScalar(1.0);
753	}
754	else
755	{
756	weight = imA / (imA + imB);
757	}
758	const btVector3& pA = m_calculatedTransformA.getOrigin();
759	const btVector3& pB = m_calculatedTransformB.getOrigin();
760	m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
761	return;
762	}
763
764
765
766	void btGeneric6DofConstraint::calculateLinearInfo()
767	{
768	m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
769	m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
770	for(int i = 0; i < 3; i++)
771	{
772	m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
773	m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
774	}
775	}
776
777
778
779	int btGeneric6DofConstraint::get_limit_motor_info2(
780	btRotationalLimitMotor * limot,
781	const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
782	btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
783	{
784	int srow = row * info->rowskip;
785	int powered = limot->m_enableMotor;
786	int limit = limot->m_currentLimit;
787	if (powered \|\| limit)
788	{ // if the joint is powered, or has joint limits, add in the extra row
789	btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
790	btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
791	J1[srow+0] = ax1[0];
792	J1[srow+1] = ax1[1];
793	J1[srow+2] = ax1[2];
794	if(rotational)
795	{
796	J2[srow+0] = -ax1[0];
797	J2[srow+1] = -ax1[1];
798	J2[srow+2] = -ax1[2];
799	}
800	if((!rotational))
801	{
802	if (m_useOffsetForConstraintFrame)
803	{
804	btVector3 tmpA, tmpB, relA, relB;
805	// get vector from bodyB to frameB in WCS
806	relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
807	// get its projection to constraint axis
808	btVector3 projB = ax1 * relB.dot(ax1);
809	// get vector directed from bodyB to constraint axis (and orthogonal to it)
810	btVector3 orthoB = relB - projB;
811	// same for bodyA
812	relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
813	btVector3 projA = ax1 * relA.dot(ax1);
814	btVector3 orthoA = relA - projA;
815	// get desired offset between frames A and B along constraint axis
816	btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
817	// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
818	btVector3 totalDist = projA + ax1 * desiredOffs - projB;
819	// get offset vectors relA and relB
820	relA = orthoA + totalDist * m_factA;
821	relB = orthoB - totalDist * m_factB;
822	tmpA = relA.cross(ax1);
823	tmpB = relB.cross(ax1);
824	if(m_hasStaticBody && (!rotAllowed))
825	{
826	tmpA *= m_factA;
827	tmpB *= m_factB;
828	}
829	int i;
830	for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
831	for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
832	} else
833	{
834	btVector3 ltd; // Linear Torque Decoupling vector
835	btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
836	ltd = c.cross(ax1);
837	info->m_J1angularAxis[srow+0] = ltd[0];
838	info->m_J1angularAxis[srow+1] = ltd[1];
839	info->m_J1angularAxis[srow+2] = ltd[2];
840
841	c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
842	ltd = -c.cross(ax1);
843	info->m_J2angularAxis[srow+0] = ltd[0];
844	info->m_J2angularAxis[srow+1] = ltd[1];
845	info->m_J2angularAxis[srow+2] = ltd[2];
846	}
847	}
848	// if we're limited low and high simultaneously, the joint motor is
849	// ineffective
850	if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
851	info->m_constraintError[srow] = btScalar(0.f);
852	if (powered)
853	{
854	info->cfm[srow] = limot->m_normalCFM;
855	if(!limit)
856	{
857	btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
858
859	btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
860	limot->m_loLimit,
861	limot->m_hiLimit,
862	tag_vel,
863	info->fps * limot->m_stopERP);
864	info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
865	info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
866	info->m_upperLimit[srow] = limot->m_maxMotorForce;
867	}
868	}
869	if(limit)
870	{
871	btScalar k = info->fps * limot->m_stopERP;
872	if(!rotational)
873	{
874	info->m_constraintError[srow] += k * limot->m_currentLimitError;
875	}
876	else
877	{
878	info->m_constraintError[srow] += -k * limot->m_currentLimitError;
879	}
880	info->cfm[srow] = limot->m_stopCFM;
881	if (limot->m_loLimit == limot->m_hiLimit)
882	{ // limited low and high simultaneously
883	info->m_lowerLimit[srow] = -SIMD_INFINITY;
884	info->m_upperLimit[srow] = SIMD_INFINITY;
885	}
886	else
887	{
888	if (limit == 1)
889	{
890	info->m_lowerLimit[srow] = 0;
891	info->m_upperLimit[srow] = SIMD_INFINITY;
892	}
893	else
894	{
895	info->m_lowerLimit[srow] = -SIMD_INFINITY;
896	info->m_upperLimit[srow] = 0;
897	}
898	// deal with bounce
899	if (limot->m_bounce > 0)
900	{
901	// calculate joint velocity
902	btScalar vel;
903	if (rotational)
904	{
905	vel = angVelA.dot(ax1);
906	//make sure that if no body -> angVelB == zero vec
907	// if (body1)
908	vel -= angVelB.dot(ax1);
909	}
910	else
911	{
912	vel = linVelA.dot(ax1);
913	//make sure that if no body -> angVelB == zero vec
914	// if (body1)
915	vel -= linVelB.dot(ax1);
916	}
917	// only apply bounce if the velocity is incoming, and if the
918	// resulting c[] exceeds what we already have.
919	if (limit == 1)
920	{
921	if (vel < 0)
922	{
923	btScalar newc = -limot->m_bounce* vel;
924	if (newc > info->m_constraintError[srow])
925	info->m_constraintError[srow] = newc;
926	}
927	}
928	else
929	{
930	if (vel > 0)
931	{
932	btScalar newc = -limot->m_bounce * vel;
933	if (newc < info->m_constraintError[srow])
934	info->m_constraintError[srow] = newc;
935	}
936	}
937	}
938	}
939	}
940	return 1;
941	}
942	else return 0;
943	}
944
945
946
947
948
949
950	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
951	///If no axis is provided, it uses the default axis for this constraint.
952	void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
953	{
954	if((axis >= 0) && (axis < 3))
955	{
956	switch(num)
957	{
958	case BT_CONSTRAINT_STOP_ERP :
959	m_linearLimits.m_stopERP[axis] = value;
960	m_flags \|= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
961	break;
962	case BT_CONSTRAINT_STOP_CFM :
963	m_linearLimits.m_stopCFM[axis] = value;
964	m_flags \|= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
965	break;
966	case BT_CONSTRAINT_CFM :
967	m_linearLimits.m_normalCFM[axis] = value;
968	m_flags \|= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
969	break;
970	default :
971	btAssertConstrParams(0);
972	}
973	}
974	else if((axis >=3) && (axis < 6))
975	{
976	switch(num)
977	{
978	case BT_CONSTRAINT_STOP_ERP :
979	m_angularLimits[axis - 3].m_stopERP = value;
980	m_flags \|= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
981	break;
982	case BT_CONSTRAINT_STOP_CFM :
983	m_angularLimits[axis - 3].m_stopCFM = value;
984	m_flags \|= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
985	break;
986	case BT_CONSTRAINT_CFM :
987	m_angularLimits[axis - 3].m_normalCFM = value;
988	m_flags \|= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
989	break;
990	default :
991	btAssertConstrParams(0);
992	}
993	}
994	else
995	{
996	btAssertConstrParams(0);
997	}
998	}
999
1000	///return the local value of parameter
1001	btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
1002	{
1003	btScalar retVal = 0;
1004	if((axis >= 0) && (axis < 3))
1005	{
1006	switch(num)
1007	{
1008	case BT_CONSTRAINT_STOP_ERP :
1009	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1010	retVal = m_linearLimits.m_stopERP[axis];
1011	break;
1012	case BT_CONSTRAINT_STOP_CFM :
1013	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1014	retVal = m_linearLimits.m_stopCFM[axis];
1015	break;
1016	case BT_CONSTRAINT_CFM :
1017	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1018	retVal = m_linearLimits.m_normalCFM[axis];
1019	break;
1020	default :
1021	btAssertConstrParams(0);
1022	}
1023	}
1024	else if((axis >=3) && (axis < 6))
1025	{
1026	switch(num)
1027	{
1028	case BT_CONSTRAINT_STOP_ERP :
1029	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1030	retVal = m_angularLimits[axis - 3].m_stopERP;
1031	break;
1032	case BT_CONSTRAINT_STOP_CFM :
1033	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1034	retVal = m_angularLimits[axis - 3].m_stopCFM;
1035	break;
1036	case BT_CONSTRAINT_CFM :
1037	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1038	retVal = m_angularLimits[axis - 3].m_normalCFM;
1039	break;
1040	default :
1041	btAssertConstrParams(0);
1042	}
1043	}
1044	else
1045	{
1046	btAssertConstrParams(0);
1047	}
1048	return retVal;
1049	}
1050
1051
1052
1053	void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)
1054	{
1055	btVector3 zAxis = axis1.normalized();
1056	btVector3 yAxis = axis2.normalized();
1057	btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
1058
1059	btTransform frameInW;
1060	frameInW.setIdentity();
1061	frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
1062	xAxis[1], yAxis[1], zAxis[1],
1063	xAxis[2], yAxis[2], zAxis[2]);
1064
1065	// now get constraint frame in local coordinate systems
1066	m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
1067	m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
1068
1069	calculateTransforms();
1070	}

Note: See TracBrowser for help on using the repository browser.

Download in other formats: