Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

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

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

Rev	Line
[1963]	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
[2882]	25	//-----------------------------------------------------------------------------
	26
	27	#define CONETWIST_USE_OBSOLETE_SOLVER false
	28	#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
	29
	30	//-----------------------------------------------------------------------------
	31
[1963]	32	btConeTwistConstraint::btConeTwistConstraint()
[2882]	33	:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE),
	34	m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
[1963]	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),
[2882]	42	m_angularOnly(false),
	43	m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
[1963]	44	{
[2882]	45	init();
[1963]	46	}
	47
	48	btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
	49	:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
[2882]	50	m_angularOnly(false),
	51	m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
[1963]	52	{
	53	m_rbBFrame = m_rbAFrame;
[2882]	54	init();
	55	}
[1963]	56
	57
[2882]	58	void btConeTwistConstraint::init()
[1963]	59	{
[2882]	60	m_angularOnly = false;
[1963]	61	m_solveTwistLimit = false;
	62	m_solveSwingLimit = false;
[2882]	63	m_bMotorEnabled = false;
	64	m_maxMotorImpulse = btScalar(-1);
[1963]	65
[2882]	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)
[1963]	77	{
[2882]	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	//-----------------------------------------------------------------------------
[1963]	105
[2882]	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))
[1963]	151	{
[2882]	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;
[1963]	176	}
	177	else
	178	{
[2882]	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;
[1963]	194	}
[2882]	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	{
[1963]	212
[2882]	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	//-----------------------------------------------------------------------------
[1963]	234
[2882]	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)
[1963]	244	{
[2882]	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(
[1963]	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());
[2882]	273	}
[1963]	274	}
[2882]	275
	276	calcAngleInfo2();
[1963]	277	}
[2882]	278	}
[1963]	279
[2882]	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
[1963]	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) );
[2882]	567	m_twistAngle = twist;
[1963]	568
[2882]	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.);
[1963]	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;
[2882]	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()
[1963]	588
	589
[2882]	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)
[1963]	619	{
[2882]	620	m_solveSwingLimit = true;
[1963]	621
[2882]	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	}
[1963]	631
[2882]	632	// swing correction tries to get back to soft limit
	633	m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
[1963]	634
[2882]	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));
[1963]	646	}
[2882]	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	}
[1963]	724
[2882]	725	if (m_twistSpan >= btScalar(0.f))
	726	{
	727	btVector3 twistAxis;
	728	computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
[1963]	729
[2882]	730	if (m_twistAngle > m_twistSpan*m_limitSoftness)
	731	{
	732	m_solveTwistLimit = true;
[1963]	733
[2882]	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	}
[1963]	740
[2882]	741	// twist correction tries to get back to soft limit
	742	m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
[1963]	743
[2882]	744	m_twistAxis = quatRotate(qB, -twistAxis);
[1963]	745
[2882]	746	m_kTwist = btScalar(1.) /
	747	(getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
	748	getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
	749	}
[1963]	750
[2882]	751	if (m_solveSwingLimit)
	752	m_twistAxisA = quatRotate(qA, -twistAxis);
[1963]	753	}
[2882]	754	else
	755	{
	756	m_twistAngle = btScalar(0.f);
	757	}
[1963]	758	}
[2882]	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)
[1963]	773	{
[2882]	774	vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
	775	vSwingAxis.normalize();
	776	if (fabs(vSwingAxis.x()) > SIMD_EPSILON)
[1963]	777	{
[2882]	778	// non-zero twist?! this should never happen.
	779	int wtf = 0; wtf = wtf;
	780	}
[1963]	781
[2882]	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.)
[1963]	785
[2882]	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();
[1963]	791
[2882]	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.
[1963]	798
[2882]	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);
[1963]	807	}
	808
[2882]	809	// test!
	810	/*swingLimit = m_swingSpan2;
	811	if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
[1963]	812	{
[2882]	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	}
[1963]	828
[2882]	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);
[1963]	834
[2882]	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.
[1963]	841
[2882]	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	}
[1963]	851
[2882]	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();
[1963]	873	}
[2882]	874	if (twistAngle < 0)
	875	{
	876	// this should never happen
	877	int wtf = 0; wtf = wtf;
	878	}
[1963]	879
[2882]	880	vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
	881	if (twistAngle > SIMD_EPSILON)
	882	vTwistAxis.normalize();
[1963]	883	}
	884
[2882]	885
	886	void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
[1963]	887	{
[2882]	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.
[1963]	892
[2882]	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	}
[1963]	916	}
[2882]	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: