Planet

navi

home

PPS

about

screenshots

download

development

forum

Context Navigation

source: code/branches/kicklib/src/external/bullet/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @ 8012

Last change on this file since 8012 was 7983, checked in by rgrieder, 14 years ago
Updated Bullet Physics Engine from v2.74 to v2.77
Property svn:eol-style set to `native`
File size: 30.7 KB

Rev	Line
[1963]	1	/*
	2	Bullet Continuous Collision Detection and Physics Library
	3	Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
	4
	5	This software is provided 'as-is', without any express or implied warranty.
	6	In no event will the authors be held liable for any damages arising from the use of this software.
	7	Permission is granted to anyone to use this software for any purpose,
	8	including commercial applications, and to alter it and redistribute it freely,
	9	subject to the following restrictions:
	10
	11	1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
	12	2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
	13	3. This notice may not be removed or altered from any source distribution.
	14	*/
	15	/*
	16	2007-09-09
	17	Refactored by Francisco Le?n
	18	email: projectileman@yahoo.com
	19	http://gimpact.sf.net
	20	*/
	21
	22	#include "btGeneric6DofConstraint.h"
	23	#include "BulletDynamics/Dynamics/btRigidBody.h"
	24	#include "LinearMath/btTransformUtil.h"
[7983]	25	#include "LinearMath/btTransformUtil.h"
[1963]	26	#include <new>
	27
	28
[7983]	29
[2882]	30	#define D6_USE_OBSOLETE_METHOD false
[7983]	31	#define D6_USE_FRAME_OFFSET true
[2882]	32
	33
	34
[7983]	35
	36
	37
[2882]	38	btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
	39	: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)
	40	, m_frameInA(frameInA)
	41	, m_frameInB(frameInB),
	42	m_useLinearReferenceFrameA(useLinearReferenceFrameA),
[7983]	43	m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
	44	m_flags(0),
[2882]	45	m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
	46	{
[7983]	47	calculateTransforms();
	48	}
[2882]	49
[7983]	50
	51
	52	btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
	53	: btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
	54	m_frameInB(frameInB),
	55	m_useLinearReferenceFrameA(useLinearReferenceFrameB),
	56	m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
	57	m_flags(0),
	58	m_useSolveConstraintObsolete(false)
	59	{
	60	///not providing rigidbody A means implicitly using worldspace for body A
	61	m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
	62	calculateTransforms();
[2882]	63	}
	64
	65
[7983]	66
	67
[1963]	68	#define GENERIC_D6_DISABLE_WARMSTARTING 1
	69
[2882]	70
[7983]	71
[1963]	72	btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
	73	btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
	74	{
	75	int i = index%3;
	76	int j = index/3;
	77	return mat[i][j];
	78	}
	79
[2882]	80
[7983]	81
[1963]	82	///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
	83	bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);
	84	bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
	85	{
[2882]	86	// // rot = cycz -cysz sy
	87	// // czsxsy+cxsz cxcz-sxsysz -cy*sx
	88	// // -cxczsy+sxsz czsx+cxsysz cx*cy
	89	//
[1963]	90
[2882]	91	btScalar fi = btGetMatrixElem(mat,2);
	92	if (fi < btScalar(1.0f))
	93	{
	94	if (fi > btScalar(-1.0f))
[1963]	95	{
[2882]	96	xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
	97	xyz[1] = btAsin(btGetMatrixElem(mat,2));
	98	xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
	99	return true;
[1963]	100	}
	101	else
	102	{
[2882]	103	// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
	104	xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
	105	xyz[1] = -SIMD_HALF_PI;
	106	xyz[2] = btScalar(0.0);
	107	return false;
[1963]	108	}
[2882]	109	}
	110	else
	111	{
	112	// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
	113	xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
	114	xyz[1] = SIMD_HALF_PI;
	115	xyz[2] = 0.0;
	116	}
[1963]	117	return false;
	118	}
	119
	120	//////////////////////////// btRotationalLimitMotor ////////////////////////////////////
	121
	122	int btRotationalLimitMotor::testLimitValue(btScalar test_value)
	123	{
	124	if(m_loLimit>m_hiLimit)
	125	{
	126	m_currentLimit = 0;//Free from violation
	127	return 0;
	128	}
	129	if (test_value < m_loLimit)
	130	{
	131	m_currentLimit = 1;//low limit violation
	132	m_currentLimitError = test_value - m_loLimit;
	133	return 1;
	134	}
	135	else if (test_value> m_hiLimit)
	136	{
	137	m_currentLimit = 2;//High limit violation
	138	m_currentLimitError = test_value - m_hiLimit;
	139	return 2;
	140	};
	141
	142	m_currentLimit = 0;//Free from violation
	143	return 0;
[2882]	144
[1963]	145	}
	146
	147
[7983]	148
[1963]	149	btScalar btRotationalLimitMotor::solveAngularLimits(
[2882]	150	btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
[7983]	151	btRigidBody * body0, btRigidBody * body1 )
[1963]	152	{
[2882]	153	if (needApplyTorques()==false) return 0.0f;
[1963]	154
[2882]	155	btScalar target_velocity = m_targetVelocity;
	156	btScalar maxMotorForce = m_maxMotorForce;
[1963]	157
	158	//current error correction
[2882]	159	if (m_currentLimit!=0)
	160	{
[7983]	161	target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
[2882]	162	maxMotorForce = m_maxLimitForce;
	163	}
[1963]	164
[2882]	165	maxMotorForce *= timeStep;
[1963]	166
[2882]	167	// current velocity difference
[1963]	168
[2882]	169	btVector3 angVelA;
[7983]	170	body0->internalGetAngularVelocity(angVelA);
[2882]	171	btVector3 angVelB;
[7983]	172	body1->internalGetAngularVelocity(angVelB);
[1963]	173
[2882]	174	btVector3 vel_diff;
	175	vel_diff = angVelA-angVelB;
[1963]	176
	177
[2882]	178
	179	btScalar rel_vel = axis.dot(vel_diff);
	180
[1963]	181	// correction velocity
[2882]	182	btScalar motor_relvel = m_limitSoftness(target_velocity - m_dampingrel_vel);
[1963]	183
	184
[2882]	185	if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON )
	186	{
	187	return 0.0f;//no need for applying force
	188	}
[1963]	189
	190
	191	// correction impulse
[2882]	192	btScalar unclippedMotorImpulse = (1+m_bounce)motor_relveljacDiagABInv;
[1963]	193
	194	// clip correction impulse
[2882]	195	btScalar clippedMotorImpulse;
[1963]	196
[2882]	197	///@todo: should clip against accumulated impulse
	198	if (unclippedMotorImpulse>0.0f)
	199	{
	200	clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
	201	}
	202	else
	203	{
	204	clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
	205	}
[1963]	206
	207
	208	// sort with accumulated impulses
[7983]	209	btScalar lo = btScalar(-BT_LARGE_FLOAT);
	210	btScalar hi = btScalar(BT_LARGE_FLOAT);
[1963]	211
[2882]	212	btScalar oldaccumImpulse = m_accumulatedImpulse;
	213	btScalar sum = oldaccumImpulse + clippedMotorImpulse;
	214	m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
[1963]	215
[2882]	216	clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
[1963]	217
[2882]	218	btVector3 motorImp = clippedMotorImpulse * axis;
[1963]	219
[2882]	220	//body0->applyTorqueImpulse(motorImp);
	221	//body1->applyTorqueImpulse(-motorImp);
[1963]	222
[7983]	223	body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
	224	body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);
[1963]	225
	226
[2882]	227	return clippedMotorImpulse;
[1963]	228
	229
	230	}
	231
	232	//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
	233
[2882]	234
	235
	236
[1963]	237	//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
[2882]	238
	239
	240	int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
[1963]	241	{
[2882]	242	btScalar loLimit = m_lowerLimit[limitIndex];
	243	btScalar hiLimit = m_upperLimit[limitIndex];
	244	if(loLimit > hiLimit)
	245	{
	246	m_currentLimit[limitIndex] = 0;//Free from violation
	247	m_currentLimitError[limitIndex] = btScalar(0.f);
	248	return 0;
	249	}
[1963]	250
[2882]	251	if (test_value < loLimit)
	252	{
	253	m_currentLimit[limitIndex] = 2;//low limit violation
	254	m_currentLimitError[limitIndex] = test_value - loLimit;
	255	return 2;
	256	}
	257	else if (test_value> hiLimit)
	258	{
	259	m_currentLimit[limitIndex] = 1;//High limit violation
	260	m_currentLimitError[limitIndex] = test_value - hiLimit;
	261	return 1;
	262	};
[1963]	263
[2882]	264	m_currentLimit[limitIndex] = 0;//Free from violation
	265	m_currentLimitError[limitIndex] = btScalar(0.f);
	266	return 0;
[7983]	267	}
[1963]	268
	269
[7983]	270
[2882]	271	btScalar btTranslationalLimitMotor::solveLinearAxis(
	272	btScalar timeStep,
	273	btScalar jacDiagABInv,
[7983]	274	btRigidBody& body1,const btVector3 &pointInA,
	275	btRigidBody& body2,const btVector3 &pointInB,
[2882]	276	int limit_index,
	277	const btVector3 & axis_normal_on_a,
	278	const btVector3 & anchorPos)
	279	{
[1963]	280
[2882]	281	///find relative velocity
	282	// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
	283	// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
	284	btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
	285	btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
[1963]	286
[2882]	287	btVector3 vel1;
[7983]	288	body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
[2882]	289	btVector3 vel2;
[7983]	290	body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
[2882]	291	btVector3 vel = vel1 - vel2;
[1963]	292
[2882]	293	btScalar rel_vel = axis_normal_on_a.dot(vel);
[1963]	294
	295
	296
[2882]	297	/// apply displacement correction
[1963]	298
[2882]	299	//positional error (zeroth order error)
	300	btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
[7983]	301	btScalar lo = btScalar(-BT_LARGE_FLOAT);
	302	btScalar hi = btScalar(BT_LARGE_FLOAT);
[1963]	303
[2882]	304	btScalar minLimit = m_lowerLimit[limit_index];
	305	btScalar maxLimit = m_upperLimit[limit_index];
[1963]	306
[2882]	307	//handle the limits
	308	if (minLimit < maxLimit)
	309	{
	310	{
	311	if (depth > maxLimit)
	312	{
	313	depth -= maxLimit;
	314	lo = btScalar(0.);
[1963]	315
[2882]	316	}
	317	else
	318	{
	319	if (depth < minLimit)
	320	{
	321	depth -= minLimit;
	322	hi = btScalar(0.);
	323	}
	324	else
	325	{
	326	return 0.0f;
	327	}
	328	}
	329	}
	330	}
[1963]	331
[2882]	332	btScalar normalImpulse= m_limitSoftness(m_restitutiondepth/timeStep - m_dampingrel_vel) jacDiagABInv;
[1963]	333
	334
	335
	336
[2882]	337	btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
	338	btScalar sum = oldNormalImpulse + normalImpulse;
	339	m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
	340	normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
[1963]	341
[2882]	342	btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
	343	//body1.applyImpulse( impulse_vector, rel_pos1);
	344	//body2.applyImpulse(-impulse_vector, rel_pos2);
[1963]	345
[2882]	346	btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
	347	btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
[7983]	348	body1.internalApplyImpulse(axis_normal_on_abody1.getInvMass(), body1.getInvInertiaTensorWorld()ftorqueAxis1,normalImpulse);
	349	body2.internalApplyImpulse(axis_normal_on_abody2.getInvMass(), body2.getInvInertiaTensorWorld()ftorqueAxis2,-normalImpulse);
[1963]	350
	351
	352
	353
[2882]	354	return normalImpulse;
	355	}
[1963]	356
[2882]	357	//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
	358
[1963]	359	void btGeneric6DofConstraint::calculateAngleInfo()
	360	{
	361	btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();
	362	matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
	363	// in euler angle mode we do not actually constrain the angular velocity
[2882]	364	// along the axes axis[0] and axis[2] (although we do use axis[1]) :
	365	//
	366	// to get constrain w2-w1 along ...not
	367	// ------ --------------------- ------
	368	// d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
	369	// d(angle[1])/dt = 0 ax[1]
	370	// d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
	371	//
	372	// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
	373	// to prove the result for angle[0], write the expression for angle[0] from
	374	// GetInfo1 then take the derivative. to prove this for angle[2] it is
	375	// easier to take the euler rate expression for d(angle[2])/dt with respect
	376	// to the components of w and set that to 0.
[1963]	377	btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
	378	btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
	379
	380	m_calculatedAxis[1] = axis2.cross(axis0);
	381	m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
	382	m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
	383
[2882]	384	m_calculatedAxis[0].normalize();
	385	m_calculatedAxis[1].normalize();
	386	m_calculatedAxis[2].normalize();
[1963]	387
	388	}
	389
	390	void btGeneric6DofConstraint::calculateTransforms()
	391	{
[7983]	392	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
	393	}
	394
	395	void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
	396	{
	397	m_calculatedTransformA = transA * m_frameInA;
	398	m_calculatedTransformB = transB * m_frameInB;
[2882]	399	calculateLinearInfo();
	400	calculateAngleInfo();
[7983]	401	if(m_useOffsetForConstraintFrame)
	402	{ // get weight factors depending on masses
	403	btScalar miA = getRigidBodyA().getInvMass();
	404	btScalar miB = getRigidBodyB().getInvMass();
	405	m_hasStaticBody = (miA < SIMD_EPSILON) \|\| (miB < SIMD_EPSILON);
	406	btScalar miS = miA + miB;
	407	if(miS > btScalar(0.f))
	408	{
	409	m_factA = miB / miS;
	410	}
	411	else
	412	{
	413	m_factA = btScalar(0.5f);
	414	}
	415	m_factB = btScalar(1.0f) - m_factA;
	416	}
[1963]	417	}
	418
	419
[7983]	420
[1963]	421	void btGeneric6DofConstraint::buildLinearJacobian(
[2882]	422	btJacobianEntry & jacLinear,const btVector3 & normalWorld,
	423	const btVector3 & pivotAInW,const btVector3 & pivotBInW)
[1963]	424	{
[2882]	425	new (&jacLinear) btJacobianEntry(
[1963]	426	m_rbA.getCenterOfMassTransform().getBasis().transpose(),
	427	m_rbB.getCenterOfMassTransform().getBasis().transpose(),
	428	pivotAInW - m_rbA.getCenterOfMassPosition(),
	429	pivotBInW - m_rbB.getCenterOfMassPosition(),
	430	normalWorld,
	431	m_rbA.getInvInertiaDiagLocal(),
	432	m_rbA.getInvMass(),
	433	m_rbB.getInvInertiaDiagLocal(),
	434	m_rbB.getInvMass());
	435	}
	436
[2882]	437
[7983]	438
[1963]	439	void btGeneric6DofConstraint::buildAngularJacobian(
[2882]	440	btJacobianEntry & jacAngular,const btVector3 & jointAxisW)
[1963]	441	{
[2882]	442	new (&jacAngular) btJacobianEntry(jointAxisW,
[1963]	443	m_rbA.getCenterOfMassTransform().getBasis().transpose(),
	444	m_rbB.getCenterOfMassTransform().getBasis().transpose(),
	445	m_rbA.getInvInertiaDiagLocal(),
	446	m_rbB.getInvInertiaDiagLocal());
	447
	448	}
	449
[2882]	450
[7983]	451
[1963]	452	bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
	453	{
[2882]	454	btScalar angle = m_calculatedAxisAngleDiff[axis_index];
[7983]	455	angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
	456	m_angularLimits[axis_index].m_currentPosition = angle;
[2882]	457	//test limits
	458	m_angularLimits[axis_index].testLimitValue(angle);
	459	return m_angularLimits[axis_index].needApplyTorques();
[1963]	460	}
	461
[2882]	462
[7983]	463
[1963]	464	void btGeneric6DofConstraint::buildJacobian()
	465	{
[7983]	466	#ifndef __SPU__
[2882]	467	if (m_useSolveConstraintObsolete)
	468	{
[1963]	469
[2882]	470	// Clear accumulated impulses for the next simulation step
	471	m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
	472	int i;
	473	for(i = 0; i < 3; i++)
	474	{
	475	m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
	476	}
	477	//calculates transform
[7983]	478	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
[1963]	479
[2882]	480	// const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
	481	// const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
	482	calcAnchorPos();
	483	btVector3 pivotAInW = m_AnchorPos;
	484	btVector3 pivotBInW = m_AnchorPos;
[1963]	485
[2882]	486	// not used here
	487	// btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
	488	// btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
[1963]	489
[2882]	490	btVector3 normalWorld;
	491	//linear part
	492	for (i=0;i<3;i++)
	493	{
	494	if (m_linearLimits.isLimited(i))
	495	{
	496	if (m_useLinearReferenceFrameA)
	497	normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
	498	else
	499	normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
[1963]	500
[2882]	501	buildLinearJacobian(
	502	m_jacLinear[i],normalWorld ,
	503	pivotAInW,pivotBInW);
[1963]	504
[2882]	505	}
	506	}
[1963]	507
[2882]	508	// angular part
	509	for (i=0;i<3;i++)
	510	{
	511	//calculates error angle
	512	if (testAngularLimitMotor(i))
	513	{
	514	normalWorld = this->getAxis(i);
	515	// Create angular atom
	516	buildAngularJacobian(m_jacAng[i],normalWorld);
	517	}
	518	}
[1963]	519
[2882]	520	}
[7983]	521	#endif //__SPU__
	522
[2882]	523	}
[1963]	524
[2882]	525
	526	void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)
	527	{
	528	if (m_useSolveConstraintObsolete)
	529	{
	530	info->m_numConstraintRows = 0;
	531	info->nub = 0;
	532	} else
	533	{
	534	//prepare constraint
[7983]	535	calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
[2882]	536	info->m_numConstraintRows = 0;
	537	info->nub = 6;
	538	int i;
	539	//test linear limits
	540	for(i = 0; i < 3; i++)
	541	{
	542	if(m_linearLimits.needApplyForce(i))
	543	{
	544	info->m_numConstraintRows++;
	545	info->nub--;
	546	}
	547	}
	548	//test angular limits
	549	for (i=0;i<3 ;i++ )
	550	{
	551	if(testAngularLimitMotor(i))
	552	{
	553	info->m_numConstraintRows++;
	554	info->nub--;
	555	}
	556	}
	557	}
[1963]	558	}
	559
[7983]	560	void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
	561	{
	562	if (m_useSolveConstraintObsolete)
	563	{
	564	info->m_numConstraintRows = 0;
	565	info->nub = 0;
	566	} else
	567	{
	568	//pre-allocate all 6
	569	info->m_numConstraintRows = 6;
	570	info->nub = 0;
	571	}
	572	}
[1963]	573
[7983]	574
[2882]	575	void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)
[1963]	576	{
[2882]	577	btAssert(!m_useSolveConstraintObsolete);
[7983]	578
	579	const btTransform& transA = m_rbA.getCenterOfMassTransform();
	580	const btTransform& transB = m_rbB.getCenterOfMassTransform();
	581	const btVector3& linVelA = m_rbA.getLinearVelocity();
	582	const btVector3& linVelB = m_rbB.getLinearVelocity();
	583	const btVector3& angVelA = m_rbA.getAngularVelocity();
	584	const btVector3& angVelB = m_rbB.getAngularVelocity();
	585
	586	if(m_useOffsetForConstraintFrame)
	587	{ // for stability better to solve angular limits first
	588	int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
	589	setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
	590	}
	591	else
	592	{ // leave old version for compatibility
	593	int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
	594	setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
	595	}
	596
[2882]	597	}
[1963]	598
	599
[7983]	600	void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
[2882]	601	{
[7983]	602
	603	btAssert(!m_useSolveConstraintObsolete);
	604	//prepare constraint
	605	calculateTransforms(transA,transB);
	606
	607	int i;
	608	for (i=0;i<3 ;i++ )
	609	{
	610	testAngularLimitMotor(i);
	611	}
	612
	613	if(m_useOffsetForConstraintFrame)
	614	{ // for stability better to solve angular limits first
	615	int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
	616	setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
	617	}
	618	else
	619	{ // leave old version for compatibility
	620	int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);
	621	setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);
	622	}
	623	}
	624
	625
	626
	627	int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
	628	{
	629	// int row = 0;
[2882]	630	//solve linear limits
	631	btRotationalLimitMotor limot;
	632	for (int i=0;i<3 ;i++ )
	633	{
	634	if(m_linearLimits.needApplyForce(i))
	635	{ // re-use rotational motor code
	636	limot.m_bounce = btScalar(0.f);
	637	limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
[7983]	638	limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
[2882]	639	limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
	640	limot.m_damping = m_linearLimits.m_damping;
	641	limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
	642	limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
	643	limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
	644	limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
	645	limot.m_maxLimitForce = btScalar(0.f);
	646	limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
	647	limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
	648	btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
[7983]	649	int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
	650	limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
	651	limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
	652	limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
	653	if(m_useOffsetForConstraintFrame)
	654	{
	655	int indx1 = (i + 1) % 3;
	656	int indx2 = (i + 2) % 3;
	657	int rotAllowed = 1; // rotations around orthos to current axis
	658	if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
	659	{
	660	rotAllowed = 0;
	661	}
	662	row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
	663	}
	664	else
	665	{
	666	row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);
	667	}
[2882]	668	}
	669	}
	670	return row;
	671	}
[1963]	672
	673
[7983]	674
	675	int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
[2882]	676	{
	677	btGeneric6DofConstraint * d6constraint = this;
	678	int row = row_offset;
	679	//solve angular limits
	680	for (int i=0;i<3 ;i++ )
	681	{
	682	if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
	683	{
	684	btVector3 axis = d6constraint->getAxis(i);
[7983]	685	int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
	686	if(!(flags & BT_6DOF_FLAGS_CFM_NORM))
	687	{
	688	m_angularLimits[i].m_normalCFM = info->cfm[0];
	689	}
	690	if(!(flags & BT_6DOF_FLAGS_CFM_STOP))
	691	{
	692	m_angularLimits[i].m_stopCFM = info->cfm[0];
	693	}
	694	if(!(flags & BT_6DOF_FLAGS_ERP_STOP))
	695	{
	696	m_angularLimits[i].m_stopERP = info->erp;
	697	}
	698	row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
	699	transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
[2882]	700	}
	701	}
[1963]	702
[2882]	703	return row;
	704	}
[1963]	705
	706
	707
	708
	709	void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
	710	{
[2882]	711	(void)timeStep;
[1963]	712
	713	}
	714
[2882]	715
[7983]	716
[1963]	717	btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
	718	{
[2882]	719	return m_calculatedAxis[axis_index];
[1963]	720	}
	721
[2882]	722
[7983]	723	btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
[1963]	724	{
[7983]	725	return m_calculatedLinearDiff[axisIndex];
[1963]	726	}
	727
[2882]	728
[7983]	729	btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
	730	{
	731	return m_calculatedAxisAngleDiff[axisIndex];
	732	}
	733
	734
	735
[1963]	736	void btGeneric6DofConstraint::calcAnchorPos(void)
	737	{
	738	btScalar imA = m_rbA.getInvMass();
	739	btScalar imB = m_rbB.getInvMass();
	740	btScalar weight;
	741	if(imB == btScalar(0.0))
	742	{
	743	weight = btScalar(1.0);
	744	}
	745	else
	746	{
	747	weight = imA / (imA + imB);
	748	}
	749	const btVector3& pA = m_calculatedTransformA.getOrigin();
	750	const btVector3& pB = m_calculatedTransformB.getOrigin();
	751	m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
	752	return;
[7983]	753	}
[1963]	754
[2882]	755
[7983]	756
[2882]	757	void btGeneric6DofConstraint::calculateLinearInfo()
	758	{
	759	m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
	760	m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
	761	for(int i = 0; i < 3; i++)
	762	{
[7983]	763	m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
[2882]	764	m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
	765	}
[7983]	766	}
[2882]	767
	768
[7983]	769
[2882]	770	int btGeneric6DofConstraint::get_limit_motor_info2(
	771	btRotationalLimitMotor * limot,
[7983]	772	const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
	773	btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
[2882]	774	{
	775	int srow = row * info->rowskip;
	776	int powered = limot->m_enableMotor;
	777	int limit = limot->m_currentLimit;
	778	if (powered \|\| limit)
	779	{ // if the joint is powered, or has joint limits, add in the extra row
	780	btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
	781	btScalar *J2 = rotational ? info->m_J2angularAxis : 0;
	782	J1[srow+0] = ax1[0];
	783	J1[srow+1] = ax1[1];
	784	J1[srow+2] = ax1[2];
	785	if(rotational)
	786	{
	787	J2[srow+0] = -ax1[0];
	788	J2[srow+1] = -ax1[1];
	789	J2[srow+2] = -ax1[2];
	790	}
[7983]	791	if((!rotational))
[2882]	792	{
[7983]	793	if (m_useOffsetForConstraintFrame)
	794	{
	795	btVector3 tmpA, tmpB, relA, relB;
	796	// get vector from bodyB to frameB in WCS
	797	relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
	798	// get its projection to constraint axis
	799	btVector3 projB = ax1 * relB.dot(ax1);
	800	// get vector directed from bodyB to constraint axis (and orthogonal to it)
	801	btVector3 orthoB = relB - projB;
	802	// same for bodyA
	803	relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
	804	btVector3 projA = ax1 * relA.dot(ax1);
	805	btVector3 orthoA = relA - projA;
	806	// get desired offset between frames A and B along constraint axis
	807	btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
	808	// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
	809	btVector3 totalDist = projA + ax1 * desiredOffs - projB;
	810	// get offset vectors relA and relB
	811	relA = orthoA + totalDist * m_factA;
	812	relB = orthoB - totalDist * m_factB;
	813	tmpA = relA.cross(ax1);
	814	tmpB = relB.cross(ax1);
	815	if(m_hasStaticBody && (!rotAllowed))
	816	{
	817	tmpA *= m_factA;
	818	tmpB *= m_factB;
	819	}
	820	int i;
	821	for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
	822	for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
	823	} else
	824	{
	825	btVector3 ltd; // Linear Torque Decoupling vector
	826	btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
	827	ltd = c.cross(ax1);
	828	info->m_J1angularAxis[srow+0] = ltd[0];
	829	info->m_J1angularAxis[srow+1] = ltd[1];
	830	info->m_J1angularAxis[srow+2] = ltd[2];
[2882]	831
[7983]	832	c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
	833	ltd = -c.cross(ax1);
	834	info->m_J2angularAxis[srow+0] = ltd[0];
	835	info->m_J2angularAxis[srow+1] = ltd[1];
	836	info->m_J2angularAxis[srow+2] = ltd[2];
	837	}
[2882]	838	}
	839	// if we're limited low and high simultaneously, the joint motor is
	840	// ineffective
	841	if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
	842	info->m_constraintError[srow] = btScalar(0.f);
	843	if (powered)
	844	{
[7983]	845	info->cfm[srow] = limot->m_normalCFM;
[2882]	846	if(!limit)
	847	{
[7983]	848	btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
	849
	850	btScalar mot_fact = getMotorFactor( limot->m_currentPosition,
	851	limot->m_loLimit,
	852	limot->m_hiLimit,
	853	tag_vel,
	854	info->fps * limot->m_stopERP);
	855	info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
[2882]	856	info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
	857	info->m_upperLimit[srow] = limot->m_maxMotorForce;
	858	}
	859	}
	860	if(limit)
	861	{
[7983]	862	btScalar k = info->fps * limot->m_stopERP;
[2882]	863	if(!rotational)
	864	{
	865	info->m_constraintError[srow] += k * limot->m_currentLimitError;
	866	}
	867	else
	868	{
	869	info->m_constraintError[srow] += -k * limot->m_currentLimitError;
	870	}
[7983]	871	info->cfm[srow] = limot->m_stopCFM;
[2882]	872	if (limot->m_loLimit == limot->m_hiLimit)
	873	{ // limited low and high simultaneously
	874	info->m_lowerLimit[srow] = -SIMD_INFINITY;
	875	info->m_upperLimit[srow] = SIMD_INFINITY;
	876	}
	877	else
	878	{
	879	if (limit == 1)
	880	{
	881	info->m_lowerLimit[srow] = 0;
	882	info->m_upperLimit[srow] = SIMD_INFINITY;
	883	}
	884	else
	885	{
	886	info->m_lowerLimit[srow] = -SIMD_INFINITY;
	887	info->m_upperLimit[srow] = 0;
	888	}
	889	// deal with bounce
	890	if (limot->m_bounce > 0)
	891	{
	892	// calculate joint velocity
	893	btScalar vel;
	894	if (rotational)
	895	{
[7983]	896	vel = angVelA.dot(ax1);
	897	//make sure that if no body -> angVelB == zero vec
	898	// if (body1)
	899	vel -= angVelB.dot(ax1);
[2882]	900	}
	901	else
	902	{
[7983]	903	vel = linVelA.dot(ax1);
	904	//make sure that if no body -> angVelB == zero vec
	905	// if (body1)
	906	vel -= linVelB.dot(ax1);
[2882]	907	}
	908	// only apply bounce if the velocity is incoming, and if the
	909	// resulting c[] exceeds what we already have.
	910	if (limit == 1)
	911	{
	912	if (vel < 0)
	913	{
	914	btScalar newc = -limot->m_bounce* vel;
	915	if (newc > info->m_constraintError[srow])
	916	info->m_constraintError[srow] = newc;
	917	}
	918	}
	919	else
	920	{
	921	if (vel > 0)
	922	{
	923	btScalar newc = -limot->m_bounce * vel;
	924	if (newc < info->m_constraintError[srow])
	925	info->m_constraintError[srow] = newc;
	926	}
	927	}
	928	}
	929	}
	930	}
	931	return 1;
	932	}
	933	else return 0;
	934	}
	935
[7983]	936
	937
	938
	939
	940
	941	///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
	942	///If no axis is provided, it uses the default axis for this constraint.
	943	void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
	944	{
	945	if((axis >= 0) && (axis < 3))
	946	{
	947	switch(num)
	948	{
	949	case BT_CONSTRAINT_STOP_ERP :
	950	m_linearLimits.m_stopERP[axis] = value;
	951	m_flags \|= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
	952	break;
	953	case BT_CONSTRAINT_STOP_CFM :
	954	m_linearLimits.m_stopCFM[axis] = value;
	955	m_flags \|= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
	956	break;
	957	case BT_CONSTRAINT_CFM :
	958	m_linearLimits.m_normalCFM[axis] = value;
	959	m_flags \|= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
	960	break;
	961	default :
	962	btAssertConstrParams(0);
	963	}
	964	}
	965	else if((axis >=3) && (axis < 6))
	966	{
	967	switch(num)
	968	{
	969	case BT_CONSTRAINT_STOP_ERP :
	970	m_angularLimits[axis - 3].m_stopERP = value;
	971	m_flags \|= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
	972	break;
	973	case BT_CONSTRAINT_STOP_CFM :
	974	m_angularLimits[axis - 3].m_stopCFM = value;
	975	m_flags \|= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
	976	break;
	977	case BT_CONSTRAINT_CFM :
	978	m_angularLimits[axis - 3].m_normalCFM = value;
	979	m_flags \|= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
	980	break;
	981	default :
	982	btAssertConstrParams(0);
	983	}
	984	}
	985	else
	986	{
	987	btAssertConstrParams(0);
	988	}
	989	}
	990
	991	///return the local value of parameter
	992	btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
	993	{
	994	btScalar retVal = 0;
	995	if((axis >= 0) && (axis < 3))
	996	{
	997	switch(num)
	998	{
	999	case BT_CONSTRAINT_STOP_ERP :
	1000	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
	1001	retVal = m_linearLimits.m_stopERP[axis];
	1002	break;
	1003	case BT_CONSTRAINT_STOP_CFM :
	1004	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
	1005	retVal = m_linearLimits.m_stopCFM[axis];
	1006	break;
	1007	case BT_CONSTRAINT_CFM :
	1008	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
	1009	retVal = m_linearLimits.m_normalCFM[axis];
	1010	break;
	1011	default :
	1012	btAssertConstrParams(0);
	1013	}
	1014	}
	1015	else if((axis >=3) && (axis < 6))
	1016	{
	1017	switch(num)
	1018	{
	1019	case BT_CONSTRAINT_STOP_ERP :
	1020	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
	1021	retVal = m_angularLimits[axis - 3].m_stopERP;
	1022	break;
	1023	case BT_CONSTRAINT_STOP_CFM :
	1024	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
	1025	retVal = m_angularLimits[axis - 3].m_stopCFM;
	1026	break;
	1027	case BT_CONSTRAINT_CFM :
	1028	btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
	1029	retVal = m_angularLimits[axis - 3].m_normalCFM;
	1030	break;
	1031	default :
	1032	btAssertConstrParams(0);
	1033	}
	1034	}
	1035	else
	1036	{
	1037	btAssertConstrParams(0);
	1038	}
	1039	return retVal;
	1040	}

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

Download in other formats: