C++ dMultiply0_331函数代码示例

void
dxJointHinge2::makeV1andV2()
{
    if ( node[0].body )
    {
        // get axis 1 and 2 in global coords
        dVector3 ax1, ax2, v;
        dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
        dMultiply0_331( ax2, node[1].body->posr.R, axis2 );

        // don't do anything if the axis1 or axis2 vectors are zero or the same
        if ((_dequal(ax1[0], 0.0) && _dequal(ax1[1], 0.0) && _dequal(ax1[2], 0.0)) ||
            (_dequal(ax2[0], 0.0) && _dequal(ax2[1], 0.0) && _dequal(ax2[2], 0.0)) ||
            (_dequal(ax1[0], ax2[0]) && _dequal(ax1[1], ax2[1]) && _dequal(ax1[2], ax2[2])))
          return;

        // modify axis 2 so it's perpendicular to axis 1
        dReal k = dCalcVectorDot3( ax1, ax2 );
        for ( int i = 0; i < 3; i++ ) ax2[i] -= k * ax1[i];
        dNormalize3( ax2 );

        // make v1 = modified axis2, v2 = axis1 x (modified axis2)
        dCalcVectorCross3( v, ax1, ax2 );
        dMultiply1_331( v1, node[0].body->posr.R, ax2 );
        dMultiply1_331( v2, node[0].body->posr.R, v );
    }
}

void
dxJointAMotor::setEulerReferenceVectors()
{
    if ( node[0].body && node[1].body )
    {
        dVector3 r;  // axis[2] and axis[0] in global coordinates
        dMultiply0_331( r, node[1].body->posr.R, axis[2] );
        dMultiply1_331( reference1, node[0].body->posr.R, r );
        dMultiply0_331( r, node[0].body->posr.R, axis[0] );
        dMultiply1_331( reference2, node[1].body->posr.R, r );
    }

    else     // jds
    {
        // else if (j->node[0].body) {
        // dMultiply1_331 (j->reference1,j->node[0].body->posr.R,j->axis[2]);
        // dMultiply0_331 (j->reference2,j->node[0].body->posr.R,j->axis[0]);

        // We want to handle angular motors attached to passive geoms
        dVector3 r;  // axis[2] and axis[0] in global coordinates
        r[0] = axis[2][0];
        r[1] = axis[2][1];
        r[2] = axis[2][2];
        r[3] = axis[2][3];
        dMultiply1_331( reference1, node[0].body->posr.R, r );
        dMultiply0_331( r, node[0].body->posr.R, axis[0] );
        reference2[0] += r[0];
        reference2[1] += r[1];
        reference2[2] += r[2];
        reference2[3] += r[3];
    }
}

void
dxJointHinge2::makeW1andW2()
{
    if ( node[1].body )
    {
        // get axis 1 and 2 in global coords
        dVector3 ax1, ax2, w;
        dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
        dMultiply0_331( ax2, node[1].body->posr.R, axis2 );

        // don't do anything if the axis1 or axis2 vectors are zero or the same
        if (( ax1[0] == 0 && ax1[1] == 0 && ax1[2] == 0 ) ||
            ( ax2[0] == 0 && ax2[1] == 0 && ax2[2] == 0 ) ||
            ( ax1[0] == ax2[0] && ax1[1] == ax2[1] && ax1[2] == ax2[2] ) ) return;

        // modify axis 1 so it's perpendicular to axis 2
        dReal k = dCalcVectorDot3( ax2, ax1 );
        for ( int i = 0; i < 3; i++ ) ax1[i] -= k * ax2[i];
        dNormalize3( ax1 );

        // make w1 = modified axis1, w2 = axis2 x (modified axis1)
        dCalcVectorCross3( w, ax2, ax1 );
        dMultiply1_331( w1, node[1].body->posr.R, ax1 );
        dMultiply1_331( w2, node[1].body->posr.R, w );
    }
}

void dJointGetAMotorAxis( dJointID j, int anum, dVector3 result )
{
    dxJointAMotor* joint = ( dxJointAMotor* )j;
    dAASSERT( joint && anum >= 0 && anum < 3 );
    checktype( joint, AMotor );
    if ( anum < 0 ) anum = 0;
    if ( anum > 2 ) anum = 2;
    if ( joint->rel[anum] > 0 )
    {
        if ( joint->rel[anum] == 1 )
        {
            dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis[anum] );
        }
        else
        {
            if ( joint->node[1].body )   // jds
            {
                dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis[anum] );
            }
            else
            {
                result[0] = joint->axis[anum][0];
                result[1] = joint->axis[anum][1];
                result[2] = joint->axis[anum][2];
                result[3] = joint->axis[anum][3];
            }
        }
    }
    else
    {
        result[0] = joint->axis[anum][0];
        result[1] = joint->axis[anum][1];
        result[2] = joint->axis[anum][2];
    }
}

void dJointGetAMotorAxis( dJointID j, int anum, dVector3 result )
{
    dxJointAMotor* joint = ( dxJointAMotor* )j;
    dAASSERT( joint && anum >= 0 && anum < 3 );
    checktype( joint, AMotor );
    if ( anum < 0 ) anum = 0;
    if ( anum > 2 ) anum = 2;
    
    // If we're in Euler mode, joint->axis[1] doesn't
    // have anything sensible in it.  So don't just return
    // that, find the actual effective axis.
    // Likewise, the actual axis of rotation for the
    // the other axes is different from what's stored.
    if ( joint->mode == dAMotorEuler  ) {
      dVector3 axes[3];
      joint->computeGlobalAxes(axes);
      if (anum == 1) {
        result[0]=axes[1][0];
        result[1]=axes[1][1];
        result[2]=axes[1][2];
      } else if (anum == 0) {
        // This won't be unit length in general,
        // but it's what's used in getInfo2
        // This may be why things freak out as
        // the body-relative axes get close to each other.
        dCalcVectorCross3( result, axes[1], axes[2] );
      } else if (anum == 2) {
        // Same problem as above.
        dCalcVectorCross3( result, axes[0], axes[1] );
      }
    } else if ( joint->rel[anum] > 0 ) {
        if ( joint->rel[anum] == 1 )
        {
            dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis[anum] );
        }
        else
        {
            if ( joint->node[1].body )   // jds
            {
                dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis[anum] );
            }
            else
            {
                result[0] = joint->axis[anum][0];
                result[1] = joint->axis[anum][1];
                result[2] = joint->axis[anum][2];
                result[3] = joint->axis[anum][3];
            }
        }
    }
    else
    {
        result[0] = joint->axis[anum][0];
        result[1] = joint->axis[anum][1];
        result[2] = joint->axis[anum][2];
    }
}

// compute the 3 axes in global coordinates
void
dxJointAMotor::computeGlobalAxes( dVector3 ax[3] )
{
    if ( mode == dAMotorEuler )
    {
        // special handling for euler mode
        dMultiply0_331( ax[0], node[0].body->posr.R, axis[0] );
        if ( node[1].body )
        {
            dMultiply0_331( ax[2], node[1].body->posr.R, axis[2] );
        }
        else
        {
            ax[2][0] = axis[2][0];
            ax[2][1] = axis[2][1];
            ax[2][2] = axis[2][2];
        }
        dCalcVectorCross3( ax[1], ax[2], ax[0] );
        dNormalize3( ax[1] );
    }
    else
    {
        for ( int i = 0; i < num; i++ )
        {
            if ( rel[i] == 1 )
            {
                // relative to b1
                dMultiply0_331( ax[i], node[0].body->posr.R, axis[i] );
            }
            else if ( rel[i] == 2 )
            {
                // relative to b2
                if ( node[1].body )   // jds: don't assert, just ignore
                {
                    dMultiply0_331( ax[i], node[1].body->posr.R, axis[i] );
                }
                else
                {
                    // global - just copy it
                    ax[i][0] = axis[i][0];
                    ax[i][1] = axis[i][1];
                    ax[i][2] = axis[i][2];
                }
            }
            else
            {
                // global - just copy it
                ax[i][0] = axis[i][0];
                ax[i][1] = axis[i][1];
                ax[i][2] = axis[i][2];
            }
        }
    }
}

dReal dJointGetPRPosition( dJointID j )
{
    dxJointPR* joint = ( dxJointPR* ) j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, PR );

    dVector3 q;
    // get the offset in global coordinates
    dMultiply0_331( q, joint->node[0].body->posr.R, joint->offset );

    if ( joint->node[1].body )
    {
        dVector3 anchor2;

        // get the anchor2 in global coordinates
        dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 );

        q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
            ( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
        q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
            ( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
        q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
            ( joint->node[1].body->posr.pos[2] + anchor2[2] ) );

    }
    else
    {
        //N.B. When there is no body 2 the joint->anchor2 is already in
        //     global coordinates

        q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
            ( joint->anchor2[0] ) );
        q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
            ( joint->anchor2[1] ) );
        q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
            ( joint->anchor2[2] ) );

        if ( joint->flags & dJOINT_REVERSE )
        {
            q[0] = -q[0];
            q[1] = -q[1];
            q[2] = -q[2];
        }
    }

    dVector3 axP;
    // get prismatic axis in global coordinates
    dMultiply0_331( axP, joint->node[0].body->posr.R, joint->axisP1 );

    return dCalcVectorDot3( axP, q );
}

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

void dMassRotate (dMass *m, const dMatrix3 R)
{
  // if the body is rotated by `R' relative to its point of reference,
  // the new inertia about the point of reference is:
  //
  //   R * I * R'
  //
  // where I is the old inertia.

  dMatrix3 t1;
  dReal t2[3];

  dAASSERT (m);

  // rotate inertia matrix
  dMultiply2_333 (t1,m->I,R);
  dMultiply0_333 (m->I,R,t1);

  // ensure perfect symmetry
  m->_I(1,0) = m->_I(0,1);
  m->_I(2,0) = m->_I(0,2);
  m->_I(2,1) = m->_I(1,2);

  // rotate center of mass
  dMultiply0_331 (t2,R,m->c);
  m->c[0] = t2[0];
  m->c[1] = t2[1];
  m->c[2] = t2[2];

# ifndef dNODEBUG
  dMassCheck (m);
# endif
}

void dxTriMesh::computeAABB() {
    const dxTriMeshData* d = Data;
    dVector3 c;
    const dMatrix3& R = final_posr->R;
    const dVector3& pos = final_posr->pos;
    
    dMultiply0_331( c, R, d->AABBCenter );
    
    dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) +
        dFabs(R[1] * Data->AABBExtents[1]) + 
        dFabs(R[2] * Data->AABBExtents[2]);
    dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) +
        dFabs(R[5] * Data->AABBExtents[1]) + 
        dFabs(R[6] * Data->AABBExtents[2]);
    dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) +
        dFabs(R[9] * Data->AABBExtents[1]) + 
        dFabs(R[10] * Data->AABBExtents[2]);

    aabb[0] = c[0] + pos[0] - xrange;
    aabb[1] = c[0] + pos[0] + xrange;
    aabb[2] = c[1] + pos[1] - yrange;
    aabb[3] = c[1] + pos[1] + yrange;
    aabb[4] = c[2] + pos[2] - zrange;
    aabb[5] = c[2] + pos[2] + zrange;
}

void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z)
{
    dAASSERT (g);
    dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
    CHECK_NOT_LOCKED (g->parent_space);
    if (g->offset_posr) {
        // move body such that body+offset = position
        dVector3 world_offset;
        dMultiply0_331(world_offset, g->body->posr.R, g->offset_posr->pos);
        dBodySetPosition(g->body,
            x - world_offset[0],
            y - world_offset[1],
            z - world_offset[2]);
    }
    else if (g->body) {
        // this will call dGeomMoved (g), so we don't have to
        dBodySetPosition (g->body,x,y,z);
    }
    else {
        g->final_posr->pos[0] = x;
        g->final_posr->pos[1] = y;
        g->final_posr->pos[2] = z;
        dGeomMoved (g);
    }
}

void
dxJointUniversal::getAxes( dVector3 ax1, dVector3 ax2 )
{
    // This says "ax1 = joint->node[0].body->posr.R * joint->axis1"
    dMultiply0_331( ax1, node[0].body->posr.R, axis1 );

    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];
    }
}

void dJointAddHinge2Torques( dJointID j, dReal torque1, dReal torque2 )
{
    dxJointHinge2* joint = ( dxJointHinge2* )j;
    dVector3 axis1, axis2;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, Hinge2 );

    if ( joint->node[0].body && joint->node[1].body )
    {
        dMultiply0_331( axis1, joint->node[0].body->posr.R, joint->axis1 );
        dMultiply0_331( axis2, joint->node[1].body->posr.R, joint->axis2 );
        axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
        axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
        axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
        dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] );
        dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] );
    }
}

void getAnchor( dxJoint *j, dVector3 result, dVector3 anchor1 )
{
    if ( j->node[0].body )
    {
        dMultiply0_331( result, j->node[0].body->posr.R, anchor1 );
        result[0] += j->node[0].body->posr.pos[0];
        result[1] += j->node[0].body->posr.pos[1];
        result[2] += j->node[0].body->posr.pos[2];
    }
}

void setBall( dxJoint *joint, dxJoint::Info2 *info,
              dVector3 anchor1, dVector3 anchor2 )
{
    // anchor points in global coordinates with respect to body PORs.
    dVector3 a1, a2;

    int s = info->rowskip;

    // set jacobian
    info->J1l[0] = 1;
    info->J1l[s+1] = 1;
    info->J1l[2*s+2] = 1;
    dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
    dSetCrossMatrixMinus( info->J1a, a1, s );
    if ( joint->node[1].body )
    {
        info->J2l[0] = -1;
        info->J2l[s+1] = -1;
        info->J2l[2*s+2] = -1;
        dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 );
        dSetCrossMatrixPlus( info->J2a, a2, s );
    }

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