Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

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

Last change on this file since 10283 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

Property svn:eol-style set to native

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

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

Download in other formats: