Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @ 8008

Last change on this file since 8008 was 5781, checked in by rgrieder, 15 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: 23.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
17	#include "btHingeConstraint.h"
18	#include "BulletDynamics/Dynamics/btRigidBody.h"
19	#include "LinearMath/btTransformUtil.h"
20	#include "LinearMath/btMinMax.h"
21	#include <new>
22	#include "btSolverBody.h"
23
24	//-----------------------------------------------------------------------------
25
26	#define HINGE_USE_OBSOLETE_SOLVER false
27
28	//-----------------------------------------------------------------------------
29
30
31	btHingeConstraint::btHingeConstraint()
32	: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
33	m_enableAngularMotor(false),
34	m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
35	m_useReferenceFrameA(false)
36	{
37	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
38	}
39
40	//-----------------------------------------------------------------------------
41
42	btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
43	btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
44	:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
45	m_angularOnly(false),
46	m_enableAngularMotor(false),
47	m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
48	m_useReferenceFrameA(useReferenceFrameA)
49	{
50	m_rbAFrame.getOrigin() = pivotInA;
51
52	// since no frame is given, assume this to be zero angle and just pick rb transform axis
53	btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
54
55	btVector3 rbAxisA2;
56	btScalar projection = axisInA.dot(rbAxisA1);
57	if (projection >= 1.0f - SIMD_EPSILON) {
58	rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
59	rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
60	} else if (projection <= -1.0f + SIMD_EPSILON) {
61	rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
62	rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
63	} else {
64	rbAxisA2 = axisInA.cross(rbAxisA1);
65	rbAxisA1 = rbAxisA2.cross(axisInA);
66	}
67
68	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
69	rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
70	rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
71
72	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
73	btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
74	btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
75
76	m_rbBFrame.getOrigin() = pivotInB;
77	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
78	rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
79	rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
80
81	//start with free
82	m_lowerLimit = btScalar(1e30);
83	m_upperLimit = btScalar(-1e30);
84	m_biasFactor = 0.3f;
85	m_relaxationFactor = 1.0f;
86	m_limitSoftness = 0.9f;
87	m_solveLimit = false;
88	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
89	}
90
91	//-----------------------------------------------------------------------------
92
93	btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
94	:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false),
95	m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
96	m_useReferenceFrameA(useReferenceFrameA)
97	{
98
99	// since no frame is given, assume this to be zero angle and just pick rb transform axis
100	// fixed axis in worldspace
101	btVector3 rbAxisA1, rbAxisA2;
102	btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
103
104	m_rbAFrame.getOrigin() = pivotInA;
105	m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
106	rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
107	rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
108
109	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
110
111	btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
112	btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
113	btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
114
115
116	m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
117	m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
118	rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
119	rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
120
121	//start with free
122	m_lowerLimit = btScalar(1e30);
123	m_upperLimit = btScalar(-1e30);
124	m_biasFactor = 0.3f;
125	m_relaxationFactor = 1.0f;
126	m_limitSoftness = 0.9f;
127	m_solveLimit = false;
128	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
129	}
130
131	//-----------------------------------------------------------------------------
132
133	btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
134	const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
135	:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
136	m_angularOnly(false),
137	m_enableAngularMotor(false),
138	m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
139	m_useReferenceFrameA(useReferenceFrameA)
140	{
141	//start with free
142	m_lowerLimit = btScalar(1e30);
143	m_upperLimit = btScalar(-1e30);
144	m_biasFactor = 0.3f;
145	m_relaxationFactor = 1.0f;
146	m_limitSoftness = 0.9f;
147	m_solveLimit = false;
148	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
149	}
150
151	//-----------------------------------------------------------------------------
152
153	btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
154	:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
155	m_angularOnly(false),
156	m_enableAngularMotor(false),
157	m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
158	m_useReferenceFrameA(useReferenceFrameA)
159	{
160	///not providing rigidbody B means implicitly using worldspace for body B
161
162	m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
163
164	//start with free
165	m_lowerLimit = btScalar(1e30);
166	m_upperLimit = btScalar(-1e30);
167	m_biasFactor = 0.3f;
168	m_relaxationFactor = 1.0f;
169	m_limitSoftness = 0.9f;
170	m_solveLimit = false;
171	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
172	}
173
174	//-----------------------------------------------------------------------------
175
176	void btHingeConstraint::buildJacobian()
177	{
178	if (m_useSolveConstraintObsolete)
179	{
180	m_appliedImpulse = btScalar(0.);
181
182	if (!m_angularOnly)
183	{
184	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
185	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
186	btVector3 relPos = pivotBInW - pivotAInW;
187
188	btVector3 normal[3];
189	if (relPos.length2() > SIMD_EPSILON)
190	{
191	normal[0] = relPos.normalized();
192	}
193	else
194	{
195	normal[0].setValue(btScalar(1.0),0,0);
196	}
197
198	btPlaneSpace1(normal[0], normal[1], normal[2]);
199
200	for (int i=0;i<3;i++)
201	{
202	new (&m_jac[i]) btJacobianEntry(
203	m_rbA.getCenterOfMassTransform().getBasis().transpose(),
204	m_rbB.getCenterOfMassTransform().getBasis().transpose(),
205	pivotAInW - m_rbA.getCenterOfMassPosition(),
206	pivotBInW - m_rbB.getCenterOfMassPosition(),
207	normal[i],
208	m_rbA.getInvInertiaDiagLocal(),
209	m_rbA.getInvMass(),
210	m_rbB.getInvInertiaDiagLocal(),
211	m_rbB.getInvMass());
212	}
213	}
214
215	//calculate two perpendicular jointAxis, orthogonal to hingeAxis
216	//these two jointAxis require equal angular velocities for both bodies
217
218	//this is unused for now, it's a todo
219	btVector3 jointAxis0local;
220	btVector3 jointAxis1local;
221
222	btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
223
224	getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
225	btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
226	btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
227	btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
228
229	new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
230	m_rbA.getCenterOfMassTransform().getBasis().transpose(),
231	m_rbB.getCenterOfMassTransform().getBasis().transpose(),
232	m_rbA.getInvInertiaDiagLocal(),
233	m_rbB.getInvInertiaDiagLocal());
234
235	new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
236	m_rbA.getCenterOfMassTransform().getBasis().transpose(),
237	m_rbB.getCenterOfMassTransform().getBasis().transpose(),
238	m_rbA.getInvInertiaDiagLocal(),
239	m_rbB.getInvInertiaDiagLocal());
240
241	new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
242	m_rbA.getCenterOfMassTransform().getBasis().transpose(),
243	m_rbB.getCenterOfMassTransform().getBasis().transpose(),
244	m_rbA.getInvInertiaDiagLocal(),
245	m_rbB.getInvInertiaDiagLocal());
246
247	// clear accumulator
248	m_accLimitImpulse = btScalar(0.);
249
250	// test angular limit
251	testLimit();
252
253	//Compute K = JWJ' for hinge axis
254	btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
255	m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
256	getRigidBodyB().computeAngularImpulseDenominator(axisA));
257
258	}
259	}
260
261	//-----------------------------------------------------------------------------
262
263	void btHingeConstraint::getInfo1(btConstraintInfo1* info)
264	{
265	if (m_useSolveConstraintObsolete)
266	{
267	info->m_numConstraintRows = 0;
268	info->nub = 0;
269	}
270	else
271	{
272	info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
273	info->nub = 1;
274	//prepare constraint
275	testLimit();
276	if(getSolveLimit() \|\| getEnableAngularMotor())
277	{
278	info->m_numConstraintRows++; // limit 3rd anguar as well
279	info->nub--;
280	}
281	}
282	} // btHingeConstraint::getInfo1 ()
283
284	//-----------------------------------------------------------------------------
285
286	void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
287	{
288	btAssert(!m_useSolveConstraintObsolete);
289	int i, s = info->rowskip;
290	// transforms in world space
291	btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
292	btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
293	// pivot point
294	btVector3 pivotAInW = trA.getOrigin();
295	btVector3 pivotBInW = trB.getOrigin();
296	// linear (all fixed)
297	info->m_J1linearAxis[0] = 1;
298	info->m_J1linearAxis[s + 1] = 1;
299	info->m_J1linearAxis[2 * s + 2] = 1;
300	btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
301	{
302	btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
303	btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
304	btVector3* angular2 = (btVector3)(info->m_J1angularAxis + 2 s);
305	btVector3 a1neg = -a1;
306	a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
307	}
308	btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
309	{
310	btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
311	btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
312	btVector3* angular2 = (btVector3)(info->m_J2angularAxis + 2 s);
313	a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
314	}
315	// linear RHS
316	btScalar k = info->fps * info->erp;
317	for(i = 0; i < 3; i++)
318	{
319	info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
320	}
321	// make rotations around X and Y equal
322	// the hinge axis should be the only unconstrained
323	// rotational axis, the angular velocity of the two bodies perpendicular to
324	// the hinge axis should be equal. thus the constraint equations are
325	// pw1 - pw2 = 0
326	// qw1 - qw2 = 0
327	// where p and q are unit vectors normal to the hinge axis, and w1 and w2
328	// are the angular velocity vectors of the two bodies.
329	// get hinge axis (Z)
330	btVector3 ax1 = trA.getBasis().getColumn(2);
331	// get 2 orthos to hinge axis (X, Y)
332	btVector3 p = trA.getBasis().getColumn(0);
333	btVector3 q = trA.getBasis().getColumn(1);
334	// set the two hinge angular rows
335	int s3 = 3 * info->rowskip;
336	int s4 = 4 * info->rowskip;
337
338	info->m_J1angularAxis[s3 + 0] = p[0];
339	info->m_J1angularAxis[s3 + 1] = p[1];
340	info->m_J1angularAxis[s3 + 2] = p[2];
341	info->m_J1angularAxis[s4 + 0] = q[0];
342	info->m_J1angularAxis[s4 + 1] = q[1];
343	info->m_J1angularAxis[s4 + 2] = q[2];
344
345	info->m_J2angularAxis[s3 + 0] = -p[0];
346	info->m_J2angularAxis[s3 + 1] = -p[1];
347	info->m_J2angularAxis[s3 + 2] = -p[2];
348	info->m_J2angularAxis[s4 + 0] = -q[0];
349	info->m_J2angularAxis[s4 + 1] = -q[1];
350	info->m_J2angularAxis[s4 + 2] = -q[2];
351	// compute the right hand side of the constraint equation. set relative
352	// body velocities along p and q to bring the hinge back into alignment.
353	// if ax1,ax2 are the unit length hinge axes as computed from body1 and
354	// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
355	// if `theta' is the angle between ax1 and ax2, we need an angular velocity
356	// along u to cover angle erp*theta in one step :
357	// \|angular_velocity\| = angle/time = erp*theta / stepsize
358	// = (erpfps) theta
359	// angular_velocity = \|angular_velocity\| * (ax1 x ax2) / \|ax1 x ax2\|
360	// = (erpfps) theta * (ax1 x ax2) / sin(theta)
361	// ...as ax1 and ax2 are unit length. if theta is smallish,
362	// theta ~= sin(theta), so
363	// angular_velocity = (erpfps) (ax1 x ax2)
364	// ax1 x ax2 is in the plane space of ax1, so we project the angular
365	// velocity to p and q to find the right hand side.
366	btVector3 ax2 = trB.getBasis().getColumn(2);
367	btVector3 u = ax1.cross(ax2);
368	info->m_constraintError[s3] = k * u.dot(p);
369	info->m_constraintError[s4] = k * u.dot(q);
370	// check angular limits
371	int nrow = 4; // last filled row
372	int srow;
373	btScalar limit_err = btScalar(0.0);
374	int limit = 0;
375	if(getSolveLimit())
376	{
377	limit_err = m_correction * m_referenceSign;
378	limit = (limit_err > btScalar(0.0)) ? 1 : 2;
379	}
380	// if the hinge has joint limits or motor, add in the extra row
381	int powered = 0;
382	if(getEnableAngularMotor())
383	{
384	powered = 1;
385	}
386	if(limit \|\| powered)
387	{
388	nrow++;
389	srow = nrow * info->rowskip;
390	info->m_J1angularAxis[srow+0] = ax1[0];
391	info->m_J1angularAxis[srow+1] = ax1[1];
392	info->m_J1angularAxis[srow+2] = ax1[2];
393
394	info->m_J2angularAxis[srow+0] = -ax1[0];
395	info->m_J2angularAxis[srow+1] = -ax1[1];
396	info->m_J2angularAxis[srow+2] = -ax1[2];
397
398	btScalar lostop = getLowerLimit();
399	btScalar histop = getUpperLimit();
400	if(limit && (lostop == histop))
401	{ // the joint motor is ineffective
402	powered = 0;
403	}
404	info->m_constraintError[srow] = btScalar(0.0f);
405	if(powered)
406	{
407	info->cfm[srow] = btScalar(0.0);
408	btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
409	info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
410	info->m_lowerLimit[srow] = - m_maxMotorImpulse;
411	info->m_upperLimit[srow] = m_maxMotorImpulse;
412	}
413	if(limit)
414	{
415	k = info->fps * info->erp;
416	info->m_constraintError[srow] += k * limit_err;
417	info->cfm[srow] = btScalar(0.0);
418	if(lostop == histop)
419	{
420	// limited low and high simultaneously
421	info->m_lowerLimit[srow] = -SIMD_INFINITY;
422	info->m_upperLimit[srow] = SIMD_INFINITY;
423	}
424	else if(limit == 1)
425	{ // low limit
426	info->m_lowerLimit[srow] = 0;
427	info->m_upperLimit[srow] = SIMD_INFINITY;
428	}
429	else
430	{ // high limit
431	info->m_lowerLimit[srow] = -SIMD_INFINITY;
432	info->m_upperLimit[srow] = 0;
433	}
434	// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
435	btScalar bounce = m_relaxationFactor;
436	if(bounce > btScalar(0.0))
437	{
438	btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
439	vel -= m_rbB.getAngularVelocity().dot(ax1);
440	// only apply bounce if the velocity is incoming, and if the
441	// resulting c[] exceeds what we already have.
442	if(limit == 1)
443	{ // low limit
444	if(vel < 0)
445	{
446	btScalar newc = -bounce * vel;
447	if(newc > info->m_constraintError[srow])
448	{
449	info->m_constraintError[srow] = newc;
450	}
451	}
452	}
453	else
454	{ // high limit - all those computations are reversed
455	if(vel > 0)
456	{
457	btScalar newc = -bounce * vel;
458	if(newc < info->m_constraintError[srow])
459	{
460	info->m_constraintError[srow] = newc;
461	}
462	}
463	}
464	}
465	info->m_constraintError[srow] *= m_biasFactor;
466	} // if(limit)
467	} // if angular limit or powered
468	}
469
470	//-----------------------------------------------------------------------------
471
472	void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
473	{
474
475	///for backwards compatibility during the transition to 'getInfo/getInfo2'
476	if (m_useSolveConstraintObsolete)
477	{
478
479	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
480	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
481
482	btScalar tau = btScalar(0.3);
483
484	//linear part
485	if (!m_angularOnly)
486	{
487	btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
488	btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
489
490	btVector3 vel1,vel2;
491	bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
492	bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
493	btVector3 vel = vel1 - vel2;
494
495	for (int i=0;i<3;i++)
496	{
497	const btVector3& normal = m_jac[i].m_linearJointAxis;
498	btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
499
500	btScalar rel_vel;
501	rel_vel = normal.dot(vel);
502	//positional error (zeroth order error)
503	btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
504	btScalar impulse = depthtau/timeStep jacDiagABInv - rel_vel * jacDiagABInv;
505	m_appliedImpulse += impulse;
506	btVector3 impulse_vector = normal * impulse;
507	btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
508	btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
509	bodyA.applyImpulse(normalm_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()ftorqueAxis1,impulse);
510	bodyB.applyImpulse(normalm_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()ftorqueAxis2,-impulse);
511	}
512	}
513
514
515	{
516	///solve angular part
517
518	// get axes in world space
519	btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
520	btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(2);
521
522	btVector3 angVelA;
523	bodyA.getAngularVelocity(angVelA);
524	btVector3 angVelB;
525	bodyB.getAngularVelocity(angVelB);
526
527	btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
528	btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
529
530	btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
531	btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
532	btVector3 velrelOrthog = angAorthog-angBorthog;
533	{
534
535
536	//solve orthogonal angular velocity correction
537	btScalar relaxation = btScalar(1.);
538	btScalar len = velrelOrthog.length();
539	if (len > btScalar(0.00001))
540	{
541	btVector3 normal = velrelOrthog.normalized();
542	btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
543	getRigidBodyB().computeAngularImpulseDenominator(normal);
544	// scale for mass and relaxation
545	//velrelOrthog = (btScalar(1.)/denom) m_relaxationFactor;
546
547	bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
548	bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
549
550	}
551
552	//solve angular positional correction
553	btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/timeStep);
554	btScalar len2 = angularError.length();
555	if (len2>btScalar(0.00001))
556	{
557	btVector3 normal2 = angularError.normalized();
558	btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
559	getRigidBodyB().computeAngularImpulseDenominator(normal2);
560	//angularError = (btScalar(1.)/denom2) relaxation;
561
562	bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
563	bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
564
565	}
566
567
568
569
570
571	// solve limit
572	if (m_solveLimit)
573	{
574	btScalar amplitude = ( (angVelB - angVelA).dot( axisA )m_relaxationFactor + m_correction (btScalar(1.)/timeStep)m_biasFactor ) m_limitSign;
575
576	btScalar impulseMag = amplitude * m_kHinge;
577
578	// Clamp the accumulated impulse
579	btScalar temp = m_accLimitImpulse;
580	m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
581	impulseMag = m_accLimitImpulse - temp;
582
583
584
585	bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()axisA,impulseMag m_limitSign);
586	bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()axisA,-(impulseMag m_limitSign));
587
588	}
589	}
590
591	//apply motor
592	if (m_enableAngularMotor)
593	{
594	//todo: add limits too
595	btVector3 angularLimit(0,0,0);
596
597	btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
598	btScalar projRelVel = velrel.dot(axisA);
599
600	btScalar desiredMotorVel = m_motorTargetVelocity;
601	btScalar motor_relvel = desiredMotorVel - projRelVel;
602
603	btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
604	//todo: should clip against accumulated impulse
605	btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
606	clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
607	btVector3 motorImp = clippedMotorImpulse * axisA;
608
609	bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
610	bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
611
612	}
613	}
614	}
615
616	}
617
618	//-----------------------------------------------------------------------------
619
620	void btHingeConstraint::updateRHS(btScalar timeStep)
621	{
622	(void)timeStep;
623
624	}
625
626	//-----------------------------------------------------------------------------
627
628	btScalar btHingeConstraint::getHingeAngle()
629	{
630	const btVector3 refAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
631	const btVector3 refAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
632	const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
633	btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
634	return m_referenceSign * angle;
635	}
636
637	//-----------------------------------------------------------------------------
638
639	void btHingeConstraint::testLimit()
640	{
641	// Compute limit information
642	m_hingeAngle = getHingeAngle();
643	m_correction = btScalar(0.);
644	m_limitSign = btScalar(0.);
645	m_solveLimit = false;
646	if (m_lowerLimit <= m_upperLimit)
647	{
648	if (m_hingeAngle <= m_lowerLimit)
649	{
650	m_correction = (m_lowerLimit - m_hingeAngle);
651	m_limitSign = 1.0f;
652	m_solveLimit = true;
653	}
654	else if (m_hingeAngle >= m_upperLimit)
655	{
656	m_correction = m_upperLimit - m_hingeAngle;
657	m_limitSign = -1.0f;
658	m_solveLimit = true;
659	}
660	}
661	return;
662	} // btHingeConstraint::testLimit()
663
664	//-----------------------------------------------------------------------------
665	//-----------------------------------------------------------------------------
666	//-----------------------------------------------------------------------------

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

Download in other formats: