Пример #1
0
void
dxJointTransmission::getInfo2( dReal worldFPS, 
                               dReal /*worldERP*/,
                               const Info2Descr* info )
 {
    dVector3 a[2], n[2], l[2], r[2], c[2], s, t, O, d, z, u, v;
    dReal theta, delta, nn, na_0, na_1, cosphi, sinphi, m;
    const dReal *p[2], *omega[2];
    int i;

    // Transform all needed quantities to the global frame.

    for (i = 0 ; i < 2 ; i += 1) {
        dBodyGetRelPointPos(node[i].body,
                            anchors[i][0], anchors[i][1], anchors[i][2],
                            a[i]);

        dBodyVectorToWorld(node[i].body, axes[i][0], axes[i][1], axes[i][2],
                           n[i]);

        p[i] = dBodyGetPosition(node[i].body);
        omega[i] = dBodyGetAngularVel(node[i].body);
    }

    if (update) {
        // Make sure both gear reference frames end up with the same
        // handedness.
    
        if (dCalcVectorDot3(n[0], n[1]) < 0) {
            dNegateVector3(axes[0]);
            dNegateVector3(n[0]);
        }
    }

    // Calculate the mesh geometry based on the current mode.
    
    switch (mode) {
    case dTransmissionParallelAxes:
        // Simply calculate the contact point as the point on the
        // baseline that will yield the correct ratio.

        dIASSERT (ratio > 0);
        
        dSubtractVectors3(d, a[1], a[0]);
        dAddScaledVectors3(c[0], a[0], d, 1, ratio / (1 + ratio));
        dCopyVector3(c[1], c[0]);
        
        dNormalize3(d);
        
        for (i = 0 ; i < 2 ; i += 1) {
            dCalcVectorCross3(l[i], d, n[i]);
        }

        break;
    case dTransmissionIntersectingAxes:
        // Calculate the line of intersection between the planes of the
        // gears.

        dCalcVectorCross3(l[0], n[0], n[1]);
        dCopyVector3(l[1], l[0]);

        nn = dCalcVectorDot3(n[0], n[1]);
        dIASSERT(fabs(nn) != 1);
        
        na_0 = dCalcVectorDot3(n[0], a[0]);
        na_1 = dCalcVectorDot3(n[1], a[1]);

        dAddScaledVectors3(O, n[0], n[1],
                           (na_0 - na_1 * nn) / (1 - nn * nn),
                           (na_1 - na_0 * nn) / (1 - nn * nn));

        // Find the contact point as:
        //
        // c = ((r_a - O) . l) l + O
        //
        // where r_a the anchor point of either gear and l, O the tangent
        // line direction and origin.

        for (i = 0 ; i < 2 ; i += 1) {
            dSubtractVectors3(d, a[i], O);
            m = dCalcVectorDot3(d, l[i]);        
            dAddScaledVectors3(c[i], O, l[i], 1, m);
        }

        break;
    case dTransmissionChainDrive:
        dSubtractVectors3(d, a[0], a[1]);
        m = dCalcVectorLength3(d);

        dIASSERT(m > 0);
        
        // Caclulate the angle of the contact point relative to the
        // baseline.

        cosphi = clamp((radii[1] - radii[0]) / m, REAL(-1.0), REAL(1.0)); // Force into range to fix possible computation errors
        sinphi = dSqrt (REAL(1.0) - cosphi * cosphi);

        dNormalize3(d);

        for (i = 0 ; i < 2 ; i += 1) {
            // Calculate the contact radius in the local reference
            // frame of the chain.  This has axis x pointing along the
            // baseline, axis y pointing along the sprocket axis and
            // the remaining axis normal to both.

            u[0] = radii[i] * cosphi;
            u[1] = 0;
            u[2] = radii[i] * sinphi;

            // Transform the contact radius into the global frame.

            dCalcVectorCross3(z, d, n[i]);
            
            v[0] = dCalcVectorDot3(d, u);
            v[1] = dCalcVectorDot3(n[i], u);
            v[2] = dCalcVectorDot3(z, u);

            // Finally calculate contact points and l.
            
            dAddVectors3(c[i], a[i], v);
            dCalcVectorCross3(l[i], v, n[i]);
            dNormalize3(l[i]);

            // printf ("%d: %f, %f, %f\n",
            //      i, l[i][0], l[i][1], l[i][2]);
        }

        break;
    }

    if (update) {
        // We need to calculate an initial reference frame for each
        // wheel which we can measure the current phase against.  This
        // frame will have the initial contact radius as the x axis,
        // the wheel axis as the z axis and their cross product as the
        // y axis.

        for (i = 0 ; i < 2 ; i += 1) {
            dSubtractVectors3 (r[i], c[i], a[i]);
            radii[i] = dCalcVectorLength3(r[i]);
            dIASSERT(radii[i] > 0);
            
            dBodyVectorFromWorld(node[i].body, r[i][0], r[i][1], r[i][2],
                                 reference[i]);
            dNormalize3(reference[i]);
            dCopyVector3(reference[i] + 8, axes[i]);
            dCalcVectorCross3(reference[i] + 4, reference[i] + 8, reference[i]);

            // printf ("%f\n", dDOT(r[i], n[i]));
            // printf ("(%f, %f, %f,\n %f, %f, %f,\n %f, %f, %f)\n",
            //      reference[i][0],reference[i][1],reference[i][2],
            //      reference[i][4],reference[i][5],reference[i][6],
            //      reference[i][8],reference[i][9],reference[i][10]);

            radii[i] = radii[i];
            phase[i] = 0;
        }

        ratio = radii[0] / radii[1];
        update = 0;
    }
    
    for (i = 0 ; i < 2 ; i += 1) {
        dReal phase_hat;

        dSubtractVectors3 (r[i], c[i], a[i]);
        
        // Transform the (global) contact radius into the gear's
        // reference frame.

        dBodyVectorFromWorld (node[i].body, r[i][0], r[i][1], r[i][2], s);
        dMultiply0_331(t, reference[i], s);

        // Now simply calculate its angle on the plane relative to the
        // x-axis which is the initial contact radius.  This will be
        // an angle between -pi and pi that is coterminal with the
        // actual phase of the wheel.  To find the real phase we
        // estimate it by adding omega * dt to the old phase and then
        // find the closest angle to that, that is coterminal to
        // theta.

        theta = atan2(t[1], t[0]);
        phase_hat = phase[i] + dCalcVectorDot3(omega[i], n[i]) / worldFPS;

        if (phase_hat > M_PI_2) {
            if (theta < 0) {
                theta += (dReal)(2 * M_PI);
            }

            theta += (dReal)(floor(phase_hat / (2 * M_PI)) * (2 * M_PI));
        } else if (phase_hat < -M_PI_2) {
            if (theta > 0) {
                theta -= (dReal)(2 * M_PI);
            }

            theta += (dReal)(ceil(phase_hat / (2 * M_PI)) * (2 * M_PI));
        }
                
        if (phase_hat - theta > M_PI) {
            phase[i] = theta + (dReal)(2 * M_PI);
        } else if (phase_hat - theta < -M_PI) {
            phase[i] = theta - (dReal)(2 * M_PI);
        } else {
            phase[i] = theta;
        }

        dIASSERT(fabs(phase_hat - phase[i]) < M_PI);
    }

    // Calculate the phase error.  Depending on the mode the condition
    // is that the distances traveled by each contact point must be
    // either equal (chain and sprockets) or opposite (gears).

    if (mode == dTransmissionChainDrive) {
        delta = (dCalcVectorLength3(r[0]) * phase[0] -
                 dCalcVectorLength3(r[1]) * phase[1]);
    } else {
        delta = (dCalcVectorLength3(r[0]) * phase[0] +
                 dCalcVectorLength3(r[1]) * phase[1]);
    }

    // When in chain mode a torque reversal, signified by the change
    // in sign of the wheel phase difference, has the added effect of
    // switching the active chain branch.  We must therefore reflect
    // the contact points and tangents across the baseline.
    
    if (mode == dTransmissionChainDrive && delta < 0) {
        dVector3 d;

        dSubtractVectors3(d, a[0], a[1]);
        
        for (i = 0 ; i < 2 ; i += 1) {
            dVector3 nn;
            dReal a;
            
            dCalcVectorCross3(nn, n[i], d);
            a = dCalcVectorDot3(nn, nn);
            dIASSERT(a > 0);
            
            dAddScaledVectors3(c[i], c[i], nn,
                               1, -2 * dCalcVectorDot3(c[i], nn) / a);
            dAddScaledVectors3(l[i], l[i], nn,
                               -1, 2 * dCalcVectorDot3(l[i], nn) / a);
        }
    }

    // Do not add the constraint if there's backlash and we're in the
    // backlash gap.

    if (backlash == 0 || fabs(delta) > backlash) {
        // The constraint is satisfied iff the absolute velocity of the
        // contact point projected onto the tangent of the wheels is equal
        // for both gears.  This velocity can be calculated as:
        // 
        // u = v + omega x r_c
        // 
        // The constraint therefore becomes:
        // (v_1 + omega_1 x r_c1) . l = (v_2 + omega_2 x r_c2) . l <=>
        // (v_1 . l + (r_c1 x l) . omega_1 = v_2 . l + (r_c2 x l) . omega_2

        for (i = 0 ; i < 2 ; i += 1) {
            dSubtractVectors3 (r[i], c[i], p[i]);
        }

        dCalcVectorCross3(info->J1a, r[0], l[0]);
        dCalcVectorCross3(info->J2a, l[1], r[1]);

        dCopyVector3(info->J1l, l[0]);
        dCopyNegatedVector3(info->J2l, l[1]);

        if (delta > 0) {
            if (backlash > 0) {
                info->lo[0] = -dInfinity;
                info->hi[0] = 0;
            }

            info->c[0] = -worldFPS * erp * (delta - backlash);
        } else {
            if (backlash > 0) {
                info->lo[0] = 0;
                info->hi[0] = dInfinity;
            }

            info->c[0] = -worldFPS * erp * (delta + backlash);
        }
    }

    info->cfm[0] = cfm;

    // printf ("%f, %f, %f, %f, %f\n", delta, phase[0], phase[1], -phase[1] / phase[0], ratio);

    // Cache the contact point (in world coordinates) to avoid
    // recalculation if requested by the user.

    dCopyVector3(contacts[0], c[0]);
    dCopyVector3(contacts[1], c[1]);
}
Пример #2
0
void
dxJointPU::getInfo2( dReal worldFPS, dReal worldERP, 
    int rowskip, dReal *J1, dReal *J2,
    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
    int *findex )
{
    const dReal k = worldFPS * worldERP;

    // ======================================================================
    // The angular constraint
    //
    dVector3 ax1, ax2; // Global axes of rotation
    getAxis(this, ax1, axis1);
    getAxis2(this,ax2, axis2);

    dVector3 uniPerp;  // Axis perpendicular to axes of rotation
    dCalcVectorCross3(uniPerp,ax1,ax2);
    dNormalize3( uniPerp );

    dCopyVector3( J1 + GI2__JA_MIN, uniPerp );

    dxBody *body1 = node[1].body;

    if ( body1 ) {
        dCopyNegatedVector3( J2 + GI2__JA_MIN , uniPerp );
    }
    // Corrective velocity attempting to keep uni axes perpendicular
    dReal val = dCalcVectorDot3( ax1, ax2 );
    // Small angle approximation : 
    // theta = asin(val)
    // theta is approximately val when val is near zero.
    pairRhsCfm[GI2_RHS] = -k * val; 
    
    // ==========================================================================
    // Handle axes orthogonal to the prismatic 
    dVector3 an1, an2; // Global anchor positions
    dVector3 axP, sep; // Prismatic axis and separation vector
    getAnchor(this, an1, anchor1);
    getAnchor2(this, an2, anchor2);

    if (flags & dJOINT_REVERSE) {
        getAxis2(this, axP, axisP1);
    } else {
        getAxis(this, axP, axisP1);
    }
    dSubtractVectors3(sep, an2, an1);

    dVector3 p, q;
    dPlaneSpace(axP, p, q);

    dCopyVector3( J1 + rowskip + GI2__JL_MIN, p );
    dCopyVector3( J1 + 2 * rowskip + GI2__JL_MIN, q );
    // Make the anchors be body local
    // Aliasing isn't a problem here.
    dSubtractVectors3(an1, an1, node[0].body->posr.pos);
    dCalcVectorCross3( J1 + rowskip + GI2__JA_MIN, an1, p );
    dCalcVectorCross3( J1 + 2 * rowskip + GI2__JA_MIN, an1, q );

    if (body1) {
        dCopyNegatedVector3( J2 + rowskip + GI2__JL_MIN, p );
        dCopyNegatedVector3( J2 + 2 * rowskip + GI2__JL_MIN, q );
        dSubtractVectors3(an2, an2, body1->posr.pos);
        dCalcVectorCross3( J2 + rowskip + GI2__JA_MIN, p, an2 );
        dCalcVectorCross3( J2 + 2 * rowskip + GI2__JA_MIN, q, an2 );
    }

    pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( p, sep );
    pairRhsCfm[2 * pairskip + GI2_RHS] = k * dCalcVectorDot3( q, sep );
    
    // ==========================================================================
    // Handle the limits/motors
    int currRowSkip = 3 * rowskip, currPairSkip = 3 * pairskip;

    if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 )) {
        currRowSkip += rowskip; currPairSkip += pairskip;
    }

    if (limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 )) {
        currRowSkip += rowskip; currPairSkip += pairskip;
    }

    if (  body1 || (flags & dJOINT_REVERSE) == 0 ) {
        limotP.addTwoPointLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2 );
    } else {
        dNegateVector3(axP);
        limotP.addTwoPointLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2  );
    }
}
Пример #3
0
void
dxJointPU::getInfo2( dReal worldFPS, dReal worldERP, const Info2Descr *info )
{
    const int s1 = info->rowskip;
    const int s2 = 2 * s1;
    const dReal k = worldFPS * worldERP;

    // ======================================================================
    // The angular constraint
    //
    dVector3 ax1, ax2; // Global axes of rotation
    getAxis(this, ax1, axis1);
    getAxis2(this,ax2, axis2);

    dVector3 uniPerp;  // Axis perpendicular to axes of rotation
    dCalcVectorCross3(uniPerp,ax1,ax2);
    dNormalize3( uniPerp );
    dCopyVector3( info->J1a , uniPerp );
    if ( node[1].body )
    {
        dCopyNegatedVector3( info->J2a , uniPerp );
    }
    // Corrective velocity attempting to keep uni axes perpendicular
    dReal val = dCalcVectorDot3( ax1, ax2 );
    // Small angle approximation : 
    // theta = asin(val)
    // theta is approximately val when val is near zero.
    info->c[0] = -k * val; 
    
    // ==========================================================================
    // Handle axes orthogonal to the prismatic 
    dVector3 an1, an2; // Global anchor positions
    dVector3 axP, sep; // Prismatic axis and separation vector
    getAnchor(this,an1,anchor1);
    getAnchor2(this,an2,anchor2);
    if (flags & dJOINT_REVERSE) {
        getAxis2(this, axP, axisP1);
    } else {
        getAxis(this, axP, axisP1);
    }
    dSubtractVectors3(sep,an2,an1);

    dVector3 p,q;
    dPlaneSpace(axP,p,q);

    dCopyVector3(( info->J1l ) + s1, p );
    dCopyVector3(( info->J1l ) + s2, q );
    // Make the anchors be body local
    // Aliasing isn't a problem here.
    dSubtractVectors3(an1,an1,node[0].body->posr.pos);
    dCalcVectorCross3(( info->J1a ) + s1, an1, p );
    dCalcVectorCross3(( info->J1a ) + s2, an1, q );

    if (node[1].body) {
        dCopyNegatedVector3(( info->J2l ) + s1, p );
        dCopyNegatedVector3(( info->J2l ) + s2, q );
        dSubtractVectors3(an2,an2,node[1].body->posr.pos);
        dCalcVectorCross3(( info->J2a ) + s1, p, an2 );
        dCalcVectorCross3(( info->J2a ) + s2, q, an2 );
    }

    info->c[1] = k * dCalcVectorDot3( p, sep );
    info->c[2] = k * dCalcVectorDot3( q, sep );
    
    // ==========================================================================
    // Handle the limits/motors
    int row = 3 + limot1.addLimot( this, worldFPS, info, 3, ax1, 1 );
    row += limot2.addLimot( this, worldFPS, info, row, ax2, 1 );

    if (  node[1].body || !(flags & dJOINT_REVERSE) )
        limotP.addTwoPointLimot( this, worldFPS, info, row, axP, an1, an2 );
    else
    {
        axP[0] = -axP[0];
        axP[1] = -axP[1];
        axP[2] = -axP[2];
        limotP.addTwoPointLimot ( this, worldFPS, info, row, axP, an1, an2  );
    }
}
Пример #4
0
void
dxJointPU::getInfo2( dxJoint::Info2 *info )
{
    const int s0 = 0;
    const int s1 = info->rowskip;
    const int s2 = 2 * s1;

    const dReal k = info->fps * info->erp;

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

    dReal *pos1, *pos2 = 0, *R1, *R2 = 0;
    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;
    }

    dVector3 axP; // Axis of the prismatic joint in global frame
    dMultiply0_331( axP, R1, axisP1 );

    // distance between the body1 and the anchor2 in global frame
    // Calculated in the same way as the offset
    dVector3 dist;
    dVector3 wanchor2 = {0,0,0};
    if ( node[1].body )
    {
        dMultiply0_331( wanchor2, R2, anchor2 );
        dist[0] = wanchor2[0] + pos2[0] - pos1[0];
        dist[1] = wanchor2[1] + pos2[1] - pos1[1];
        dist[2] = wanchor2[2] + pos2[2] - pos1[2];
    }
    else
    {
        if (flags & dJOINT_REVERSE )
        {
            // Invert the sign of dist
            dist[0] = pos1[0] - anchor2[0];
            dist[1] = pos1[1] - anchor2[1];
            dist[2] = pos1[2] - anchor2[2];
        }
        else
        {
            dist[0] = anchor2[0] - pos1[0];
            dist[1] = anchor2[1] - pos1[1];
            dist[2] = anchor2[2] - pos1[2];
        }
    }

    dVector3 q; // Temporary axis vector
    // Will be used at 2 places with 2 different meaning

    // ======================================================================
    // Work on the angular part (i.e. row 0)
    //

    // The axis perpendicular to both axis1 and axis2 should be the only unconstrained
    // rotational axis, the angular velocity of the two bodies perpendicular to
    // the rotoide axes should be equal. Thus the constraint equations are
    //    p*w1 - p*w2 = 0
    // where p is a unit vector perpendicular to both axis1 and axis2
    // and w1 and w2 are the angular velocity vectors of the two bodies.
    dVector3 ax1, ax2;
    getAxes( ax1, ax2 );
    dReal val = dCalcVectorDot3( ax1, ax2 );
    q[0] = ax2[0] - val * ax1[0];
    q[1] = ax2[1] - val * ax1[1];
    q[2] = ax2[2] - val * ax1[2];

    dVector3 p;
    dCalcVectorCross3( p, ax1, q );
    dNormalize3( p );

    //   info->J1a[s0+i] = p[i];
    dCopyVector3(( info->J1a ) + s0, p );

    if ( node[1].body )
    {
        //   info->J2a[s0+i] = -p[i];
        dCopyNegatedVector3(( info->J2a ) + s0, 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)

    info->c[0] = k * - val;



    // ==========================================================================
    // Work on the linear part (i.e rows 1 and 2)
    //
    // 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 axisP is disregarded.
    //
    // p1 + R1 dist' = p2 + R2 anchor2'
    // v1 + w1 x R1 dist' + v_p = v2 + w2 x R2 anchor2'
    // v_p is speed of prismatic joint (i.e. elongation rate)
    // Since the constraints are perpendicular to v_p we have:
    // e1 dot v_p = 0 and e2 dot v_p = 0
    // e1 dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
    // e2 dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
    // ==
    // e1 . v1 + e1 . w1 x dist = e1 . v2 + e1 . w2 x anchor2
    // since a . (b x c) = - b . (a x c) = - (a x c) . b
    // and a x b = - b x a
    // e1 . v1 - e1 x dist . w1 - e1 . v2 - (- e1 x anchor2 . w2) = 0
    // e1 . v1 + dist x e1 . w1 - e1 . v2 - anchor2 x e1 . w2 = 0
    // Coeff for 1er line of: J1l => e1, J2l => -e1
    // Coeff for 2er line of: J1l => e2, J2l => -ax2
    // Coeff for 1er line of: J1a => dist x e1, J2a => - anchor2 x e1
    // Coeff for 2er line of: J1a => dist x e2, J2a => - anchor2 x e2
    // e1 and e2 are perpendicular to axP
    // so e1 = ax1 and e2 = ax1 x axP
    // N.B. ax2 is not always perpendicular to axP since it is attached to body 2
    dCalcVectorCross3( q , ax1, axP );

    dMultiply0_331( axP, R1, axisP1 );

    dCalcVectorCross3(( info->J1a ) + s1, dist, ax1 );
    dCalcVectorCross3(( info->J1a ) + s2, dist, q );

    // info->J1l[s1+i] = ax[i];
    dCopyVector3(( info->J1l ) + s1, ax1 );

    // info->J1l[s2+i] = q[i];
    dCopyVector3(( info->J1l ) + s2, q );

    if ( node[1].body )
    {
        // Calculate anchor2 in world coordinate

        // q x anchor2 instead of anchor2 x q since we want the negative value
        dCalcVectorCross3(( info->J2a ) + s1, ax1, wanchor2 );
        // The cross product is in reverse order since we want the negative value
        dCalcVectorCross3(( info->J2a ) + s2, q, wanchor2 );


        // info->J2l[s1+i] = -ax1[i];
        dCopyNegatedVector3(( info->J2l ) + s1, ax1 );
        // info->J2l[s2+i] = -ax1[i];
        dCopyNegatedVector3(( info->J2l ) + s2, q );

    }


    // We want to make correction for motion not in the line of the axisP
    // We calculate the displacement w.r.t. the anchor pt.
    //
    // compute the elements 1 and 2 of right hand side.
    // We want to align the offset point (in body 2's frame) with the center of body 1.
    // The position should be the same when we are not along the prismatic axis
    dVector3 err;
    dMultiply0_331( err, R1, anchor1 );
    // err[i] = dist[i] - err[i];
    dSubtractVectors3( err, dist, err );
    info->c[1] = k * dCalcVectorDot3( ax1, err );
    info->c[2] = k * dCalcVectorDot3( q, err );

    int row = 3 + limot1.addLimot( this, info, 3, ax1, 1 );
    row += limot2.addLimot( this, info, row, ax2, 1 );

    if (  node[1].body || !(flags & dJOINT_REVERSE) )
        limotP.addLimot( this, info, row, axP, 0 );
    else
    {
        axP[0] = -axP[0];
        axP[1] = -axP[1];
        axP[2] = -axP[2];
        limotP.addLimot ( this, info, row, axP, 0 );
    }
}
Пример #5
0
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 );
}
Пример #6
0
void
dxJointPR::getInfo2( dReal worldFPS, dReal worldERP, 
    int rowskip, dReal *J1, dReal *J2,
    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
    int *findex )
{
    dReal k = worldFPS * worldERP;


    dVector3 q;  // plane space of axP and after that axR

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

    dReal *pos2 = NULL, *R2 = NULL;
    
    dReal *pos1 = node[0].body->posr.pos;
    dReal *R1 = node[0].body->posr.R;

    dxBody *body1 = node[1].body;

    if ( body1 )
    {
        pos2 = body1->posr.pos;
        R2 = body1->posr.R;
    }


    dVector3 axP; // Axis of the prismatic joint in global frame
    dMultiply0_331( axP, R1, axisP1 );

    // distance between the body1 and the anchor2 in global frame
    // Calculated in the same way as the offset
    dVector3 wanchor2 = {0, 0, 0}, dist;

    if ( body1 )
    {
        // Calculate anchor2 in world coordinate
        dMultiply0_331( wanchor2, R2, anchor2 );
        dist[0] = wanchor2[0] + pos2[0] - pos1[0];
        dist[1] = wanchor2[1] + pos2[1] - pos1[1];
        dist[2] = wanchor2[2] + pos2[2] - pos1[2];
    }
    else
    {
        if ( (flags & dJOINT_REVERSE) != 0 )
        {
            dSubtractVectors3(dist, pos1, anchor2); // Invert the value
        }
        else
        {
            dSubtractVectors3(dist, anchor2, pos1); // Invert the value
        }
    }


    // ======================================================================
    // Work on the Rotoide part (i.e. row 0, 1 and maybe 4 if rotoide powered

    // Set the two rotoide rows. The rotoide axis should be the only unconstrained
    // rotational axis, the angular velocity of the two bodies perpendicular to
    // the rotoide 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 rotoide axis, and w1 and w2
    // are the angular velocity vectors of the two bodies.
    dVector3 ax2;
    dVector3 ax1;
    dMultiply0_331( ax1, R1, axisR1 );
    dCalcVectorCross3( q , ax1, axP );

    dCopyVector3(J1 + GI2__JA_MIN, axP);

    if ( body1 )
    {
        dCopyNegatedVector3(J2 + GI2__JA_MIN, axP);
    }

    dCopyVector3(J1 + rowskip + GI2__JA_MIN, q);

    if ( body1 )
    {
        dCopyNegatedVector3(J2 + rowskip + GI2__JA_MIN, q);
    }

    // Compute the right hand side of the constraint equation set. Relative
    // body velocities along p and q to bring the rotoide back into alignment.
    // ax1,ax2 are the unit length rotoide axes of body1 and body2 in world frame.
    // 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.

    if ( body1 )
    {
        dMultiply0_331( ax2, R2, axisR2 );
    }
    else
    {
        dCopyVector3(ax2, axisR2);
    }

    dVector3 b;
    dCalcVectorCross3( b, ax1, ax2 );
    pairRhsCfm[GI2_RHS] = k * dCalcVectorDot3( b, axP );
    pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( b, q );



    // ==========================
    // Work on the Prismatic part (i.e row 2,3 and 4 if only the prismatic is powered
    // or 5 if rotoide and prismatic powered

    // 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 prismatic axis is disregarded. for symmetry we
    // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.

    // p1 + R1 dist' = p2 + R2 anchor2' ## OLD ## p1 + R1 anchor1' = p2 + R2 dist'
    // v1 + w1 x R1 dist' + v_p = v2 + w2 x R2 anchor2'## OLD  v1 + w1 x R1 anchor1' = v2 + w2 x R2 dist' + v_p
    // v_p is speed of prismatic joint (i.e. elongation rate)
    // Since the constraints are perpendicular to v_p we have:
    // p dot v_p = 0 and q dot v_p = 0
    // ax1 dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
    // q dot ( v1 + w1 x dist = v2 + w2 x anchor2 )
    // ==
    // ax1 . v1 + ax1 . w1 x dist = ax1 . v2 + ax1 . w2 x anchor2 ## OLD ## ax1 . v1 + ax1 . w1 x anchor1 = ax1 . v2 + ax1 . w2 x dist
    // since a . (b x c) = - b . (a x c) = - (a x c) . b
    // and a x b = - b x a
    // ax1 . v1 - ax1 x dist . w1 - ax1 . v2 - (- ax1 x anchor2 . w2) = 0
    // ax1 . v1 + dist x ax1 . w1 - ax1 . v2 - anchor2 x ax1 . w2 = 0
    // Coeff for 1er line of: J1l => ax1, J2l => -ax1
    // Coeff for 2er line of: J1l => q, J2l => -q
    // Coeff for 1er line of: J1a => dist x ax1, J2a => - anchor2 x ax1
    // Coeff for 2er line of: J1a => dist x q,   J2a => - anchor2 x q

    int currRowSkip = 2 * rowskip;
    {
        dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, ax1 );
        dCalcVectorCross3( J1 + currRowSkip + GI2__JA_MIN, dist, ax1 );

        if ( body1 )
        {
            dCopyNegatedVector3( J2 + currRowSkip + GI2__JL_MIN, ax1 );
            // ax2 x anchor2 instead of anchor2 x ax2 since we want the negative value
            dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, ax2, wanchor2 );   // since ax1 == ax2
        }
    }

    currRowSkip += rowskip;
    {
        dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, q );
        dCalcVectorCross3(J1 + currRowSkip + GI2__JA_MIN, dist, q );

        if ( body1 )
        {
            dCopyNegatedVector3( J2 + currRowSkip + GI2__JL_MIN, q);
            // The cross product is in reverse order since we want the negative value
            dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, q, wanchor2 );
        }
    }

    // We want to make correction for motion not in the line of the axisP
    // We calculate the displacement w.r.t. the anchor pt.
    //
    // compute the elements 2 and 3 of right hand side.
    // we want to align the offset point (in body 2's frame) with the center of body 1.
    // The position should be the same when we are not along the prismatic axis
    dVector3 err;
    dMultiply0_331( err, R1, offset );
    dSubtractVectors3(err, dist, err);

    int currPairSkip = 2 * pairskip;
    {
        pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( ax1, err );
    }

    currPairSkip += pairskip;
    {
        pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3( q, err );
    }

    currRowSkip += rowskip; currPairSkip += pairskip;

    if (  body1 || (flags & dJOINT_REVERSE) == 0 )
    {
        if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, 0 ))
        {
            currRowSkip += rowskip; currPairSkip += pairskip;
        }
    }
    else
    {
        dVector3 rAxP;
        dCopyNegatedVector3(rAxP, axP);

        if (limotP.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, rAxP, 0 ))
        {
            currRowSkip += rowskip; currPairSkip += pairskip;
        }
    }

    limotR.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 );
}
Пример #7
0
void dxPlanarJoint::getInfo2(dReal worldFPS, dReal worldERP, const Info2Descr* info)
{
    /* ====================================================================
     * This joint restricts 2 rotational and 1 translational DOF.
     *
     * The 2 rotational constraints are essentially the same as in dxJointPiston, so the implementation here is mostly
     * just a copy of them.
     *
     * The translational 1DOF constraint has not yet been implemented for any joint in ODE, so we'll talk more about it.
     * It is similar to the constraints in a slider joint, but in this case we only want body2 to move the plane, and
     * body1 anchor has no use in this case, too.
     *
     * We basically want to express the plane in body2 local coordinates (because it should move along with body2), and
     * check, if body1 origin lies in the plane.
     *
     * Let's say n' is the plane normal in body2 coordinates and a' is the anchor point in body2 coordinates
     * (with n, a, being the corresponding global variables).
     * Further, let's call the global position of body1 p1, and similarly p2 for body2.
     * Last, let's call body2 rotation matrix R2 (so that n = R2*n' and a = R2*a' + p2).
     *
     * The plane can then be expressed in global coordinates as:
     * (n^ . x) - (n^ . a) = 0,
     * where x is in global coordinates and ^ denotes vector/matrix transpose.
     *
     * Rewriting the equation using body2 local coordinates and setting x=p1, we get the constraint:
     * (R2*n')^ * p1 - (R2*n')^ * (R2*a' + p2) = 0
     * ...
     * (n'^ * R2^ * p1) - (n'^ * R2^ * p2) - (n'^ * a') = 0
     * (n^ * p1) - (n^ * p2) - (n'^ * a') = 0
     *
     * The part left to "=" will be the "c" on the RHS of the Jacobian equation (because it expresses
     * the distance of p1 from the plane).
     *
     * Next, we need to take time derivative of the constraint to get the J1 and J2 parts.
     * v1, v2, w1 and w2 denote the linear and angular velocities od body1 and body2.
     * [a]x denotes the skew-symmetric cross-product matrix of vector a.
     *
     * d/dt [(n'^ * R2^ * p1) - (n'^ * R2^ * p2) - (n'^ * a') = 0]        (n', a' are constant in time)
     * n'^ * ((d/dt[R2^] * p1) + (R2^ * v1)) - n'^ * ((d/dt[R2^] * p2) + (R2^ * v2)) = 0
     * n'^ * ((([w2]x R2)^ * p1) + (R2^ * v1)) - n'^ * ((([w2]x R2)^ * p2) + (R2^ * v2)) = 0
     * ...
     * n^*v1 - n^*v2 + [n]x (p1 - p2) . w2 = 0
     * -n^*v1 + n^*v2 + [n]x (p2 - p1) . w2 = 0
     *
     * Thus we see that
     * J1l = -n
     * J2l = n
     * J1a = 0
     * J2a = [n]x (p2 - p1)
     * c = eps * ( (n^ * p1) - (n^ * a) )
     *
     */

    const int row0Index = 0;
    const int row1Index = info->rowskip;
    const int row2Index = 2 * row1Index;
    const dReal eps = worldFPS * worldERP;

    dVector3 globalAxis1;
    dBodyVectorToWorld(node[0].body, axis1[0], axis1[1], axis1[2], globalAxis1);

    dVector3 globalAxis2;
    dVector3 globalAnchor;

    if (node[1].body) {
        dBodyVectorToWorld(node[1].body, axis2[0], axis2[1], axis2[2], globalAxis2);
        dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalAnchor);
    } else {
        dCopyVector3(globalAxis2, axis2);
        dCopyVector3(globalAnchor, anchor2);
    }

    ///////////////////////////////////////////////////////
    // Angular velocity constraints (2 rows)
    //
    // Refer to the piston joint source code for details.
    ///////////////////////////////////////////////////////

    // Find the 2 direction vectors of the plane.
    dVector3 p, q;
    dPlaneSpace ( globalAxis2, p, q );

    // LHS 0
    dCopyNegatedVector3 (info->J1a + row0Index, p );
    dCopyNegatedVector3 (info->J1a + row1Index, q );

    // LHS 1
    dCopyVector3 (info->J2a + row0Index, p );
    dCopyVector3 (info->J2a + row1Index, q );

    // RHS 0 & 1 (absolute errors of the axis direction computed by dot product of the desired and actual axis)
    dVector3 b;
    dCalcVectorCross3( b, globalAxis2, globalAxis1 );
    info->c[0] = eps * dCalcVectorDot3 ( p, b );
    info->c[1] = eps * dCalcVectorDot3 ( q, b );

    //////////////////////////////////////////////////////////////////////////////////////////
    // Linear velocity constraint (1 row, removes 1 translational DOF along the plane normal)
    //
    // Only body1 should be restricted by the plane, which is moved along with body2.
    //////////////////////////////////////////////////////////////////////////////////////////

    dVector3 body1Center;
    dCopyVector3(body1Center, node[0].body->posr.pos);

    dVector3 body2Center = {0, 0, 0};
    if (node[1].body) {
        dCopyVector3(body2Center, node[1].body->posr.pos);
    }

    // LHS 2
    dCopyNegatedVector3(info->J1l + row2Index, globalAxis2);
    dCopyVector3(       info->J2l + row2Index, globalAxis2);

    dVector3 body2_body1;
    dSubtractVectors3(body2_body1, body2Center, body1Center);
    dCalcVectorCross3(info->J2a + row2Index, globalAxis2, body2_body1);

    // RHS 2 (distance of body1 center from the plane)
    info->c[2] = eps * (dCalcVectorDot3(globalAxis2, body1Center) - dCalcVectorDot3(globalAxis2, globalAnchor));
}