示例#1
0
void
dxJointFixed::getInfo2 ( dxJoint::Info2 *info )
{
    int s = info->rowskip;

    // Three rows for orientation
    setFixedOrientation ( this, info, qrel, 3 );

    // Three rows for position.
    // set jacobian
    info->J1l[0] = 1;
    info->J1l[s+1] = 1;
    info->J1l[2*s+2] = 1;

    info->erp = erp;
    info->cfm[0] = cfm;
    info->cfm[1] = cfm;
    info->cfm[2] = cfm;

    dVector3 ofs;
    dMULTIPLY0_331 ( ofs, node[0].body->posr.R, offset );
    if ( node[1].body )
    {
        dCROSSMAT ( info->J1a, ofs, s, + , - );
        info->J2l[0] = -1;
        info->J2l[s+1] = -1;
        info->J2l[2*s+2] = -1;
    }
示例#2
0
void
dxJointFixed::getInfo2 ( dxJoint::Info2 *info )
{
    // 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;
    }

    int s = info->rowskip;

    // Three rows for orientation
    setFixedOrientation ( this, info, qrel, 3 );

    // Three rows for position.
    // set jacobian
    info->J1l[0] = 1;
    info->J1l[s+1] = 1;
    info->J1l[2*s+2] = 1;

    dVector3 ofs;
    dMultiply0_331 ( ofs, node[0].body->posr.R, offset );
    if ( node[1].body )
    {
        dSetCrossMatrixPlus( info->J1a, ofs, s );
        info->J2l[0] = -1;
        info->J2l[s+1] = -1;
        info->J2l[2*s+2] = -1;
    }

    // set right hand side for the first three rows (linear)
    dReal k = info->fps * info->erp;
    if ( node[1].body )
    {
        for ( int j = 0; j < 3; j++ )
            info->c[j] = k * ( node[1].body->posr.pos[j]
                               - node[0].body->posr.pos[j]
                               + ofs[j] );
    }
    else
    {
        for ( int j = 0; j < 3; j++ )
            info->c[j] = k * ( offset[j] - node[0].body->posr.pos[j] );
    }
}
示例#3
0
void
dxJointFixed::getInfo2 ( dxJoint::Info2 *info )
{
    int s = info->rowskip;

    // Three rows for orientation
    setFixedOrientation ( this, info, qrel, 3 );

    // Three rows for position.
    // set jacobian
    info->J1l[0] = 1;
    info->J1l[s+1] = 1;
    info->J1l[2*s+2] = 1;

    info->erp = erp;
    info->cfm[0] = cfm;
    info->cfm[1] = cfm;
    info->cfm[2] = cfm;

    dVector3 ofs;
    dMultiply0_331 ( ofs, node[0].body->posr.R, offset );
    if ( node[1].body )
    {
        dSetCrossMatrixPlus( info->J1a, ofs, s );
        info->J2l[0] = -1;
        info->J2l[s+1] = -1;
        info->J2l[2*s+2] = -1;
    }

    // set right hand side for the first three rows (linear)
    dReal k = info->fps * info->erp;
    if ( node[1].body )
    {
        for ( int j = 0; j < 3; j++ )
            info->c[j] = k * ( node[1].body->posr.pos[j]
                               - node[0].body->posr.pos[j]
                               + ofs[j] );
    }
    else
    {
        for ( int j = 0; j < 3; j++ )
            info->c[j] = k * ( offset[j] - node[0].body->posr.pos[j] );
    }
}
示例#4
0
void
dxJointSlider::getInfo2 ( dReal worldFPS, dReal worldERP, const Info2Descr *info )
{
    int i, s = info->rowskip;
    int s3 = 3 * s, s4 = 4 * s;

    // pull out pos and R for both bodies. also get the `connection'
    // vector pos2-pos1.

    dReal *pos1, *pos2, *R1, *R2;
    dVector3 c;
    pos1 = node[0].body->posr.pos;
    R1 = node[0].body->posr.R;
    if ( node[1].body )
    {
        pos2 = node[1].body->posr.pos;
        R2 = node[1].body->posr.R;
        for ( i = 0; i < 3; i++ )
            c[i] = pos2[i] - pos1[i];
    }
    else
    {
        pos2 = 0;
        R2 = 0;
    }

    // 3 rows to make body rotations equal
    setFixedOrientation ( this, worldFPS, worldERP, info, qrel, 0 );

    // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would
    // result in three equations, so we project along the planespace vectors
    // so that sliding along the slider axis is disregarded. for symmetry we
    // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.

    dVector3 ax1; // joint axis in global coordinates (unit length)
    dVector3 p, q; // plane space of ax1
    dMultiply0_331 ( ax1, R1, axis1 );
    dPlaneSpace ( ax1, p, q );
    if ( node[1].body )
    {
        dVector3 tmp;
        dCalcVectorCross3( tmp, c, p );
        dScaleVector3r4( tmp, REAL( 0.5 ));
        for ( i = 0; i < 3; i++ ) info->J1a[s3+i] = tmp[i];
        for ( i = 0; i < 3; i++ ) info->J2a[s3+i] = tmp[i];
        dCalcVectorCross3( tmp, c, q );
        dScaleVector3r4( tmp, REAL( 0.5 ));
        for ( i = 0; i < 3; i++ ) info->J1a[s4+i] = tmp[i];
        for ( i = 0; i < 3; i++ ) info->J2a[s4+i] = tmp[i];
        for ( i = 0; i < 3; i++ ) info->J2l[s3+i] = -p[i];
        for ( i = 0; i < 3; i++ ) info->J2l[s4+i] = -q[i];
    }
    for ( i = 0; i < 3; i++ ) info->J1l[s3+i] = p[i];
    for ( i = 0; i < 3; i++ ) info->J1l[s4+i] = q[i];

    // compute last two elements of right hand side. we want to align the offset
    // point (in body 2's frame) with the center of body 1.
    dReal k = worldFPS * worldERP;
    if ( node[1].body )
    {
        dVector3 ofs;  // offset point in global coordinates
        dMultiply0_331 ( ofs, R2, offset );
        for ( i = 0; i < 3; i++ ) c[i] += ofs[i];
        info->c[3] = k * dCalcVectorDot3 ( p, c );
        info->c[4] = k * dCalcVectorDot3 ( q, c );
    }
    else
    {
        dVector3 ofs;  // offset point in global coordinates
        for ( i = 0; i < 3; i++ ) ofs[i] = offset[i] - pos1[i];
        info->c[3] = k * dCalcVectorDot3 ( p, ofs );
        info->c[4] = k * dCalcVectorDot3 ( q, ofs );

        if ( flags & dJOINT_REVERSE )
            for ( i = 0; i < 3; ++i ) ax1[i] = -ax1[i];
    }

    // if the slider is powered, or has joint limits, add in the extra row
    limot.addLimot ( this, worldFPS, info, 5, ax1, 0 );
}