void
dxJointBall::getInfo2( dReal worldFPS, dReal /*worldERP*/, const Info2Descr* info )
{
info->cfm[0] = cfm;
info->cfm[1] = cfm;
info->cfm[2] = cfm;
setBall( this, worldFPS, this->erp, info, anchor1, anchor2 );
}
void
dxJointBall::getInfo2( dxJoint::Info2 *info )
{
info->erp = erp;
info->cfm[0] = cfm;
info->cfm[1] = cfm;
info->cfm[2] = cfm;
setBall( this, info, anchor1, anchor2 );
}
void GameLayer::onTouchEnded(Touch *touch, Event *unused_event){
if(getGameStatus()==GAME_STATUS_READY){
setGameStatus(GAME_STATUS_START);
delegator->onGamePlay();
snake->resetSnake();
setBall();
ball->setVisible(true);
bSetDirection=false;
return ;
}
if(getGameStatus()==GAME_STATUS_OVER){
setGameStatus(GAME_STATUS_READY);
delegator->onGameStart();
snake->removeAllChildren();
ball->setVisible(false);
}
if(getGameStatus()==GAME_STATUS_START)
this->setSnakeDirection(touch->getLocation());
}
void
dxJointHinge::getInfo2( dxJoint::Info2 *info )
{
// Added by OSRF
// If joint values of erp and cfm are negative, then ignore them.
// info->erp, info->cfm already have the global values from quickstep
if (this->erp >= 0)
info->erp = erp;
if (this->cfm >= 0)
{
info->cfm[0] = cfm;
info->cfm[1] = cfm;
info->cfm[2] = cfm;
info->cfm[3] = cfm;
info->cfm[4] = cfm;
info->cfm[5] = cfm;
}
// set the three ball-and-socket rows
setBall( this, info, anchor1, anchor2 );
// set the two hinge rows. the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body
dVector3 p, q; // plane space vectors for ax1
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dPlaneSpace( ax1, p, q );
// strange the rotation matrix is not really a rotation matrix (non-orthogonal vectors)
// normals of columns and rows are not exactly 1 when velocity is large.
// printf("posr.R\n[%f %f %f %f]\n[%f %f %f %f]\n[%f %f %f %f]\n",
// node[0].body->posr.R[0*4+0],node[0].body->posr.R[0*4+1],node[0].body->posr.R[0*4+2],node[0].body->posr.R[0*4+3],
// node[0].body->posr.R[1*4+0],node[0].body->posr.R[1*4+1],node[0].body->posr.R[1*4+2],node[0].body->posr.R[1*4+3],
// node[0].body->posr.R[2*4+0],node[0].body->posr.R[2*4+1],node[0].body->posr.R[2*4+2],node[0].body->posr.R[2*4+3]);
// printf("axis1 [%f %f %f] ax1 [%f %f %f]\n",
// axis1[0], axis1[1], axis1[2],
// ax1[0], ax1[1], ax1[2]);
int s3 = 3 * info->rowskip;
int s4 = 4 * info->rowskip;
info->J1a[s3+0] = p[0];
info->J1a[s3+1] = p[1];
info->J1a[s3+2] = p[2];
info->J1a[s4+0] = q[0];
info->J1a[s4+1] = q[1];
info->J1a[s4+2] = q[2];
if ( node[1].body )
{
info->J2a[s3+0] = -p[0];
info->J2a[s3+1] = -p[1];
info->J2a[s3+2] = -p[2];
info->J2a[s4+0] = -q[0];
info->J2a[s4+1] = -q[1];
info->J2a[s4+2] = -q[2];
}
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1,ax2 are the unit length hinge axes as computed from body1 and
// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if `theta' is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
dVector3 ax2, b;
if ( node[1].body )
{
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
}
else
{
ax2[0] = axis2[0];
ax2[1] = axis2[1];
ax2[2] = axis2[2];
}
dCalcVectorCross3( b, ax1, ax2 );
dReal k = info->fps * info->erp;
info->c[3] = k * dCalcVectorDot3( b, p );
info->c[4] = k * dCalcVectorDot3( b, q );
// if the hinge is powered, or has joint limits, add in the stuff
limot.addLimot( this, info, 5, ax1, 1 );
// joint damping
if (this->use_damping)
{
// added J1ad and J2ad for damping, only 1 row
info->J1ad[0] = ax1[0];
info->J1ad[1] = ax1[1];
info->J1ad[2] = ax1[2];
if ( this->node[1].body )
{
info->J2ad[0] = -ax1[0];
info->J2ad[1] = -ax1[1];
info->J2ad[2] = -ax1[2];
}
// there's no rhs for damping setup, all we want to use is the jacobian information above
}
}
void
dxJointUniversal::getInfo2( dReal worldFPS, dReal worldERP,
int rowskip, dReal *J1, dReal *J2,
int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
int *findex )
{
// set the three ball-and-socket rows
setBall( this, worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, anchor1, anchor2 );
// set the universal joint row. the angular velocity about an axis
// perpendicular to both joint axes should be equal. thus the constraint
// equation is
// p*w1 - p*w2 = 0
// where p is a vector normal to both joint axes, and w1 and w2
// are the angular velocity vectors of the two bodies.
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
// length 1 vector perpendicular to ax1 and ax2. Neither body can rotate
// about this.
dVector3 p;
// Since axis1 and axis2 may not be perpendicular
// we find a axis2_tmp which is really perpendicular to axis1
// and in the plane of axis1 and axis2
getAxes( ax1, ax2 );
dReal k = dCalcVectorDot3( ax1, ax2 );
dVector3 ax2_temp;
dAddVectorScaledVector3(ax2_temp, ax2, ax1, -k);
dCalcVectorCross3( p, ax1, ax2_temp );
dNormalize3( p );
int currRowSkip = 3 * rowskip;
{
dCopyVector3( J1 + currRowSkip + GI2__JA_MIN, p);
if ( node[1].body )
{
dCopyNegatedVector3( J2 + currRowSkip + GI2__JA_MIN, p);
}
}
// compute the right hand side of the constraint equation. set relative
// body velocities along p to bring the axes back to perpendicular.
// If ax1, ax2 are unit length joint axes as computed from body1 and
// body2, we need to rotate both bodies along the axis p. If theta
// is the angle between ax1 and ax2, we need an angular velocity
// along p to cover the angle erp * (theta - Pi/2) in one step:
//
// |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize
// = (erp*fps) * (theta - Pi/2)
//
// if theta is close to Pi/2,
// theta - Pi/2 ~= cos(theta), so
// |angular_velocity| ~= (erp*fps) * (ax1 dot ax2)
int currPairSkip = 3 * pairskip;
{
pairRhsCfm[currPairSkip + GI2_RHS] = worldFPS * worldERP * (-k);
}
currRowSkip += rowskip; currPairSkip += pairskip;
// if the first angle is powered, or has joint limits, add in the stuff
if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 ))
{
currRowSkip += rowskip; currPairSkip += pairskip;
}
// if the second angle is powered, or has joint limits, add in more stuff
limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 );
}
void
dxJointUniversal::getInfo2( dxJoint::Info2 *info )
{
// set the three ball-and-socket rows
setBall( this, info, anchor1, anchor2 );
// set the universal joint row. the angular velocity about an axis
// perpendicular to both joint axes should be equal. thus the constraint
// equation is
// p*w1 - p*w2 = 0
// where p is a vector normal to both joint axes, and w1 and w2
// are the angular velocity vectors of the two bodies.
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
dVector3 ax2_temp;
// length 1 vector perpendicular to ax1 and ax2. Neither body can rotate
// about this.
dVector3 p;
dReal k;
// Since axis1 and axis2 may not be perpendicular
// we find a axis2_tmp which is really perpendicular to axis1
// and in the plane of axis1 and axis2
getAxes( ax1, ax2 );
k = dCalcVectorDot3( ax1, ax2 );
ax2_temp[0] = ax2[0] - k * ax1[0];
ax2_temp[1] = ax2[1] - k * ax1[1];
ax2_temp[2] = ax2[2] - k * ax1[2];
dCalcVectorCross3( p, ax1, ax2_temp );
dNormalize3( p );
int s3 = 3 * info->rowskip;
info->J1a[s3+0] = p[0];
info->J1a[s3+1] = p[1];
info->J1a[s3+2] = p[2];
if ( node[1].body )
{
info->J2a[s3+0] = -p[0];
info->J2a[s3+1] = -p[1];
info->J2a[s3+2] = -p[2];
}
// compute the right hand side of the constraint equation. set relative
// body velocities along p to bring the axes back to perpendicular.
// If ax1, ax2 are unit length joint axes as computed from body1 and
// body2, we need to rotate both bodies along the axis p. If theta
// is the angle between ax1 and ax2, we need an angular velocity
// along p to cover the angle erp * (theta - Pi/2) in one step:
//
// |angular_velocity| = angle/time = erp*(theta - Pi/2) / stepsize
// = (erp*fps) * (theta - Pi/2)
//
// if theta is close to Pi/2,
// theta - Pi/2 ~= cos(theta), so
// |angular_velocity| ~= (erp*fps) * (ax1 dot ax2)
info->c[3] = info->fps * info->erp * - k;
// if the first angle is powered, or has joint limits, add in the stuff
int row = 4 + limot1.addLimot( this, info, 4, ax1, 1 );
// if the second angle is powered, or has joint limits, add in more stuff
limot2.addLimot( this, info, row, ax2, 1 );
}
