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