Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

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

Last change on this file since 5942 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: 23.7 KB

Rev	Line
[1963]	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>
[2882]	22	#include "btSolverBody.h"
[1963]	23
[2882]	24	//-----------------------------------------------------------------------------
[1963]	25
[2882]	26	#define HINGE_USE_OBSOLETE_SOLVER false
	27
	28	//-----------------------------------------------------------------------------
	29
	30
[1963]	31	btHingeConstraint::btHingeConstraint()
	32	: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
[2882]	33	m_enableAngularMotor(false),
	34	m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
	35	m_useReferenceFrameA(false)
[1963]	36	{
[2882]	37	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
[1963]	38	}
	39
[2882]	40	//-----------------------------------------------------------------------------
	41
[1963]	42	btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
[2882]	43	btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
[1963]	44	:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
	45	m_angularOnly(false),
[2882]	46	m_enableAngularMotor(false),
	47	m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
	48	m_useReferenceFrameA(useReferenceFrameA)
[1963]	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;
[2882]	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() );
[1963]	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;
[2882]	88	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
[1963]	89	}
	90
[2882]	91	//-----------------------------------------------------------------------------
[1963]	92
[2882]	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)
[1963]	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
[2882]	109	btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
[1963]	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;
[2882]	128	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
[1963]	129	}
	130
[2882]	131	//-----------------------------------------------------------------------------
	132
[1963]	133	btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB,
[2882]	134	const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
[1963]	135	:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
	136	m_angularOnly(false),
[2882]	137	m_enableAngularMotor(false),
	138	m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
	139	m_useReferenceFrameA(useReferenceFrameA)
[1963]	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;
[2882]	148	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
[1963]	149	}
	150
[2882]	151	//-----------------------------------------------------------------------------
[1963]	152
[2882]	153	btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
[1963]	154	:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
	155	m_angularOnly(false),
[2882]	156	m_enableAngularMotor(false),
	157	m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
	158	m_useReferenceFrameA(useReferenceFrameA)
[1963]	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;
[2882]	171	m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
[1963]	172	}
	173
[2882]	174	//-----------------------------------------------------------------------------
	175
[1963]	176	void btHingeConstraint::buildJacobian()
	177	{
[2882]	178	if (m_useSolveConstraintObsolete)
[1963]	179	{
[2882]	180	m_appliedImpulse = btScalar(0.);
[1963]	181
[2882]	182	if (!m_angularOnly)
[1963]	183	{
[2882]	184	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
	185	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
	186	btVector3 relPos = pivotBInW - pivotAInW;
[1963]	187
[2882]	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	}
[1963]	197
[2882]	198	btPlaneSpace1(normal[0], normal[1], normal[2]);
	199
	200	for (int i=0;i<3;i++)
	201	{
	202	new (&m_jac[i]) btJacobianEntry(
[1963]	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());
[2882]	212	}
[1963]	213	}
	214
[2882]	215	//calculate two perpendicular jointAxis, orthogonal to hingeAxis
	216	//these two jointAxis require equal angular velocities for both bodies
[1963]	217
[2882]	218	//this is unused for now, it's a todo
	219	btVector3 jointAxis0local;
	220	btVector3 jointAxis1local;
[1963]	221
[2882]	222	btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
[1963]	223
[2882]	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());
[1963]	234
[2882]	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());
[1963]	240
[2882]	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());
[1963]	246
[2882]	247	// clear accumulator
	248	m_accLimitImpulse = btScalar(0.);
[1963]	249
[2882]	250	// test angular limit
	251	testLimit();
[1963]	252
[2882]	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)
[1963]	266	{
[2882]	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())
[1963]	277	{
[2882]	278	info->m_numConstraintRows++; // limit 3rd anguar as well
	279	info->nub--;
[1963]	280	}
	281	}
[2882]	282	} // btHingeConstraint::getInfo1 ()
[1963]	283
[2882]	284	//-----------------------------------------------------------------------------
[1963]	285
[2882]	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
[1963]	468	}
	469
[2882]	470	//-----------------------------------------------------------------------------
	471
	472	void btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
[1963]	473	{
	474
[2882]	475	///for backwards compatibility during the transition to 'getInfo/getInfo2'
	476	if (m_useSolveConstraintObsolete)
	477	{
[1963]	478
[2882]	479	btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
	480	btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
[1963]	481
[2882]	482	btScalar tau = btScalar(0.3);
[1963]	483
[2882]	484	//linear part
	485	if (!m_angularOnly)
	486	{
	487	btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
	488	btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
[1963]	489
[2882]	490	btVector3 vel1,vel2;
	491	bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
	492	bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
	493	btVector3 vel = vel1 - vel2;
[1963]	494
[2882]	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	}
[1963]	512	}
	513
[2882]	514
	515	{
	516	///solve angular part
[1963]	517
[2882]	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);
[1963]	521
[2882]	522	btVector3 angVelA;
	523	bodyA.getAngularVelocity(angVelA);
	524	btVector3 angVelB;
	525	bodyB.getAngularVelocity(angVelB);
[1963]	526
[2882]	527	btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
	528	btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
[1963]	529
[2882]	530	btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
	531	btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
	532	btVector3 velrelOrthog = angAorthog-angBorthog;
[1963]	533	{
[2882]	534
[1963]	535
[2882]	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;
[1963]	546
[2882]	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));
[1963]	549
[2882]	550	}
[1963]	551
[2882]	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));
[1963]	564
[2882]	565	}
	566
	567
[1963]	568
	569
[2882]	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	}
[1963]	589	}
	590
[2882]	591	//apply motor
	592	if (m_enableAngularMotor)
	593	{
	594	//todo: add limits too
	595	btVector3 angularLimit(0,0,0);
[1963]	596
[2882]	597	btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
	598	btScalar projRelVel = velrel.dot(axisA);
[1963]	599
[2882]	600	btScalar desiredMotorVel = m_motorTargetVelocity;
	601	btScalar motor_relvel = desiredMotorVel - projRelVel;
[1963]	602
[2882]	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;
[1963]	608
[2882]	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	}
[1963]	613	}
	614	}
	615
	616	}
	617
[2882]	618	//-----------------------------------------------------------------------------
	619
[1963]	620	void btHingeConstraint::updateRHS(btScalar timeStep)
	621	{
	622	(void)timeStep;
	623
	624	}
	625
[2882]	626	//-----------------------------------------------------------------------------
	627
[1963]	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);
[2882]	633	btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
	634	return m_referenceSign * angle;
[1963]	635	}
	636
[2882]	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: