Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/kicklib/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @ 8071

Last change on this file since 8071 was 7983, checked in by rgrieder, 15 years ago
Updated Bullet Physics Engine from v2.74 to v2.77
Property svn:eol-style set to `native`
File size: 35.0 KB

Line
1	/*
2	Bullet Continuous Collision Detection and Physics Library
3	btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
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	Written by: Marcus Hennix
16	*/
17
18
19	#include "btConeTwistConstraint.h"
20	#include "BulletDynamics/Dynamics/btRigidBody.h"
21	#include "LinearMath/btTransformUtil.h"
22	#include "LinearMath/btMinMax.h"
23	#include <new>
24
25
26
27	//#define CONETWIST_USE_OBSOLETE_SOLVER true
28	#define CONETWIST_USE_OBSOLETE_SOLVER false
29	#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
30
31
32	SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
33	{
34	btVector3 vec = axis * invInertiaWorld;
35	return axis.dot(vec);
36	}
37
38
39
40
41	btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
42	const btTransform& rbAFrame,const btTransform& rbBFrame)
43	:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
44	m_angularOnly(false),
45	m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
46	{
47	init();
48	}
49
50	btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
51	:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
52	m_angularOnly(false),
53	m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
54	{
55	m_rbBFrame = m_rbAFrame;
56	init();
57	}
58
59
60	void btConeTwistConstraint::init()
61	{
62	m_angularOnly = false;
63	m_solveTwistLimit = false;
64	m_solveSwingLimit = false;
65	m_bMotorEnabled = false;
66	m_maxMotorImpulse = btScalar(-1);
67
68	setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
69	m_damping = btScalar(0.01);
70	m_fixThresh = CONETWIST_DEF_FIX_THRESH;
71	m_flags = 0;
72	m_linCFM = btScalar(0.f);
73	m_linERP = btScalar(0.7f);
74	m_angCFM = btScalar(0.f);
75	}
76
77
78	void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
79	{
80	if (m_useSolveConstraintObsolete)
81	{
82	info->m_numConstraintRows = 0;
83	info->nub = 0;
84	}
85	else
86	{
87	info->m_numConstraintRows = 3;
88	info->nub = 3;
89	calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
90	if(m_solveSwingLimit)
91	{
92	info->m_numConstraintRows++;
93	info->nub--;
94	if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
95	{
96	info->m_numConstraintRows++;
97	info->nub--;
98	}
99	}
100	if(m_solveTwistLimit)
101	{
102	info->m_numConstraintRows++;
103	info->nub--;
104	}
105	}
106	}
107
108	void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
109	{
110	//always reserve 6 rows: object transform is not available on SPU
111	info->m_numConstraintRows = 6;
112	info->nub = 0;
113
114	}
115
116
117	void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
118	{
119	getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
120	}
121
122	void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
123	{
124	calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
125
126	btAssert(!m_useSolveConstraintObsolete);
127	// set jacobian
128	info->m_J1linearAxis[0] = 1;
129	info->m_J1linearAxis[info->rowskip+1] = 1;
130	info->m_J1linearAxis[2*info->rowskip+2] = 1;
131	btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
132	{
133	btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
134	btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
135	btVector3* angular2 = (btVector3)(info->m_J1angularAxis+2info->rowskip);
136	btVector3 a1neg = -a1;
137	a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
138	}
139	btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
140	{
141	btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
142	btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
143	btVector3* angular2 = (btVector3)(info->m_J2angularAxis+2info->rowskip);
144	a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
145	}
146	// set right hand side
147	btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
148	btScalar k = info->fps * linERP;
149	int j;
150	for (j=0; j<3; j++)
151	{
152	info->m_constraintError[jinfo->rowskip] = k (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
153	info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
154	info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
155	if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
156	{
157	info->cfm[j*info->rowskip] = m_linCFM;
158	}
159	}
160	int row = 3;
161	int srow = row * info->rowskip;
162	btVector3 ax1;
163	// angular limits
164	if(m_solveSwingLimit)
165	{
166	btScalar *J1 = info->m_J1angularAxis;
167	btScalar *J2 = info->m_J2angularAxis;
168	if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
169	{
170	btTransform trA = transA*m_rbAFrame;
171	btVector3 p = trA.getBasis().getColumn(1);
172	btVector3 q = trA.getBasis().getColumn(2);
173	int srow1 = srow + info->rowskip;
174	J1[srow+0] = p[0];
175	J1[srow+1] = p[1];
176	J1[srow+2] = p[2];
177	J1[srow1+0] = q[0];
178	J1[srow1+1] = q[1];
179	J1[srow1+2] = q[2];
180	J2[srow+0] = -p[0];
181	J2[srow+1] = -p[1];
182	J2[srow+2] = -p[2];
183	J2[srow1+0] = -q[0];
184	J2[srow1+1] = -q[1];
185	J2[srow1+2] = -q[2];
186	btScalar fact = info->fps * m_relaxationFactor;
187	info->m_constraintError[srow] = fact * m_swingAxis.dot(p);
188	info->m_constraintError[srow1] = fact * m_swingAxis.dot(q);
189	info->m_lowerLimit[srow] = -SIMD_INFINITY;
190	info->m_upperLimit[srow] = SIMD_INFINITY;
191	info->m_lowerLimit[srow1] = -SIMD_INFINITY;
192	info->m_upperLimit[srow1] = SIMD_INFINITY;
193	srow = srow1 + info->rowskip;
194	}
195	else
196	{
197	ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
198	J1[srow+0] = ax1[0];
199	J1[srow+1] = ax1[1];
200	J1[srow+2] = ax1[2];
201	J2[srow+0] = -ax1[0];
202	J2[srow+1] = -ax1[1];
203	J2[srow+2] = -ax1[2];
204	btScalar k = info->fps * m_biasFactor;
205
206	info->m_constraintError[srow] = k * m_swingCorrection;
207	if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
208	{
209	info->cfm[srow] = m_angCFM;
210	}
211	// m_swingCorrection is always positive or 0
212	info->m_lowerLimit[srow] = 0;
213	info->m_upperLimit[srow] = SIMD_INFINITY;
214	srow += info->rowskip;
215	}
216	}
217	if(m_solveTwistLimit)
218	{
219	ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
220	btScalar *J1 = info->m_J1angularAxis;
221	btScalar *J2 = info->m_J2angularAxis;
222	J1[srow+0] = ax1[0];
223	J1[srow+1] = ax1[1];
224	J1[srow+2] = ax1[2];
225	J2[srow+0] = -ax1[0];
226	J2[srow+1] = -ax1[1];
227	J2[srow+2] = -ax1[2];
228	btScalar k = info->fps * m_biasFactor;
229	info->m_constraintError[srow] = k * m_twistCorrection;
230	if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
231	{
232	info->cfm[srow] = m_angCFM;
233	}
234	if(m_twistSpan > 0.0f)
235	{
236
237	if(m_twistCorrection > 0.0f)
238	{
239	info->m_lowerLimit[srow] = 0;
240	info->m_upperLimit[srow] = SIMD_INFINITY;
241	}
242	else
243	{
244	info->m_lowerLimit[srow] = -SIMD_INFINITY;
245	info->m_upperLimit[srow] = 0;
246	}
247	}
248	else
249	{
250	info->m_lowerLimit[srow] = -SIMD_INFINITY;
251	info->m_upperLimit[srow] = SIMD_INFINITY;
252	}
253	srow += info->rowskip;
254	}
255	}
256
257
258
259	void btConeTwistConstraint::buildJacobian()
260	{
261	if (m_useSolveConstraintObsolete)
262	{
263	m_appliedImpulse = btScalar(0.);
264	m_accTwistLimitImpulse = btScalar(0.);
265	m_accSwingLimitImpulse = btScalar(0.);
266	m_accMotorImpulse = btVector3(0.,0.,0.);
267
268	if (!m_angularOnly)
269	{
270	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
271	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
272	btVector3 relPos = pivotBInW - pivotAInW;
273
274	btVector3 normal[3];
275	if (relPos.length2() > SIMD_EPSILON)
276	{
277	normal[0] = relPos.normalized();
278	}
279	else
280	{
281	normal[0].setValue(btScalar(1.0),0,0);
282	}
283
284	btPlaneSpace1(normal[0], normal[1], normal[2]);
285
286	for (int i=0;i<3;i++)
287	{
288	new (&m_jac[i]) btJacobianEntry(
289	m_rbA.getCenterOfMassTransform().getBasis().transpose(),
290	m_rbB.getCenterOfMassTransform().getBasis().transpose(),
291	pivotAInW - m_rbA.getCenterOfMassPosition(),
292	pivotBInW - m_rbB.getCenterOfMassPosition(),
293	normal[i],
294	m_rbA.getInvInertiaDiagLocal(),
295	m_rbA.getInvMass(),
296	m_rbB.getInvInertiaDiagLocal(),
297	m_rbB.getInvMass());
298	}
299	}
300
301	calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
302	}
303	}
304
305
306
307	void btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep)
308	{
309	#ifndef __SPU__
310	if (m_useSolveConstraintObsolete)
311	{
312	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
313	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
314
315	btScalar tau = btScalar(0.3);
316
317	//linear part
318	if (!m_angularOnly)
319	{
320	btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
321	btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
322
323	btVector3 vel1;
324	bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
325	btVector3 vel2;
326	bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
327	btVector3 vel = vel1 - vel2;
328
329	for (int i=0;i<3;i++)
330	{
331	const btVector3& normal = m_jac[i].m_linearJointAxis;
332	btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
333
334	btScalar rel_vel;
335	rel_vel = normal.dot(vel);
336	//positional error (zeroth order error)
337	btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
338	btScalar impulse = depthtau/timeStep jacDiagABInv - rel_vel * jacDiagABInv;
339	m_appliedImpulse += impulse;
340
341	btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
342	btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
343	bodyA.internalApplyImpulse(normalm_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()ftorqueAxis1,impulse);
344	bodyB.internalApplyImpulse(normalm_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()ftorqueAxis2,-impulse);
345
346	}
347	}
348
349	// apply motor
350	if (m_bMotorEnabled)
351	{
352	// compute current and predicted transforms
353	btTransform trACur = m_rbA.getCenterOfMassTransform();
354	btTransform trBCur = m_rbB.getCenterOfMassTransform();
355	btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
356	btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
357	btTransform trAPred; trAPred.setIdentity();
358	btVector3 zerovec(0,0,0);
359	btTransformUtil::integrateTransform(
360	trACur, zerovec, omegaA, timeStep, trAPred);
361	btTransform trBPred; trBPred.setIdentity();
362	btTransformUtil::integrateTransform(
363	trBCur, zerovec, omegaB, timeStep, trBPred);
364
365	// compute desired transforms in world
366	btTransform trPose(m_qTarget);
367	btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
368	btTransform trADes = trBPred * trABDes;
369	btTransform trBDes = trAPred * trABDes.inverse();
370
371	// compute desired omegas in world
372	btVector3 omegaADes, omegaBDes;
373
374	btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
375	btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
376
377	// compute delta omegas
378	btVector3 dOmegaA = omegaADes - omegaA;
379	btVector3 dOmegaB = omegaBDes - omegaB;
380
381	// compute weighted avg axis of dOmega (weighting based on inertias)
382	btVector3 axisA, axisB;
383	btScalar kAxisAInv = 0, kAxisBInv = 0;
384
385	if (dOmegaA.length2() > SIMD_EPSILON)
386	{
387	axisA = dOmegaA.normalized();
388	kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
389	}
390
391	if (dOmegaB.length2() > SIMD_EPSILON)
392	{
393	axisB = dOmegaB.normalized();
394	kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
395	}
396
397	btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
398
399	static bool bDoTorque = true;
400	if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
401	{
402	avgAxis.normalize();
403	kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
404	kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
405	btScalar kInvCombined = kAxisAInv + kAxisBInv;
406
407	btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
408	(kInvCombined * kInvCombined);
409
410	if (m_maxMotorImpulse >= 0)
411	{
412	btScalar fMaxImpulse = m_maxMotorImpulse;
413	if (m_bNormalizedMotorStrength)
414	fMaxImpulse = fMaxImpulse/kAxisAInv;
415
416	btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
417	btScalar newUnclampedMag = newUnclampedAccImpulse.length();
418	if (newUnclampedMag > fMaxImpulse)
419	{
420	newUnclampedAccImpulse.normalize();
421	newUnclampedAccImpulse *= fMaxImpulse;
422	impulse = newUnclampedAccImpulse - m_accMotorImpulse;
423	}
424	m_accMotorImpulse += impulse;
425	}
426
427	btScalar impulseMag = impulse.length();
428	btVector3 impulseAxis = impulse / impulseMag;
429
430	bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
431	bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
432
433	}
434	}
435	else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
436	{
437	btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
438	btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
439	btVector3 relVel = angVelB - angVelA;
440	if (relVel.length2() > SIMD_EPSILON)
441	{
442	btVector3 relVelAxis = relVel.normalized();
443	btScalar m_kDamping = btScalar(1.) /
444	(getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
445	getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
446	btVector3 impulse = m_damping * m_kDamping * relVel;
447
448	btScalar impulseMag = impulse.length();
449	btVector3 impulseAxis = impulse / impulseMag;
450	bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
451	bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
452	}
453	}
454
455	// joint limits
456	{
457	///solve angular part
458	btVector3 angVelA;
459	bodyA.internalGetAngularVelocity(angVelA);
460	btVector3 angVelB;
461	bodyB.internalGetAngularVelocity(angVelB);
462
463	// solve swing limit
464	if (m_solveSwingLimit)
465	{
466	btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
467	btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
468	if (relSwingVel > 0)
469	amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
470	btScalar impulseMag = amplitude * m_kSwing;
471
472	// Clamp the accumulated impulse
473	btScalar temp = m_accSwingLimitImpulse;
474	m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
475	impulseMag = m_accSwingLimitImpulse - temp;
476
477	btVector3 impulse = m_swingAxis * impulseMag;
478
479	// don't let cone response affect twist
480	// (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
481	{
482	btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
483	btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
484	impulse = impulseNoTwistCouple;
485	}
486
487	impulseMag = impulse.length();
488	btVector3 noTwistSwingAxis = impulse / impulseMag;
489
490	bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
491	bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
492	}
493
494
495	// solve twist limit
496	if (m_solveTwistLimit)
497	{
498	btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
499	btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
500	if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
501	amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
502	btScalar impulseMag = amplitude * m_kTwist;
503
504	// Clamp the accumulated impulse
505	btScalar temp = m_accTwistLimitImpulse;
506	m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
507	impulseMag = m_accTwistLimitImpulse - temp;
508
509	btVector3 impulse = m_twistAxis * impulseMag;
510
511	bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
512	bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
513	}
514	}
515	}
516	#else
517	btAssert(0);
518	#endif //__SPU__
519	}
520
521
522
523
524	void btConeTwistConstraint::updateRHS(btScalar timeStep)
525	{
526	(void)timeStep;
527
528	}
529
530
531	#ifndef __SPU__
532	void btConeTwistConstraint::calcAngleInfo()
533	{
534	m_swingCorrection = btScalar(0.);
535	m_twistLimitSign = btScalar(0.);
536	m_solveTwistLimit = false;
537	m_solveSwingLimit = false;
538
539	btVector3 b1Axis1,b1Axis2,b1Axis3;
540	btVector3 b2Axis1,b2Axis2;
541
542	b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
543	b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
544
545	btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
546
547	btScalar swx=btScalar(0.),swy = btScalar(0.);
548	btScalar thresh = btScalar(10.);
549	btScalar fact;
550
551	// Get Frame into world space
552	if (m_swingSpan1 >= btScalar(0.05f))
553	{
554	b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
555	swx = b2Axis1.dot(b1Axis1);
556	swy = b2Axis1.dot(b1Axis2);
557	swing1 = btAtan2Fast(swy, swx);
558	fact = (swyswy + swxswx) * thresh * thresh;
559	fact = fact / (fact + btScalar(1.0));
560	swing1 *= fact;
561	}
562
563	if (m_swingSpan2 >= btScalar(0.05f))
564	{
565	b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
566	swx = b2Axis1.dot(b1Axis1);
567	swy = b2Axis1.dot(b1Axis3);
568	swing2 = btAtan2Fast(swy, swx);
569	fact = (swyswy + swxswx) * thresh * thresh;
570	fact = fact / (fact + btScalar(1.0));
571	swing2 *= fact;
572	}
573
574	btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
575	btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
576	btScalar EllipseAngle = btFabs(swing1swing1) RMaxAngle1Sq + btFabs(swing2swing2) RMaxAngle2Sq;
577
578	if (EllipseAngle > 1.0f)
579	{
580	m_swingCorrection = EllipseAngle-1.0f;
581	m_solveSwingLimit = true;
582	// Calculate necessary axis & factors
583	m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
584	m_swingAxis.normalize();
585	btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
586	m_swingAxis *= swingAxisSign;
587	}
588
589	// Twist limits
590	if (m_twistSpan >= btScalar(0.))
591	{
592	btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
593	btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
594	btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
595	btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
596	m_twistAngle = twist;
597
598	// btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
599	btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
600	if (twist <= -m_twistSpan*lockedFreeFactor)
601	{
602	m_twistCorrection = -(twist + m_twistSpan);
603	m_solveTwistLimit = true;
604	m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
605	m_twistAxis.normalize();
606	m_twistAxis *= -1.0f;
607	}
608	else if (twist > m_twistSpan*lockedFreeFactor)
609	{
610	m_twistCorrection = (twist - m_twistSpan);
611	m_solveTwistLimit = true;
612	m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
613	m_twistAxis.normalize();
614	}
615	}
616	}
617	#endif //__SPU__
618
619	static btVector3 vTwist(1,0,0); // twist axis in constraint's space
620
621
622
623	void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
624	{
625	m_swingCorrection = btScalar(0.);
626	m_twistLimitSign = btScalar(0.);
627	m_solveTwistLimit = false;
628	m_solveSwingLimit = false;
629	// compute rotation of A wrt B (in constraint space)
630	if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
631	{ // it is assumed that setMotorTarget() was alredy called
632	// and motor target m_qTarget is within constraint limits
633	// TODO : split rotation to pure swing and pure twist
634	// compute desired transforms in world
635	btTransform trPose(m_qTarget);
636	btTransform trA = transA * m_rbAFrame;
637	btTransform trB = transB * m_rbBFrame;
638	btTransform trDeltaAB = trB * trPose * trA.inverse();
639	btQuaternion qDeltaAB = trDeltaAB.getRotation();
640	btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
641	m_swingAxis = swingAxis;
642	m_swingAxis.normalize();
643	m_swingCorrection = qDeltaAB.getAngle();
644	if(!btFuzzyZero(m_swingCorrection))
645	{
646	m_solveSwingLimit = true;
647	}
648	return;
649	}
650
651
652	{
653	// compute rotation of A wrt B (in constraint space)
654	btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
655	btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
656	btQuaternion qAB = qB.inverse() * qA;
657	// split rotation into cone and twist
658	// (all this is done from B's perspective. Maybe I should be averaging axes...)
659	btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
660	btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
661	btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
662
663	if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
664	{
665	btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
666	computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
667
668	if (swingAngle > swingLimit * m_limitSoftness)
669	{
670	m_solveSwingLimit = true;
671
672	// compute limit ratio: 0->1, where
673	// 0 == beginning of soft limit
674	// 1 == hard/real limit
675	m_swingLimitRatio = 1.f;
676	if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
677	{
678	m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
679	(swingLimit - swingLimit * m_limitSoftness);
680	}
681
682	// swing correction tries to get back to soft limit
683	m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
684
685	// adjustment of swing axis (based on ellipse normal)
686	adjustSwingAxisToUseEllipseNormal(swingAxis);
687
688	// Calculate necessary axis & factors
689	m_swingAxis = quatRotate(qB, -swingAxis);
690
691	m_twistAxisA.setValue(0,0,0);
692
693	m_kSwing = btScalar(1.) /
694	(computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
695	computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
696	}
697	}
698	else
699	{
700	// you haven't set any limits;
701	// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
702	// anyway, we have either hinge or fixed joint
703	btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
704	btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
705	btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
706	btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
707	btVector3 target;
708	btScalar x = ivB.dot(ivA);
709	btScalar y = ivB.dot(jvA);
710	btScalar z = ivB.dot(kvA);
711	if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
712	{ // fixed. We'll need to add one more row to constraint
713	if((!btFuzzyZero(y)) \|\| (!(btFuzzyZero(z))))
714	{
715	m_solveSwingLimit = true;
716	m_swingAxis = -ivB.cross(ivA);
717	}
718	}
719	else
720	{
721	if(m_swingSpan1 < m_fixThresh)
722	{ // hinge around Y axis
723	if(!(btFuzzyZero(y)))
724	{
725	m_solveSwingLimit = true;
726	if(m_swingSpan2 >= m_fixThresh)
727	{
728	y = btScalar(0.f);
729	btScalar span2 = btAtan2(z, x);
730	if(span2 > m_swingSpan2)
731	{
732	x = btCos(m_swingSpan2);
733	z = btSin(m_swingSpan2);
734	}
735	else if(span2 < -m_swingSpan2)
736	{
737	x = btCos(m_swingSpan2);
738	z = -btSin(m_swingSpan2);
739	}
740	}
741	}
742	}
743	else
744	{ // hinge around Z axis
745	if(!btFuzzyZero(z))
746	{
747	m_solveSwingLimit = true;
748	if(m_swingSpan1 >= m_fixThresh)
749	{
750	z = btScalar(0.f);
751	btScalar span1 = btAtan2(y, x);
752	if(span1 > m_swingSpan1)
753	{
754	x = btCos(m_swingSpan1);
755	y = btSin(m_swingSpan1);
756	}
757	else if(span1 < -m_swingSpan1)
758	{
759	x = btCos(m_swingSpan1);
760	y = -btSin(m_swingSpan1);
761	}
762	}
763	}
764	}
765	target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
766	target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
767	target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
768	target.normalize();
769	m_swingAxis = -ivB.cross(target);
770	m_swingCorrection = m_swingAxis.length();
771	m_swingAxis.normalize();
772	}
773	}
774
775	if (m_twistSpan >= btScalar(0.f))
776	{
777	btVector3 twistAxis;
778	computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
779
780	if (m_twistAngle > m_twistSpan*m_limitSoftness)
781	{
782	m_solveTwistLimit = true;
783
784	m_twistLimitRatio = 1.f;
785	if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
786	{
787	m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
788	(m_twistSpan - m_twistSpan * m_limitSoftness);
789	}
790
791	// twist correction tries to get back to soft limit
792	m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
793
794	m_twistAxis = quatRotate(qB, -twistAxis);
795
796	m_kTwist = btScalar(1.) /
797	(computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
798	computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
799	}
800
801	if (m_solveSwingLimit)
802	m_twistAxisA = quatRotate(qA, -twistAxis);
803	}
804	else
805	{
806	m_twistAngle = btScalar(0.f);
807	}
808	}
809	}
810
811
812
813	// given a cone rotation in constraint space, (pre: twist must already be removed)
814	// this method computes its corresponding swing angle and axis.
815	// more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
816	void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
817	btScalar& swingAngle, // out
818	btVector3& vSwingAxis, // out
819	btScalar& swingLimit) // out
820	{
821	swingAngle = qCone.getAngle();
822	if (swingAngle > SIMD_EPSILON)
823	{
824	vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
825	vSwingAxis.normalize();
826	if (fabs(vSwingAxis.x()) > SIMD_EPSILON)
827	{
828	// non-zero twist?! this should never happen.
829	int wtf = 0; wtf = wtf;
830	}
831
832	// Compute limit for given swing. tricky:
833	// Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
834	// (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
835
836	// For starters, compute the direction from center to surface of ellipse.
837	// This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
838	// (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
839	btScalar xEllipse = vSwingAxis.y();
840	btScalar yEllipse = -vSwingAxis.z();
841
842	// Now, we use the slope of the vector (using x/yEllipse) and find the length
843	// of the line that intersects the ellipse:
844	// x^2 y^2
845	// --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
846	// a^2 b^2
847	// Do the math and it should be clear.
848
849	swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
850	if (fabs(xEllipse) > SIMD_EPSILON)
851	{
852	btScalar surfaceSlope2 = (yEllipseyEllipse)/(xEllipsexEllipse);
853	btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
854	norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
855	btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
856	swingLimit = sqrt(swingLimit2);
857	}
858
859	// test!
860	/*swingLimit = m_swingSpan2;
861	if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
862	{
863	btScalar mag_2 = m_swingSpan1m_swingSpan1 + m_swingSpan2m_swingSpan2;
864	btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
865	btScalar phi = asin(sinphi);
866	btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
867	btScalar alpha = 3.14159f - theta - phi;
868	btScalar sinalpha = sin(alpha);
869	swingLimit = m_swingSpan1 * sinphi/sinalpha;
870	}*/
871	}
872	else if (swingAngle < 0)
873	{
874	// this should never happen!
875	int wtf = 0; wtf = wtf;
876	}
877	}
878
879	btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
880	{
881	// compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
882	btScalar xEllipse = btCos(fAngleInRadians);
883	btScalar yEllipse = btSin(fAngleInRadians);
884
885	// Use the slope of the vector (using x/yEllipse) and find the length
886	// of the line that intersects the ellipse:
887	// x^2 y^2
888	// --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
889	// a^2 b^2
890	// Do the math and it should be clear.
891
892	float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
893	if (fabs(xEllipse) > SIMD_EPSILON)
894	{
895	btScalar surfaceSlope2 = (yEllipseyEllipse)/(xEllipsexEllipse);
896	btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
897	norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
898	btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
899	swingLimit = sqrt(swingLimit2);
900	}
901
902	// convert into point in constraint space:
903	// note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
904	btVector3 vSwingAxis(0, xEllipse, -yEllipse);
905	btQuaternion qSwing(vSwingAxis, swingLimit);
906	btVector3 vPointInConstraintSpace(fLength,0,0);
907	return quatRotate(qSwing, vPointInConstraintSpace);
908	}
909
910	// given a twist rotation in constraint space, (pre: cone must already be removed)
911	// this method computes its corresponding angle and axis.
912	void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
913	btScalar& twistAngle, // out
914	btVector3& vTwistAxis) // out
915	{
916	btQuaternion qMinTwist = qTwist;
917	twistAngle = qTwist.getAngle();
918
919	if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
920	{
921	qMinTwist = operator-(qTwist);
922	twistAngle = qMinTwist.getAngle();
923	}
924	if (twistAngle < 0)
925	{
926	// this should never happen
927	int wtf = 0; wtf = wtf;
928	}
929
930	vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
931	if (twistAngle > SIMD_EPSILON)
932	vTwistAxis.normalize();
933	}
934
935
936	void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
937	{
938	// the swing axis is computed as the "twist-free" cone rotation,
939	// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
940	// so, if we're outside the limits, the closest way back inside the cone isn't
941	// along the vector back to the center. better (and more stable) to use the ellipse normal.
942
943	// convert swing axis to direction from center to surface of ellipse
944	// (ie. rotate 2D vector by PI/2)
945	btScalar y = -vSwingAxis.z();
946	btScalar z = vSwingAxis.y();
947
948	// do the math...
949	if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
950	{
951	// compute gradient/normal of ellipse surface at current "point"
952	btScalar grad = y/z;
953	grad *= m_swingSpan2 / m_swingSpan1;
954
955	// adjust y/z to represent normal at point (instead of vector to point)
956	if (y > 0)
957	y = fabs(grad * z);
958	else
959	y = -fabs(grad * z);
960
961	// convert ellipse direction back to swing axis
962	vSwingAxis.setZ(-y);
963	vSwingAxis.setY( z);
964	vSwingAxis.normalize();
965	}
966	}
967
968
969
970	void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
971	{
972	btTransform trACur = m_rbA.getCenterOfMassTransform();
973	btTransform trBCur = m_rbB.getCenterOfMassTransform();
974	btTransform trABCur = trBCur.inverse() * trACur;
975	btQuaternion qABCur = trABCur.getRotation();
976	btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
977	btQuaternion qConstraintCur = trConstraintCur.getRotation();
978
979	btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
980	setMotorTargetInConstraintSpace(qConstraint);
981	}
982
983
984	void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
985	{
986	m_qTarget = q;
987
988	// clamp motor target to within limits
989	{
990	btScalar softness = 1.f;//m_limitSoftness;
991
992	// split into twist and cone
993	btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
994	btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
995	btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
996
997	// clamp cone
998	if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
999	{
1000	btScalar swingAngle, swingLimit; btVector3 swingAxis;
1001	computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
1002
1003	if (fabs(swingAngle) > SIMD_EPSILON)
1004	{
1005	if (swingAngle > swingLimit*softness)
1006	swingAngle = swingLimit*softness;
1007	else if (swingAngle < -swingLimit*softness)
1008	swingAngle = -swingLimit*softness;
1009	qTargetCone = btQuaternion(swingAxis, swingAngle);
1010	}
1011	}
1012
1013	// clamp twist
1014	if (m_twistSpan >= btScalar(0.05f))
1015	{
1016	btScalar twistAngle; btVector3 twistAxis;
1017	computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
1018
1019	if (fabs(twistAngle) > SIMD_EPSILON)
1020	{
1021	// eddy todo: limitSoftness used here???
1022	if (twistAngle > m_twistSpan*softness)
1023	twistAngle = m_twistSpan*softness;
1024	else if (twistAngle < -m_twistSpan*softness)
1025	twistAngle = -m_twistSpan*softness;
1026	qTargetTwist = btQuaternion(twistAxis, twistAngle);
1027	}
1028	}
1029
1030	m_qTarget = qTargetCone * qTargetTwist;
1031	}
1032	}
1033
1034	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
1035	///If no axis is provided, it uses the default axis for this constraint.
1036	void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
1037	{
1038	switch(num)
1039	{
1040	case BT_CONSTRAINT_ERP :
1041	case BT_CONSTRAINT_STOP_ERP :
1042	if((axis >= 0) && (axis < 3))
1043	{
1044	m_linERP = value;
1045	m_flags \|= BT_CONETWIST_FLAGS_LIN_ERP;
1046	}
1047	else
1048	{
1049	m_biasFactor = value;
1050	}
1051	break;
1052	case BT_CONSTRAINT_CFM :
1053	case BT_CONSTRAINT_STOP_CFM :
1054	if((axis >= 0) && (axis < 3))
1055	{
1056	m_linCFM = value;
1057	m_flags \|= BT_CONETWIST_FLAGS_LIN_CFM;
1058	}
1059	else
1060	{
1061	m_angCFM = value;
1062	m_flags \|= BT_CONETWIST_FLAGS_ANG_CFM;
1063	}
1064	break;
1065	default:
1066	btAssertConstrParams(0);
1067	break;
1068	}
1069	}
1070
1071	///return the local value of parameter
1072	btScalar btConeTwistConstraint::getParam(int num, int axis) const
1073	{
1074	btScalar retVal = 0;
1075	switch(num)
1076	{
1077	case BT_CONSTRAINT_ERP :
1078	case BT_CONSTRAINT_STOP_ERP :
1079	if((axis >= 0) && (axis < 3))
1080	{
1081	btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
1082	retVal = m_linERP;
1083	}
1084	else if((axis >= 3) && (axis < 6))
1085	{
1086	retVal = m_biasFactor;
1087	}
1088	else
1089	{
1090	btAssertConstrParams(0);
1091	}
1092	break;
1093	case BT_CONSTRAINT_CFM :
1094	case BT_CONSTRAINT_STOP_CFM :
1095	if((axis >= 0) && (axis < 3))
1096	{
1097	btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
1098	retVal = m_linCFM;
1099	}
1100	else if((axis >= 3) && (axis < 6))
1101	{
1102	btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
1103	retVal = m_angCFM;
1104	}
1105	else
1106	{
1107	btAssertConstrParams(0);
1108	}
1109	break;
1110	default :
1111	btAssertConstrParams(0);
1112	}
1113	return retVal;
1114	}
1115
1116
1117

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

Download in other formats: