Diff: joint.cpp - joint.cpp

	joint.cpp	joint.cpp

	skipping to change at line 36	skipping to change at line 36
	to the static environment (i.e. the absolute frame) is to check the second	to the static environment (i.e. the absolute frame) is to check the second
	body (joint->node[1].body), and if it is zero then behave as if its body	body (joint->node[1].body), and if it is zero then behave as if its body
	transform is the identity.	transform is the identity.

	*/	*/

	#include <ode/ode.h>	#include <ode/ode.h>
	#include <ode/odemath.h>	#include <ode/odemath.h>
	#include <ode/rotation.h>	#include <ode/rotation.h>
	#include <ode/matrix.h>	#include <ode/matrix.h>

		#include "config.h"
	#include "joint.h"	#include "joint.h"
	#include "joint_internal.h"	#include "joint_internal.h"

	extern void addObjectToList( dObject obj, dObject *first );	extern void addObjectToList( dObject obj, dObject *first );

	dxJoint::dxJoint( dxWorld *w ) :	dxJoint::dxJoint( dxWorld *w ) :
	dObject( w )	dObject( w )
	{	{
	//printf("constructing %p\n", this);	//printf("constructing %p\n", this);
	dIASSERT( w );	dIASSERT( w );

	skipping to change at line 95	skipping to change at line 96
	{	{
	// anchor points in global coordinates with respect to body PORs.	// anchor points in global coordinates with respect to body PORs.
	dVector3 a1, a2;	dVector3 a1, a2;

	int s = info->rowskip;	int s = info->rowskip;

	// set jacobian	// set jacobian
	info->J1l[0] = 1;	info->J1l[0] = 1;
	info->J1l[s+1] = 1;	info->J1l[s+1] = 1;
	info->J1l[2*s+2] = 1;	info->J1l[2*s+2] = 1;

	dMULTIPLY0_331( a1, joint->node[0].body->posr.R, anchor1 );	dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
	dCROSSMAT( info->J1a, a1, s, -, + );	dSetCrossMatrixMinus( info->J1a, a1, s );
	if ( joint->node[1].body )	if ( joint->node[1].body )
	{	{
	info->J2l[0] = -1;	info->J2l[0] = -1;
	info->J2l[s+1] = -1;	info->J2l[s+1] = -1;
	info->J2l[2*s+2] = -1;	info->J2l[2*s+2] = -1;

	dMULTIPLY0_331( a2, joint->node[1].body->posr.R, anchor2 );	dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 );
	dCROSSMAT( info->J2a, a2, s, + , - );	dSetCrossMatrixPlus( info->J2a, a2, s );
	}	}

	// set right hand side	// set right hand side
	dReal k = info->fps * info->erp;	dReal k = info->fps * info->erp;
	if ( joint->node[1].body )	if ( joint->node[1].body )
	{	{
	for ( int j = 0; j < 3; j++ )	for ( int j = 0; j < 3; j++ )
	{	{
	info->c[j] = k * ( a2[j] + joint->node[1].body->posr.pos[j] -	info->c[j] = k * ( a2[j] + joint->node[1].body->posr.pos[j] -
	a1[j] - joint->node[0].body->posr.pos[j] );	a1[j] - joint->node[0].body->posr.pos[j] );

	skipping to change at line 149	skipping to change at line 150

	// get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],	// get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],
	// [0 1 0] and [0 0 1], which makes everything much easier.	// [0 1 0] and [0 0 1], which makes everything much easier.
	dVector3 q1, q2;	dVector3 q1, q2;
	dPlaneSpace( axis, q1, q2 );	dPlaneSpace( axis, q1, q2 );

	// set jacobian	// set jacobian
	for ( i = 0; i < 3; i++ ) info->J1l[i] = axis[i];	for ( i = 0; i < 3; i++ ) info->J1l[i] = axis[i];
	for ( i = 0; i < 3; i++ ) info->J1l[s+i] = q1[i];	for ( i = 0; i < 3; i++ ) info->J1l[s+i] = q1[i];
	for ( i = 0; i < 3; i++ ) info->J1l[2*s+i] = q2[i];	for ( i = 0; i < 3; i++ ) info->J1l[2*s+i] = q2[i];

	dMULTIPLY0_331( a1, joint->node[0].body->posr.R, anchor1 );	dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
	dCROSS( info->J1a, = , a1, axis );	dCalcVectorCross3( info->J1a, a1, axis );
	dCROSS( info->J1a + s, = , a1, q1 );	dCalcVectorCross3( info->J1a + s, a1, q1 );
	dCROSS( info->J1a + 2*s, = , a1, q2 );	dCalcVectorCross3( info->J1a + 2*s, a1, q2 );
	if ( joint->node[1].body )	if ( joint->node[1].body )
	{	{
	for ( i = 0; i < 3; i++ ) info->J2l[i] = -axis[i];	for ( i = 0; i < 3; i++ ) info->J2l[i] = -axis[i];
	for ( i = 0; i < 3; i++ ) info->J2l[s+i] = -q1[i];	for ( i = 0; i < 3; i++ ) info->J2l[s+i] = -q1[i];
	for ( i = 0; i < 3; i++ ) info->J2l[2*s+i] = -q2[i];	for ( i = 0; i < 3; i++ ) info->J2l[2*s+i] = -q2[i];

	dMULTIPLY0_331( a2, joint->node[1].body->posr.R, anchor2 );	dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 );
	dCROSS( info->J2a, = -, a2, axis );	dReal *J2a = info->J2a;
	dCROSS( info->J2a + s, = -, a2, q1 );	dCalcVectorCross3( J2a, a2, axis );
	dCROSS( info->J2a + 2*s, = -, a2, q2 );	dNegateVector3( J2a );
		dReal *J2a_plus_s = J2a + s;
		dCalcVectorCross3( J2a_plus_s, a2, q1 );
		dNegateVector3( J2a_plus_s );
		dReal *J2a_plus_2s = J2a_plus_s + s;
		dCalcVectorCross3( J2a_plus_2s, a2, q2 );
		dNegateVector3( J2a_plus_2s );
	}	}

	// set right hand side - measure error along (axis,q1,q2)	// set right hand side - measure error along (axis,q1,q2)
	dReal k1 = info->fps * erp1;	dReal k1 = info->fps * erp1;
	dReal k = info->fps * info->erp;	dReal k = info->fps * info->erp;

	for ( i = 0; i < 3; i++ ) a1[i] += joint->node[0].body->posr.pos[i];	for ( i = 0; i < 3; i++ ) a1[i] += joint->node[0].body->posr.pos[i];
	if ( joint->node[1].body )	if ( joint->node[1].body )
	{	{
	for ( i = 0; i < 3; i++ ) a2[i] += joint->node[1].body->posr.pos[i] ;	for ( i = 0; i < 3; i++ ) a2[i] += joint->node[1].body->posr.pos[i] ;

	info->c[0] = k1 * ( dDOT( axis, a2 ) - dDOT( axis, a1 ) );
	info->c[1] = k * ( dDOT( q1, a2 ) - dDOT( q1, a1 ) );	dVector3 a2_minus_a1;
	info->c[2] = k * ( dDOT( q2, a2 ) - dDOT( q2, a1 ) );	dSubtractVectors3(a2_minus_a1, a2, a1);
		info->c[0] = k1 * dCalcVectorDot3( axis, a2_minus_a1 );
		info->c[1] = k * dCalcVectorDot3( q1, a2_minus_a1 );
		info->c[2] = k * dCalcVectorDot3( q2, a2_minus_a1 );
	}	}
	else	else
	{	{

	info->c[0] = k1 * ( dDOT( axis, anchor2 ) - dDOT( axis, a1 ) );	dVector3 anchor2_minus_a1;
	info->c[1] = k * ( dDOT( q1, anchor2 ) - dDOT( q1, a1 ) );	dSubtractVectors3(anchor2_minus_a1, anchor2, a1);
	info->c[2] = k * ( dDOT( q2, anchor2 ) - dDOT( q2, a1 ) );	info->c[0] = k1 * dCalcVectorDot3( axis, anchor2_minus_a1 );
		info->c[1] = k * dCalcVectorDot3( q1, anchor2_minus_a1 );
		info->c[2] = k * dCalcVectorDot3( q2, anchor2_minus_a1 );
	}	}
	}	}

	// set three orientation rows in the constraint equation, and the	// set three orientation rows in the constraint equation, and the
	// corresponding right hand side.	// corresponding right hand side.

	void setFixedOrientation( dxJoint joint, dxJoint::Info2 info, dQuaternion qrel, int start_row )	void setFixedOrientation( dxJoint joint, dxJoint::Info2 info, dQuaternion qrel, int start_row )
	{	{
	int s = info->rowskip;	int s = info->rowskip;
	int start_index = start_row * s;	int start_index = start_row * s;

	skipping to change at line 235	skipping to change at line 247
	else	else
	{	{
	dQMultiply3( qerr, joint->node[0].body->q, qrel );	dQMultiply3( qerr, joint->node[0].body->q, qrel );
	}	}
	if ( qerr[0] < 0 )	if ( qerr[0] < 0 )
	{	{
	qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small	qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small
	qerr[2] = -qerr[2];	qerr[2] = -qerr[2];
	qerr[3] = -qerr[3];	qerr[3] = -qerr[3];
	}	}

	dMULTIPLY0_331( e, joint->node[0].body->posr.R, qerr + 1 ); // @@@ bad SIMD padding!	dMultiply0_331( e, joint->node[0].body->posr.R, qerr + 1 ); // @@@ bad SIMD padding!
	dReal k = info->fps * info->erp;	dReal k = info->fps * info->erp;
	info->c[start_row] = 2 * k * e[0];	info->c[start_row] = 2 * k * e[0];
	info->c[start_row+1] = 2 * k * e[1];	info->c[start_row+1] = 2 * k * e[1];
	info->c[start_row+2] = 2 * k * e[2];	info->c[start_row+2] = 2 * k * e[2];
	}	}

	// compute anchor points relative to bodies	// compute anchor points relative to bodies

	void setAnchors( dxJoint *j, dReal x, dReal y, dReal z,	void setAnchors( dxJoint *j, dReal x, dReal y, dReal z,
	dVector3 anchor1, dVector3 anchor2 )	dVector3 anchor1, dVector3 anchor2 )
	{	{
	if ( j->node[0].body )	if ( j->node[0].body )
	{	{
	dReal q[4];	dReal q[4];
	q[0] = x - j->node[0].body->posr.pos[0];	q[0] = x - j->node[0].body->posr.pos[0];
	q[1] = y - j->node[0].body->posr.pos[1];	q[1] = y - j->node[0].body->posr.pos[1];
	q[2] = z - j->node[0].body->posr.pos[2];	q[2] = z - j->node[0].body->posr.pos[2];
	q[3] = 0;	q[3] = 0;

	dMULTIPLY1_331( anchor1, j->node[0].body->posr.R, q );	dMultiply1_331( anchor1, j->node[0].body->posr.R, q );
	if ( j->node[1].body )	if ( j->node[1].body )
	{	{
	q[0] = x - j->node[1].body->posr.pos[0];	q[0] = x - j->node[1].body->posr.pos[0];
	q[1] = y - j->node[1].body->posr.pos[1];	q[1] = y - j->node[1].body->posr.pos[1];
	q[2] = z - j->node[1].body->posr.pos[2];	q[2] = z - j->node[1].body->posr.pos[2];
	q[3] = 0;	q[3] = 0;

	dMULTIPLY1_331( anchor2, j->node[1].body->posr.R, q );	dMultiply1_331( anchor2, j->node[1].body->posr.R, q );
	}	}
	else	else
	{	{
	anchor2[0] = x;	anchor2[0] = x;
	anchor2[1] = y;	anchor2[1] = y;
	anchor2[2] = z;	anchor2[2] = z;
	}	}
	}	}
	anchor1[3] = 0;	anchor1[3] = 0;
	anchor2[3] = 0;	anchor2[3] = 0;

	skipping to change at line 289	skipping to change at line 301
	if ( j->node[0].body )	if ( j->node[0].body )
	{	{
	dReal q[4];	dReal q[4];
	q[0] = x;	q[0] = x;
	q[1] = y;	q[1] = y;
	q[2] = z;	q[2] = z;
	q[3] = 0;	q[3] = 0;
	dNormalize3( q );	dNormalize3( q );
	if ( axis1 )	if ( axis1 )
	{	{

	dMULTIPLY1_331( axis1, j->node[0].body->posr.R, q );	dMultiply1_331( axis1, j->node[0].body->posr.R, q );
	axis1[3] = 0;	axis1[3] = 0;
	}	}
	if ( axis2 )	if ( axis2 )
	{	{
	if ( j->node[1].body )	if ( j->node[1].body )
	{	{

	dMULTIPLY1_331( axis2, j->node[1].body->posr.R, q );	dMultiply1_331( axis2, j->node[1].body->posr.R, q );
	}	}
	else	else
	{	{
	axis2[0] = x;	axis2[0] = x;
	axis2[1] = y;	axis2[1] = y;
	axis2[2] = z;	axis2[2] = z;
	}	}
	axis2[3] = 0;	axis2[3] = 0;
	}	}
	}	}
	}	}

	void getAnchor( dxJoint *j, dVector3 result, dVector3 anchor1 )	void getAnchor( dxJoint *j, dVector3 result, dVector3 anchor1 )
	{	{
	if ( j->node[0].body )	if ( j->node[0].body )
	{	{

	dMULTIPLY0_331( result, j->node[0].body->posr.R, anchor1 );	dMultiply0_331( result, j->node[0].body->posr.R, anchor1 );
	result[0] += j->node[0].body->posr.pos[0];	result[0] += j->node[0].body->posr.pos[0];
	result[1] += j->node[0].body->posr.pos[1];	result[1] += j->node[0].body->posr.pos[1];
	result[2] += j->node[0].body->posr.pos[2];	result[2] += j->node[0].body->posr.pos[2];
	}	}
	}	}

	void getAnchor2( dxJoint *j, dVector3 result, dVector3 anchor2 )	void getAnchor2( dxJoint *j, dVector3 result, dVector3 anchor2 )
	{	{
	if ( j->node[1].body )	if ( j->node[1].body )
	{	{

	dMULTIPLY0_331( result, j->node[1].body->posr.R, anchor2 );	dMultiply0_331( result, j->node[1].body->posr.R, anchor2 );
	result[0] += j->node[1].body->posr.pos[0];	result[0] += j->node[1].body->posr.pos[0];
	result[1] += j->node[1].body->posr.pos[1];	result[1] += j->node[1].body->posr.pos[1];
	result[2] += j->node[1].body->posr.pos[2];	result[2] += j->node[1].body->posr.pos[2];
	}	}
	else	else
	{	{
	result[0] = anchor2[0];	result[0] = anchor2[0];
	result[1] = anchor2[1];	result[1] = anchor2[1];
	result[2] = anchor2[2];	result[2] = anchor2[2];
	}	}
	}	}

	void getAxis( dxJoint *j, dVector3 result, dVector3 axis1 )	void getAxis( dxJoint *j, dVector3 result, dVector3 axis1 )
	{	{
	if ( j->node[0].body )	if ( j->node[0].body )
	{	{

	dMULTIPLY0_331( result, j->node[0].body->posr.R, axis1 );	dMultiply0_331( result, j->node[0].body->posr.R, axis1 );
	}	}
	}	}

	void getAxis2( dxJoint *j, dVector3 result, dVector3 axis2 )	void getAxis2( dxJoint *j, dVector3 result, dVector3 axis2 )
	{	{
	if ( j->node[1].body )	if ( j->node[1].body )
	{	{

	dMULTIPLY0_331( result, j->node[1].body->posr.R, axis2 );	dMultiply0_331( result, j->node[1].body->posr.R, axis2 );
	}	}
	else	else
	{	{
	result[0] = axis2[0];	result[0] = axis2[0];
	result[1] = axis2[1];	result[1] = axis2[1];
	result[2] = axis2[2];	result[2] = axis2[2];
	}	}
	}	}

	dReal getHingeAngleFromRelativeQuat( dQuaternion qrel, dVector3 axis )	dReal getHingeAngleFromRelativeQuat( dQuaternion qrel, dVector3 axis )

	skipping to change at line 386	skipping to change at line 398
	// hinge axis and then in the opposite direction. the result is that th eta	// hinge axis and then in the opposite direction. the result is that th eta
	// will appear to go "backwards" every other cycle. here is a fix: if u	// will appear to go "backwards" every other cycle. here is a fix: if u
	// points "away" from the direction of the hinge (motor) axis (i.e. mor e	// points "away" from the direction of the hinge (motor) axis (i.e. mor e
	// than 90 degrees) then use -q instead of q. this represents the same	// than 90 degrees) then use -q instead of q. this represents the same
	// rotation, but results in the cos(theta/2) value being sign inverted.	// rotation, but results in the cos(theta/2) value being sign inverted.

	// extract the angle from the quaternion. cost2 = cos(theta/2),	// extract the angle from the quaternion. cost2 = cos(theta/2),
	// sint2 = \|sin(theta/2)\|	// sint2 = \|sin(theta/2)\|
	dReal cost2 = qrel[0];	dReal cost2 = qrel[0];
	dReal sint2 = dSqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] );	dReal sint2 = dSqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] );

	dReal theta = ( dDOT( qrel + 1, axis ) >= 0 ) ? // @@@ padding assumpti ons	dReal theta = ( dCalcVectorDot3( qrel + 1, axis ) >= 0 ) ? // @@@ paddi ng assumptions
	( 2 * dAtan2( sint2, cost2 ) ) : // if u points in direc tion of axis	( 2 * dAtan2( sint2, cost2 ) ) : // if u points in direc tion of axis
	( 2 * dAtan2( sint2, -cost2 ) ); // if u points in oppos ite direction	( 2 * dAtan2( sint2, -cost2 ) ); // if u points in oppos ite direction

	// the angle we get will be between 0..2*pi, but we want to return angl es	// the angle we get will be between 0..2*pi, but we want to return angl es
	// between -pi..pi	// between -pi..pi
	if ( theta > M_PI ) theta -= ( dReal )( 2 * M_PI );	if ( theta > M_PI ) theta -= ( dReal )( 2 * M_PI );

	// the angle we've just extracted has the wrong sign	// the angle we've just extracted has the wrong sign
	theta = -theta;	theta = -theta;


	skipping to change at line 572	skipping to change at line 584
	// extra tiny bit of computation) in doing this adjustment. note th at we	// extra tiny bit of computation) in doing this adjustment. note th at we
	// only need to do this if the constraint connects two bodies.	// only need to do this if the constraint connects two bodies.

	dVector3 ltd = {0,0,0}; // Linear Torque Decoupling vector (a torqu e)	dVector3 ltd = {0,0,0}; // Linear Torque Decoupling vector (a torqu e)
	if ( !rotational && joint->node[1].body )	if ( !rotational && joint->node[1].body )
	{	{
	dVector3 c;	dVector3 c;
	c[0] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[0] - joint ->node[0].body->posr.pos[0] );	c[0] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[0] - joint ->node[0].body->posr.pos[0] );
	c[1] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[1] - joint ->node[0].body->posr.pos[1] );	c[1] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[1] - joint ->node[0].body->posr.pos[1] );
	c[2] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[2] - joint ->node[0].body->posr.pos[2] );	c[2] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[2] - joint ->node[0].body->posr.pos[2] );

	dCROSS( ltd, = , c, ax1 );	dCalcVectorCross3( ltd, c, ax1 );
	info->J1a[srow+0] = ltd[0];	info->J1a[srow+0] = ltd[0];
	info->J1a[srow+1] = ltd[1];	info->J1a[srow+1] = ltd[1];
	info->J1a[srow+2] = ltd[2];	info->J1a[srow+2] = ltd[2];
	info->J2a[srow+0] = ltd[0];	info->J2a[srow+0] = ltd[0];
	info->J2a[srow+1] = ltd[1];	info->J2a[srow+1] = ltd[1];
	info->J2a[srow+2] = ltd[2];	info->J2a[srow+2] = ltd[2];
	}	}

	// if we're limited low and high simultaneously, the joint motor is	// if we're limited low and high simultaneously, the joint motor is
	// ineffective	// ineffective

	skipping to change at line 669	skipping to change at line 681
	info->hi[row] = 0;	info->hi[row] = 0;
	}	}

	// deal with bounce	// deal with bounce
	if ( bounce > 0 )	if ( bounce > 0 )
	{	{
	// calculate joint velocity	// calculate joint velocity
	dReal vel;	dReal vel;
	if ( rotational )	if ( rotational )
	{	{

	vel = dDOT( joint->node[0].body->avel, ax1 );	vel = dCalcVectorDot3( joint->node[0].body->avel, a x1 );
	if ( joint->node[1].body )	if ( joint->node[1].body )

	vel -= dDOT( joint->node[1].body->avel, ax1 );	vel -= dCalcVectorDot3( joint->node[1].body->av el, ax1 );
	}	}
	else	else
	{	{

	vel = dDOT( joint->node[0].body->lvel, ax1 );	vel = dCalcVectorDot3( joint->node[0].body->lvel, a x1 );
	if ( joint->node[1].body )	if ( joint->node[1].body )

	vel -= dDOT( joint->node[1].body->lvel, ax1 );	vel -= dCalcVectorDot3( joint->node[1].body->lv el, ax1 );
	}	}

	// only apply bounce if the velocity is incoming, and i f the	// only apply bounce if the velocity is incoming, and i f the
	// resulting c[] exceeds what we already have.	// resulting c[] exceeds what we already have.
	if ( limit == 1 )	if ( limit == 1 )
	{	{
	// low limit	// low limit
	if ( vel < 0 )	if ( vel < 0 )
	{	{
	dReal newc = -bounce * vel;	dReal newc = -bounce * vel;

End of changes. 22 change blocks.
	33 lines changed or deleted	45 lines changed or added
This html diff was produced by rfcdiff 1.41. The latest version is available from http://tools.ietf.org/tools/rfcdiff/