Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/kicklib2/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @ 8474

Last change on this file since 8474 was 8284, checked in by rgrieder, 14 years ago
Merged revisions 7978 - 8096 from kicklib to kicklib2.
Property svn:eol-style set to `native`
File size: 30.7 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
717	btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
718	{
719	return m_calculatedAxis[axis_index];
720	}
721
722
723	btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
724	{
725	return m_calculatedLinearDiff[axisIndex];
726	}
727
728
729	btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
730	{
731	return m_calculatedAxisAngleDiff[axisIndex];
732	}
733
734
735
736	void btGeneric6DofConstraint::calcAnchorPos(void)
737	{
738	btScalar imA = m_rbA.getInvMass();
739	btScalar imB = m_rbB.getInvMass();
740	btScalar weight;
741	if(imB == btScalar(0.0))
742	{
743	weight = btScalar(1.0);
744	}
745	else
746	{
747	weight = imA / (imA + imB);
748	}
749	const btVector3& pA = m_calculatedTransformA.getOrigin();
750	const btVector3& pB = m_calculatedTransformB.getOrigin();
751	m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
752	return;
753	}
754
755
756
757	void btGeneric6DofConstraint::calculateLinearInfo()
758	{
759	m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
760	m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
761	for(int i = 0; i < 3; i++)
762	{
763	m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
764	m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
765	}
766	}
767
768
769
770	int btGeneric6DofConstraint::get_limit_motor_info2(
771	btRotationalLimitMotor * limot,
772	const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
773	btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
774	{
775	int srow = row * info->rowskip;
776	int powered = limot->m_enableMotor;
777	int limit = limot->m_currentLimit;
778	if (powered \|\| limit)
779	{ // if the joint is powered, or has joint limits, add in the extra row
780	btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
781	btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
782	J1[srow+0] = ax1[0];
783	J1[srow+1] = ax1[1];
784	J1[srow+2] = ax1[2];
785	if(rotational)
786	{
787	J2[srow+0] = -ax1[0];
788	J2[srow+1] = -ax1[1];
789	J2[srow+2] = -ax1[2];
790	}
791	if((!rotational))
792	{
793	if (m_useOffsetForConstraintFrame)
794	{
795	btVector3 tmpA, tmpB, relA, relB;
796	// get vector from bodyB to frameB in WCS
797	relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
798	// get its projection to constraint axis
799	btVector3 projB = ax1 * relB.dot(ax1);
800	// get vector directed from bodyB to constraint axis (and orthogonal to it)
801	btVector3 orthoB = relB - projB;
802	// same for bodyA
803	relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
804	btVector3 projA = ax1 * relA.dot(ax1);
805	btVector3 orthoA = relA - projA;
806	// get desired offset between frames A and B along constraint axis
807	btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
808	// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
809	btVector3 totalDist = projA + ax1 * desiredOffs - projB;
810	// get offset vectors relA and relB
811	relA = orthoA + totalDist * m_factA;
812	relB = orthoB - totalDist * m_factB;
813	tmpA = relA.cross(ax1);
814	tmpB = relB.cross(ax1);
815	if(m_hasStaticBody && (!rotAllowed))
816	{
817	tmpA *= m_factA;
818	tmpB *= m_factB;
819	}
820	int i;
821	for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
822	for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
823	} else
824	{
825	btVector3 ltd; // Linear Torque Decoupling vector
826	btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
827	ltd = c.cross(ax1);
828	info->m_J1angularAxis[srow+0] = ltd[0];
829	info->m_J1angularAxis[srow+1] = ltd[1];
830	info->m_J1angularAxis[srow+2] = ltd[2];
831
832	c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
833	ltd = -c.cross(ax1);
834	info->m_J2angularAxis[srow+0] = ltd[0];
835	info->m_J2angularAxis[srow+1] = ltd[1];
836	info->m_J2angularAxis[srow+2] = ltd[2];
837	}
838	}
839	// if we're limited low and high simultaneously, the joint motor is
840	// ineffective
841	if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
842	info->m_constraintError[srow] = btScalar(0.f);
843	if (powered)
844	{
845	info->cfm[srow] = limot->m_normalCFM;
846	if(!limit)
847	{
848	btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
849
850	btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
851	limot->m_loLimit,
852	limot->m_hiLimit,
853	tag_vel,
854	info->fps * limot->m_stopERP);
855	info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
856	info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
857	info->m_upperLimit[srow] = limot->m_maxMotorForce;
858	}
859	}
860	if(limit)
861	{
862	btScalar k = info->fps * limot->m_stopERP;
863	if(!rotational)
864	{
865	info->m_constraintError[srow] += k * limot->m_currentLimitError;
866	}
867	else
868	{
869	info->m_constraintError[srow] += -k * limot->m_currentLimitError;
870	}
871	info->cfm[srow] = limot->m_stopCFM;
872	if (limot->m_loLimit == limot->m_hiLimit)
873	{ // limited low and high simultaneously
874	info->m_lowerLimit[srow] = -SIMD_INFINITY;
875	info->m_upperLimit[srow] = SIMD_INFINITY;
876	}
877	else
878	{
879	if (limit == 1)
880	{
881	info->m_lowerLimit[srow] = 0;
882	info->m_upperLimit[srow] = SIMD_INFINITY;
883	}
884	else
885	{
886	info->m_lowerLimit[srow] = -SIMD_INFINITY;
887	info->m_upperLimit[srow] = 0;
888	}
889	// deal with bounce
890	if (limot->m_bounce > 0)
891	{
892	// calculate joint velocity
893	btScalar vel;
894	if (rotational)
895	{
896	vel = angVelA.dot(ax1);
897	//make sure that if no body -> angVelB == zero vec
898	// if (body1)
899	vel -= angVelB.dot(ax1);
900	}
901	else
902	{
903	vel = linVelA.dot(ax1);
904	//make sure that if no body -> angVelB == zero vec
905	// if (body1)
906	vel -= linVelB.dot(ax1);
907	}
908	// only apply bounce if the velocity is incoming, and if the
909	// resulting c[] exceeds what we already have.
910	if (limit == 1)
911	{
912	if (vel < 0)
913	{
914	btScalar newc = -limot->m_bounce* vel;
915	if (newc > info->m_constraintError[srow])
916	info->m_constraintError[srow] = newc;
917	}
918	}
919	else
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	}
929	}
930	}
931	return 1;
932	}
933	else return 0;
934	}
935
936
937
938
939
940
941	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
942	///If no axis is provided, it uses the default axis for this constraint.
943	void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
944	{
945	if((axis >= 0) && (axis < 3))
946	{
947	switch(num)
948	{
949	case BT_CONSTRAINT_STOP_ERP :
950	m_linearLimits.m_stopERP[axis] = value;
951	m_flags \|= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
952	break;
953	case BT_CONSTRAINT_STOP_CFM :
954	m_linearLimits.m_stopCFM[axis] = value;
955	m_flags \|= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
956	break;
957	case BT_CONSTRAINT_CFM :
958	m_linearLimits.m_normalCFM[axis] = value;
959	m_flags \|= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
960	break;
961	default :
962	btAssertConstrParams(0);
963	}
964	}
965	else if((axis >=3) && (axis < 6))
966	{
967	switch(num)
968	{
969	case BT_CONSTRAINT_STOP_ERP :
970	m_angularLimits[axis - 3].m_stopERP = value;
971	m_flags \|= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
972	break;
973	case BT_CONSTRAINT_STOP_CFM :
974	m_angularLimits[axis - 3].m_stopCFM = value;
975	m_flags \|= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
976	break;
977	case BT_CONSTRAINT_CFM :
978	m_angularLimits[axis - 3].m_normalCFM = value;
979	m_flags \|= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
980	break;
981	default :
982	btAssertConstrParams(0);
983	}
984	}
985	else
986	{
987	btAssertConstrParams(0);
988	}
989	}
990
991	///return the local value of parameter
992	btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
993	{
994	btScalar retVal = 0;
995	if((axis >= 0) && (axis < 3))
996	{
997	switch(num)
998	{
999	case BT_CONSTRAINT_STOP_ERP :
1000	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1001	retVal = m_linearLimits.m_stopERP[axis];
1002	break;
1003	case BT_CONSTRAINT_STOP_CFM :
1004	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1005	retVal = m_linearLimits.m_stopCFM[axis];
1006	break;
1007	case BT_CONSTRAINT_CFM :
1008	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1009	retVal = m_linearLimits.m_normalCFM[axis];
1010	break;
1011	default :
1012	btAssertConstrParams(0);
1013	}
1014	}
1015	else if((axis >=3) && (axis < 6))
1016	{
1017	switch(num)
1018	{
1019	case BT_CONSTRAINT_STOP_ERP :
1020	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1021	retVal = m_angularLimits[axis - 3].m_stopERP;
1022	break;
1023	case BT_CONSTRAINT_STOP_CFM :
1024	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1025	retVal = m_angularLimits[axis - 3].m_stopCFM;
1026	break;
1027	case BT_CONSTRAINT_CFM :
1028	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
1029	retVal = m_angularLimits[axis - 3].m_normalCFM;
1030	break;
1031	default :
1032	btAssertConstrParams(0);
1033	}
1034	}
1035	else
1036	{
1037	btAssertConstrParams(0);
1038	}
1039	return retVal;
1040	}

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

Download in other formats: