Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @ 7221

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

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

Download in other formats: