Exemple #1
0
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 setAnchors( dxJoint *j, dReal x, dReal y, dReal z,
                 dVector3 anchor1, dVector3 anchor2 )
{
    if ( j->node[0].body )
    {
        dReal q[4];
        q[0] = x - j->node[0].body->posr.pos[0];
        q[1] = y - j->node[0].body->posr.pos[1];
        q[2] = z - j->node[0].body->posr.pos[2];
        q[3] = 0;
        dMultiply1_331( anchor1, j->node[0].body->posr.R, q );
        if ( j->node[1].body )
        {
            q[0] = x - j->node[1].body->posr.pos[0];
            q[1] = y - j->node[1].body->posr.pos[1];
            q[2] = z - j->node[1].body->posr.pos[2];
            q[3] = 0;
            dMultiply1_331( anchor2, j->node[1].body->posr.R, q );
        }
        else
        {
            anchor2[0] = x;
            anchor2[1] = y;
            anchor2[2] = z;
        }
    }
    anchor1[3] = 0;
    anchor2[3] = 0;
}
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 );
    }
}
Exemple #4
0
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 setAxes( dxJoint *j, dReal x, dReal y, dReal z,
              dVector3 axis1, dVector3 axis2 )
{
    if ( j->node[0].body )
    {
        dReal q[4];
        q[0] = x;
        q[1] = y;
        q[2] = z;
        q[3] = 0;
        dNormalize3( q );
        if ( axis1 )
        {
            dMultiply1_331( axis1, j->node[0].body->posr.R, q );
            axis1[3] = 0;
        }
        if ( axis2 )
        {
            if ( j->node[1].body )
            {
                dMultiply1_331( axis2, j->node[1].body->posr.R, q );
            }
            else
            {
                axis2[0] = x;
                axis2[1] = y;
                axis2[2] = z;
            }
            axis2[3] = 0;
        }
    }
}
Exemple #6
0
void dJointSetAMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal z )
{
    dxJointAMotor* joint = ( dxJointAMotor* )j;
    dAASSERT( joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2 );
    checktype( joint, AMotor );
    dUASSERT( !( !joint->node[1].body && ( joint->flags & dJOINT_REVERSE ) && rel == 1 ), "no first body, can't set axis rel=1" );
    dUASSERT( !( !joint->node[1].body && !( joint->flags & dJOINT_REVERSE ) && rel == 2 ), "no second body, can't set axis rel=2" );
    if ( anum < 0 ) anum = 0;
    if ( anum > 2 ) anum = 2;

    // adjust rel to match the internal body order
    if ( !joint->node[1].body && rel == 2 ) rel = 1;

    joint->rel[anum] = rel;

    // x,y,z is always in global coordinates regardless of rel, so we may have
    // to convert it to be relative to a body
    dVector3 r;
    r[0] = x;
    r[1] = y;
    r[2] = z;
    r[3] = 0;
    if ( rel > 0 )
    {
        if ( rel == 1 )
        {
            dMultiply1_331( joint->axis[anum], joint->node[0].body->posr.R, r );
        }
        else
        {
            // don't assert; handle the case of attachment to a bodiless geom
            if ( joint->node[1].body )   // jds
            {
                dMultiply1_331( joint->axis[anum], joint->node[1].body->posr.R, r );
            }
            else
            {
                joint->axis[anum][0] = r[0];
                joint->axis[anum][1] = r[1];
                joint->axis[anum][2] = r[2];
                joint->axis[anum][3] = r[3];
            }
        }
    }
    else
    {
        joint->axis[anum][0] = r[0];
        joint->axis[anum][1] = r[1];
        joint->axis[anum][2] = r[2];
    }
    dNormalize3( joint->axis[anum] );
    if ( joint->mode == dAMotorEuler ) joint->setEulerReferenceVectors();
}
Exemple #7
0
void dJointSetFixed ( dJointID j )
{
    dxJointFixed* joint = ( dxJointFixed* ) j;
    dUASSERT ( joint, "bad joint argument" );
    checktype ( joint, Fixed );
    int i;

    // This code is taken from dJointSetSliderAxis(), we should really put the
    // common code in its own function.
    // compute the offset between the bodies
    if ( joint->node[0].body )
    {
        if ( joint->node[1].body )
        {
            dReal ofs[4];
            for ( i = 0; i < 4; i++ )
                ofs[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i];
            dMultiply1_331 ( joint->offset, joint->node[0].body->posr.R, ofs );
        }
        else
        {
            joint->offset[0] = joint->node[0].body->posr.pos[0];
            joint->offset[1] = joint->node[0].body->posr.pos[1];
            joint->offset[2] = joint->node[0].body->posr.pos[2];
        }
    }

    joint->computeInitialRelativeRotation();
}
Exemple #8
0
dReal
dxJointHinge2::measureAngle() const
{
    dVector3 a1, a2;
    dMultiply0_331( a1, node[1].body->posr.R, axis2 );
    dMultiply1_331( a2, node[0].body->posr.R, a1 );
    dReal x = dCalcVectorDot3( v1, a2 );
    dReal y = dCalcVectorDot3( v2, a2 );
    return -dAtan2( y, x );
}
void dJointSetLMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal z )
{
    dxJointLMotor* joint = ( dxJointLMotor* )j;
    //for now we are ignoring rel!
    dAASSERT( joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2 );
    checktype( joint, LMotor );

    if ( anum < 0 ) anum = 0;
    if ( anum > 2 ) anum = 2;

    if ( !joint->node[1].body && rel == 2 ) rel = 1; //ref 1

    joint->rel[anum] = rel;

    dVector3 r;
    r[0] = x;
    r[1] = y;
    r[2] = z;
    r[3] = 0;
    if ( rel > 0 )
    {
        if ( rel == 1 )
        {
            dMultiply1_331( joint->axis[anum], joint->node[0].body->posr.R, r );
        }
        else
        {
            //second body has to exists thanks to ref 1 line
            dMultiply1_331( joint->axis[anum], joint->node[1].body->posr.R, r );
        }
    }
    else
    {
        joint->axis[anum][0] = r[0];
        joint->axis[anum][1] = r[1];
        joint->axis[anum][2] = r[2];
    }

    dNormalize3( joint->axis[anum] );
}
Exemple #10
0
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 {
        // We want to handle angular motors attached to passive geoms
        // Replace missing node.R with identity
        if (node[0].body) {
          dMultiply1_331( reference1, node[0].body->posr.R, axis[2] );
          dMultiply0_331( reference2, node[0].body->posr.R, axis[0] );
        } else if (node[1].body) {
          dMultiply0_331( reference1, node[1].body->posr.R, axis[2] );
          dMultiply1_331( reference2, node[1].body->posr.R, axis[0] );
        }
    }
}
Exemple #11
0
void dJointSetScrewAnchorDelta( dJointID j, dReal x, dReal y, dReal z,
  dReal dx, dReal dy, dReal dz )
{
    dxJointScrew* joint = ( dxJointScrew* )j;
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, Screw );

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

        if ( joint->node[1].body )
        {
            q[0] = x - joint->node[1].body->posr.pos[0];
            q[1] = y - joint->node[1].body->posr.pos[1];
            q[2] = z - joint->node[1].body->posr.pos[2];
            q[3] = 0;
            dMultiply1_331( joint->anchor2, joint->node[1].body->posr.R, q );
        }
        else
        {
            // Move the relative displacement between the passive body and the
            //  anchor in the same direction as the passive body has just moved
            joint->anchor2[0] = x + dx;
            joint->anchor2[1] = y + dy;
            joint->anchor2[2] = z + dz;
        }
    }
    joint->anchor1[3] = 0;
    joint->anchor2[3] = 0;

    joint->computeInitialRelativeRotation();
}
Exemple #12
0
void testSmallMatrixMultiply()
{
  dMatrix3 A,B,C,A2;
  dVector3 a,a2,x;

  HEADER;
  dMakeRandomMatrix (A,3,3,1.0);
  dMakeRandomMatrix (B,3,3,1.0);
  dMakeRandomMatrix (C,3,3,1.0);
  dMakeRandomMatrix (x,3,1,1.0);

  // dMultiply0_331()
  dMultiply0_331 (a,B,x);
  dMultiply0 (a2,B,x,3,3,1);
  printf ("\t%s (1)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
	  "passed");

  // dMultiply1_331()
  dMultiply1_331 (a,B,x);
  dMultiply1 (a2,B,x,3,3,1);
  printf ("\t%s (2)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
	  "passed");

  // dMultiply0_133
  dMultiply0_133 (a,x,B);
  dMultiply0 (a2,x,B,1,3,3);
  printf ("\t%s (3)\n",(dMaxDifference (a,a2,1,3) > tol) ? "FAILED" :
	  "passed");

  // dMultiply0_333()
  dMultiply0_333 (A,B,C);
  dMultiply0 (A2,B,C,3,3,3);
  printf ("\t%s (4)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
	  "passed");

  // dMultiply1_333()
  dMultiply1_333 (A,B,C);
  dMultiply1 (A2,B,C,3,3,3);
  printf ("\t%s (5)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
	  "passed");

  // dMultiply2_333()
  dMultiply2_333 (A,B,C);
  dMultiply2 (A2,B,C,3,3,3);
  printf ("\t%s (6)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
	  "passed");
}
Exemple #13
0
/// Compute center of body1 w.r.t body 2
void
dxJointScrew::computeOffset()
{
    if ( node[1].body )
    {
        dVector3 cgdiff;
        cgdiff[0] = node[0].body->posr.pos[0] - node[1].body->posr.pos[0];
        cgdiff[1] = node[0].body->posr.pos[1] - node[1].body->posr.pos[1];
        cgdiff[2] = node[0].body->posr.pos[2] - node[1].body->posr.pos[2];

        dMultiply1_331 ( offset, node[1].body->posr.R, cgdiff );
    }
    else if ( node[0].body )
    {
        offset[0] = node[0].body->posr.pos[0];
        offset[1] = node[0].body->posr.pos[1];
        offset[2] = node[0].body->posr.pos[2];
    }
}
Exemple #14
0
dReal
dxJointHinge2::measureAngle1() const
{
    // bring axis 2 into first body's reference frame
    dVector3 p, q;
    if (node[1].body)
        dMultiply0_331( p, node[1].body->posr.R, axis2 );
    else
        dCopyVector3(p, axis2);

    if (node[0].body)
        dMultiply1_331( q, node[0].body->posr.R, p );
    else
        dCopyVector3(q, p);

    dReal x = dCalcVectorDot3( v1, q );
    dReal y = dCalcVectorDot3( v2, q );
    return -dAtan2( y, x );
}
Exemple #15
0
void dGeomVectorFromWorld (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result)
{
    dAASSERT (g);
    if ((g->gflags & GEOM_PLACEABLE) == 0) {
        result[0] = px;
        result[1] = py;
        result[2] = pz;
        return;
    }

    g->recomputePosr();

    dVector3 p;
    p[0] = px;
    p[1] = py;
    p[2] = pz;
    p[3] = 0;
    dMultiply1_331 (result,g->final_posr->R,p);
}
dReal
dxJointHinge2::measureAngle2() const
{
    // bring axis 1 into second body's reference frame
    dVector3 p, q;
    if (node[0].body)
        dMultiply0_331( p, node[0].body->posr.R, axis1 );
    else
        dCopyVector3r4(p, axis1);

    if (node[1].body)
        dMultiply1_331( q, node[1].body->posr.R, p );
    else
        dCopyVector3r4(q, p);

    dReal x = dCalcVectorDot3( w1, q );
    dReal y = dCalcVectorDot3( w2, q );
    return -dAtan2( y, x );
}
Exemple #17
0
void dGeomGetPosRelPoint (dGeomID g, dReal px, dReal py, dReal pz, dVector3 result)
{
    dAASSERT (g);
    if ((g->gflags & GEOM_PLACEABLE) == 0) {
        result[0] = px;
        result[1] = py;
        result[2] = pz;
        return;
    }

    g->recomputePosr();

    dVector3 prel;
    prel[0] = px - g->final_posr->pos[0];
    prel[1] = py - g->final_posr->pos[1];
    prel[2] = pz - g->final_posr->pos[2];
    prel[3] = 0;
    dMultiply1_331 (result,g->final_posr->R,prel);
}
/// Compute center of body1 w.r.t body 2
void
dxJointSlider::computeOffset()
{
    if ( node[1].body )
    {
        dVector3 c;
        c[0] = node[0].body->posr.pos[0] - node[1].body->posr.pos[0];
        c[1] = node[0].body->posr.pos[1] - node[1].body->posr.pos[1];
        c[2] = node[0].body->posr.pos[2] - node[1].body->posr.pos[2];

        dMultiply1_331 ( offset, node[1].body->posr.R, c );
    }
    else if ( node[0].body )
    {
        offset[0] = node[0].body->posr.pos[0];
        offset[1] = node[0].body->posr.pos[1];
        offset[2] = node[0].body->posr.pos[2];
    }
}
Exemple #19
0
// Ray-Cylinder collider by Joseph Cooper (2011)
int dCollideRayCylinder( dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip )
{
    dIASSERT( skip >= (int)sizeof( dContactGeom ) );
    dIASSERT( o1->type == dRayClass );
    dIASSERT( o2->type == dCylinderClass );
    dIASSERT( (flags & NUMC_MASK) >= 1 );

    dxRay* ray = (dxRay*)( o1 );
    dxCylinder* cyl = (dxCylinder*)( o2 );

    // Fill in contact information.
    contact->g1 = ray;
    contact->g2 = cyl;
    contact->side1 = -1;
    contact->side2 = -1;

    const dReal half_length = cyl->lz * REAL( 0.5 );


    /* Possible collision cases:
     *  Ray origin between/outside caps
     *  Ray origin within/outside radius
     *  Ray direction left/right/perpendicular
     *  Ray direction parallel/perpendicular/other
     * 
     *  Ray origin cases (ignoring origin on surface)
     *
     *  A          B
     *     /-\-----------\
     *  C (   )    D      )
     *     \_/___________/
     *
     *  Cases A and D can collide with caps or cylinder
     *  Case C can only collide with the caps
     *  Case B can only collide with the cylinder
     *  Case D will produce inverted normals
     *  If the ray is perpendicular, only check the cylinder
     *  If the ray is parallel to cylinder axis,
     *  we can only check caps
     *  If the ray points right,
     *    Case A,C Check left cap
     *    Case  D  Check right cap
     *  If the ray points left
     *    Case A,C Check right cap
     *    Case  D  Check left cap
     *  Case B, check only first possible cylinder collision
     *  Case D, check only second possible cylinder collision
     */
    // Find the ray in the cylinder coordinate frame:
    dVector3 tmp;
    dVector3 pos;  // Ray origin in cylinder frame
    dVector3 dir;  // Ray direction in cylinder frame
    // Translate ray start by inverse cyl
    dSubtractVectors3(tmp,ray->final_posr->pos,cyl->final_posr->pos);
    // Rotate ray start by inverse cyl
    dMultiply1_331(pos,cyl->final_posr->R,tmp);

    // Get the ray's direction
    tmp[0] = ray->final_posr->R[2];
    tmp[1] = ray->final_posr->R[6];
    tmp[2] = ray->final_posr->R[10];
    // Rotate the ray direction by inverse cyl
    dMultiply1_331(dir,cyl->final_posr->R,tmp); 

    // Is the ray origin inside of the (extended) cylinder?
    dReal r2 = cyl->radius*cyl->radius;
    dReal C = pos[0]*pos[0] + pos[1]*pos[1] - r2;

    // Find the different cases
    // Is ray parallel to the cylinder length?
    int parallel = (dir[0]==0 && dir[1]==0);
    // Is ray perpendicular to the cylinder length?
    int perpendicular = (dir[2]==0);
    // Is ray origin within the radius of the caps?
    int inRadius = (C<=0);
    // Is ray origin between the top and bottom caps?
    int inCaps   = (dFabs(pos[2])<=half_length);

    int checkCaps = (!perpendicular && (!inCaps || inRadius));
    int checkCyl  = (!parallel && (!inRadius || inCaps));
    int flipNormals = (inCaps&&inRadius);

    dReal tt=-dInfinity; // Depth to intersection
    dVector3 tmpNorm = {dNaN, dNaN, dNaN}; // ensure we don't leak garbage

    if (checkCaps) {
        // Make it so we only need to check one cap
        int flipDir = 0;
        // Wish c had logical xor...
        if ((dir[2]<0 && flipNormals) || (dir[2]>0 && !flipNormals)) {
            flipDir = 1;
            dir[2]=-dir[2];
            pos[2]=-pos[2];
        }
        // The cap is half the cylinder's length
        // from the cylinder's origin
        // We only checkCaps if dir[2]!=0
        tt = (half_length-pos[2])/dir[2];
        if (tt>=0 && tt<=ray->length) {
            tmp[0] = pos[0] + tt*dir[0];
            tmp[1] = pos[1] + tt*dir[1];
            // Ensure collision point is within cap circle
            if (tmp[0]*tmp[0] + tmp[1]*tmp[1] <= r2) {
                // Successful collision
                tmp[2] = (flipDir)?-half_length:half_length;
                tmpNorm[0]=0;
                tmpNorm[1]=0;
                tmpNorm[2]=(flipDir!=flipNormals)?-1:1;
                checkCyl = 0;  // Short circuit cylinder check
            } else {
                // Ray hits cap plane outside of cap circle
                tt=-dInfinity; // No collision yet
            }
        } else {
            // The cap plane is beyond (or behind) the ray length
            tt=-dInfinity; // No collision yet
        }
        if (flipDir) {
            // Flip back
            dir[2]=-dir[2];
            pos[2]=-pos[2];
        }
    }
    if (checkCyl) {
        // Compute quadratic formula for parametric ray equation
        dReal A =    dir[0]*dir[0] + dir[1]*dir[1];
        dReal B = 2*(pos[0]*dir[0] + pos[1]*dir[1]);
        // Already computed C

        dReal k = B*B - 4*A*C;
        // Check collision with infinite cylinder
        // k<0 means the ray passes outside the cylinder
        // k==0 means ray is tangent to cylinder (or parallel)
        //
        //  Our quadratic formula: tt = (-B +- sqrt(k))/(2*A)   
        // 
        // A must be positive (otherwise we wouldn't be checking
        // cylinder because ray is parallel)
        //    if (k<0) ray doesn't collide with sphere
        //    if (B > sqrt(k)) then both times are negative
        //         -- don't calculate
        //    if (B<-sqrt(k)) then both times are positive (Case A or B)
        //         -- only calculate first, if first isn't valid
        //         -- second can't be without first going through a cap
        //    otherwise (fabs(B)<=sqrt(k)) then C<=0 (ray-origin inside/on cylinder)
        //         -- only calculate second collision
        if (k>=0 && (B<0 || B*B<=k)) {
            k = dSqrt(k); 
            A = dRecip(2*A);
            if (dFabs(B)<=k) {
                tt = (-B + k)*A; // Second solution
                // If ray origin is on surface and pointed out, we
                // can get a tt=0 solution...
            } else {
                tt = (-B - k)*A; // First solution
            }
            if (tt<=ray->length) {
                tmp[2] = pos[2] + tt*dir[2];
                if (dFabs(tmp[2])<=half_length) {
                    // Valid solution
                    tmp[0] = pos[0] + tt*dir[0];
                    tmp[1] = pos[1] + tt*dir[1];
                    tmpNorm[0] = tmp[0]/cyl->radius;
                    tmpNorm[1] = tmp[1]/cyl->radius;
                    tmpNorm[2] = 0;
                    if (flipNormals) {
                        // Ray origin was inside cylinder
                        tmpNorm[0] = -tmpNorm[0];
                        tmpNorm[1] = -tmpNorm[1];
                    }
                } else {
                    // Ray hits cylinder outside of caps
                    tt=-dInfinity;
                }
            } else {
                // Ray doesn't reach the cylinder
                tt=-dInfinity;
            }
        }
    }

    if (tt>0) {
        contact->depth = tt;
        // Transform the point back to world coordinates
        tmpNorm[3]=0;
        tmp[3] = 0;
        dMultiply0_331(contact->normal,cyl->final_posr->R,tmpNorm);
        dMultiply0_331(contact->pos,cyl->final_posr->R,tmp);
        contact->pos[0]+=cyl->final_posr->pos[0];
        contact->pos[1]+=cyl->final_posr->pos[1];
        contact->pos[2]+=cyl->final_posr->pos[2];

        return 1;
    }
    // No contact with anything.
    return 0;
}
Exemple #20
0
int dCollideRayBox (dxGeom *o1, dxGeom *o2, int flags,
                    dContactGeom *contact, int skip)
{
    dIASSERT (skip >= (int)sizeof(dContactGeom));
    dIASSERT (o1->type == dRayClass);
    dIASSERT (o2->type == dBoxClass);
    dIASSERT ((flags & NUMC_MASK) >= 1);

    dxRay *ray = (dxRay*) o1;
    dxBox *box = (dxBox*) o2;

    contact->g1 = ray;
    contact->g2 = box;
    contact->side1 = -1;
    contact->side2 = -1;

    int i;

    // compute the start and delta of the ray relative to the box.
    // we will do all subsequent computations in this box-relative coordinate
    // system. we have to do a translation and rotation for each point.
    dVector3 tmp,s,v;
    tmp[0] = ray->final_posr->pos[0] - box->final_posr->pos[0];
    tmp[1] = ray->final_posr->pos[1] - box->final_posr->pos[1];
    tmp[2] = ray->final_posr->pos[2] - box->final_posr->pos[2];
    dMultiply1_331 (s,box->final_posr->R,tmp);
    tmp[0] = ray->final_posr->R[0*4+2];
    tmp[1] = ray->final_posr->R[1*4+2];
    tmp[2] = ray->final_posr->R[2*4+2];
    dMultiply1_331 (v,box->final_posr->R,tmp);

    // mirror the line so that v has all components >= 0
    dVector3 sign;
    for (i=0; i<3; i++) {
        if (v[i] < 0) {
            s[i] = -s[i];
            v[i] = -v[i];
            sign[i] = 1;
        }
        else sign[i] = -1;
    }

    // compute the half-sides of the box
    dReal h[3];
    h[0] = REAL(0.5) * box->side[0];
    h[1] = REAL(0.5) * box->side[1];
    h[2] = REAL(0.5) * box->side[2];

    // do a few early exit tests
    if ((s[0] < -h[0] && v[0] <= 0) || s[0] >  h[0] ||
        (s[1] < -h[1] && v[1] <= 0) || s[1] >  h[1] ||
        (s[2] < -h[2] && v[2] <= 0) || s[2] >  h[2] ||
        (v[0] == 0 && v[1] == 0 && v[2] == 0)) {
            return 0;
    }

    // compute the t=[lo..hi] range for where s+v*t intersects the box
    dReal lo = -dInfinity;
    dReal hi = dInfinity;
    int nlo = 0, nhi = 0;
    for (i=0; i<3; i++) {
        if (v[i] != 0) {
            dReal k = (-h[i] - s[i])/v[i];
            if (k > lo) {
                lo = k;
                nlo = i;
            }
            k = (h[i] - s[i])/v[i];
            if (k < hi) {
                hi = k;
                nhi = i;
            }
        }
    }

    // check if the ray intersects
    if (lo > hi) return 0;
    dReal alpha;
    int n;
    if (lo >= 0) {
        alpha = lo;
        n = nlo;
    }
    else {
        alpha = hi;
        n = nhi;
    }
    if (alpha < 0 || alpha > ray->length) return 0;
    contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
    contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
    contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
    contact->normal[0] = box->final_posr->R[0*4+n] * sign[n];
    contact->normal[1] = box->final_posr->R[1*4+n] * sign[n];
    contact->normal[2] = box->final_posr->R[2*4+n] * sign[n];
    contact->depth = alpha;
    return 1;
}
Exemple #21
0
dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
  dUASSERT (g && g->type == dBoxClass,"argument not a box");
  g->recomputePosr();
  dxBox *b = (dxBox*) g;

  // Set p = (x,y,z) relative to box center
  //
  // This will be (0,0,0) if the point is at (side[0]/2,side[1]/2,side[2]/2)

  dVector3 p,q;

  p[0] = x - b->final_posr->pos[0];
  p[1] = y - b->final_posr->pos[1];
  p[2] = z - b->final_posr->pos[2];

  // Rotate p into box's coordinate frame, so we can
  // treat the OBB as an AABB

  dMultiply1_331 (q,b->final_posr->R,p);

  // Record distance from point to each successive box side, and see
  // if the point is inside all six sides

  dReal dist[6];
  int   i;

  bool inside = true;

  for (i=0; i < 3; i++) {
    dReal side = b->side[i] * REAL(0.5);

    dist[i  ] = side - q[i];
    dist[i+3] = side + q[i];

    if ((dist[i] < 0) || (dist[i+3] < 0)) {
      inside = false;
    }
  }

  // If point is inside the box, the depth is the smallest positive distance
  // to any side

  if (inside) {
    dReal smallest_dist = (dReal) (unsigned) -1;

    for (i=0; i < 6; i++) {
      if (dist[i] < smallest_dist) smallest_dist = dist[i];
    }

    return smallest_dist;
  }

  // Otherwise, if point is outside the box, the depth is the largest
  // distance to any side.  This is an approximation to the 'proper'
  // solution (the proper solution may be larger in some cases).

  dReal largest_dist = 0;

  for (i=0; i < 6; i++) {
    if (dist[i] > largest_dist) largest_dist = dist[i];
  }

  return -largest_dist;
}
Exemple #22
0
void
dxJointDBall::getInfo2( dxJoint::Info2 *info )
{
    info->erp = erp;
    info->cfm[0] = cfm;

    dVector3 globalA1, globalA2;
    dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1);
    if (node[1].body)
        dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalA2);
    else
        dCopyVector3(globalA2, anchor2);

    dVector3 q;
    dSubtractVectors3(q, globalA1, globalA2);

#ifdef dSINGLE
    const dReal MIN_LENGTH = REAL(1e-7);
#else
    const dReal MIN_LENGTH = REAL(1e-12);
#endif

    if (dCalcVectorLength3(q) < MIN_LENGTH) {
        // too small, let's choose an arbitrary direction
        // heuristic: difference in velocities at anchors
        dVector3 v1, v2;
        dBodyGetPointVel(node[0].body, globalA1[0], globalA1[1], globalA1[2], v1);
        if (node[1].body)
            dBodyGetPointVel(node[1].body, globalA2[0], globalA2[1], globalA2[2], v2);
        else
            dSetZero(v2, 3);
        dSubtractVectors3(q, v1, v2);

        if (dCalcVectorLength3(q) < MIN_LENGTH) {
            // this direction is as good as any
            q[0] = 1;
            q[1] = 0;
            q[2] = 0;
        }
    }
    dNormalize3(q);

    info->J1l[0] = q[0];
    info->J1l[1] = q[1];
    info->J1l[2] = q[2];

    dVector3 relA1;
    dBodyVectorToWorld(node[0].body,
                       anchor1[0], anchor1[1], anchor1[2],
                       relA1);

    dMatrix3 a1m;
    dSetZero(a1m, 12);
    dSetCrossMatrixMinus(a1m, relA1, 4);

    dMultiply1_331(info->J1a, a1m, q);

    if (node[1].body) {
        info->J2l[0] = -q[0];
        info->J2l[1] = -q[1];
        info->J2l[2] = -q[2];

        dVector3 relA2;
        dBodyVectorToWorld(node[1].body,
                           anchor2[0], anchor2[1], anchor2[2],
                           relA2);
        dMatrix3 a2m;
        dSetZero(a2m, 12);
        dSetCrossMatrixPlus(a2m, relA2, 4);
        dMultiply1_331(info->J2a, a2m, q);
    }
    
    const dReal k = info->fps * info->erp;
    info->c[0] = k * (targetDistance - dCalcPointsDistance3(globalA1, globalA2));

}
Exemple #23
0
int dBoxBox (const dVector3 p1, const dMatrix3 R1,
	     const dVector3 side1, const dVector3 p2,
	     const dMatrix3 R2, const dVector3 side2,
	     dVector3 normal, dReal *depth, int *return_code,
	     int flags, dContactGeom *contact, int skip)
{
  const dReal fudge_factor = REAL(1.05);
  dVector3 p,pp,normalC={0,0,0};
  const dReal *normalR = 0;
  dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,expr1_val;
  int i,j,invert_normal,code;

  // get vector from centers of box 1 to box 2, relative to box 1
  p[0] = p2[0] - p1[0];
  p[1] = p2[1] - p1[1];
  p[2] = p2[2] - p1[2];
  dMultiply1_331 (pp,R1,p);		// get pp = p relative to body 1

  // get side lengths / 2
  A[0] = side1[0]*REAL(0.5);
  A[1] = side1[1]*REAL(0.5);
  A[2] = side1[2]*REAL(0.5);
  B[0] = side2[0]*REAL(0.5);
  B[1] = side2[1]*REAL(0.5);
  B[2] = side2[2]*REAL(0.5);

  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
  R11 = dCalcVectorDot3_44(R1+0,R2+0); R12 = dCalcVectorDot3_44(R1+0,R2+1); R13 = dCalcVectorDot3_44(R1+0,R2+2);
  R21 = dCalcVectorDot3_44(R1+1,R2+0); R22 = dCalcVectorDot3_44(R1+1,R2+1); R23 = dCalcVectorDot3_44(R1+1,R2+2);
  R31 = dCalcVectorDot3_44(R1+2,R2+0); R32 = dCalcVectorDot3_44(R1+2,R2+1); R33 = dCalcVectorDot3_44(R1+2,R2+2);

  Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
  Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
  Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);

  // for all 15 possible separating axes:
  //   * see if the axis separates the boxes. if so, return 0.
  //   * find the depth of the penetration along the separating axis (s2)
  //   * if this is the largest depth so far, record it.
  // the normal vector will be set to the separating axis with the smallest
  // depth. note: normalR is set to point to a column of R1 or R2 if that is
  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
  // set to a vector relative to body 1. invert_normal is 1 if the sign of
  // the normal should be flipped.

  do {
#define TST(expr1,expr2,norm,cc) \
    expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
    s2 = dFabs(expr1_val) - (expr2); \
    if (s2 > 0) return 0; \
    if (s2 > s) { \
      s = s2; \
      normalR = norm; \
      invert_normal = ((expr1_val) < 0); \
      code = (cc); \
	  if (flags & CONTACTS_UNIMPORTANT) break; \
	}

    s = -dInfinity;
    invert_normal = 0;
    code = 0;

    // separating axis = u1,u2,u3
    TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
    TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
    TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);

    // separating axis = v1,v2,v3
    TST (dCalcVectorDot3_41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
    TST (dCalcVectorDot3_41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
    TST (dCalcVectorDot3_41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);

    // note: cross product axes need to be scaled when s is computed.
    // normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
    expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
    s2 = dFabs(expr1_val) - (expr2); \
    if (s2 > 0) return 0; \
    l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
    if (l > 0) { \
      s2 /= l; \
      if (s2*fudge_factor > s) { \
        s = s2; \
        normalR = 0; \
        normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
        invert_normal = ((expr1_val) < 0); \
        code = (cc); \
        if (flags & CONTACTS_UNIMPORTANT) break; \
	  } \
	}

    // We only need to check 3 edges per box 
    // since parallel edges are equivalent.

    // separating axis = u1 x (v1,v2,v3)
    TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
    TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
    TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);

    // separating axis = u2 x (v1,v2,v3)
    TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
    TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
    TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);

    // separating axis = u3 x (v1,v2,v3)
    TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
    TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
    TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
#undef TST
  } while (0);

  if (!code) return 0;

  // if we get to this point, the boxes interpenetrate. compute the normal
  // in global coordinates.
  if (normalR) {
    normal[0] = normalR[0];
    normal[1] = normalR[4];
    normal[2] = normalR[8];
  }
  else {
    dMultiply0_331 (normal,R1,normalC);
  }
  if (invert_normal) {
    normal[0] = -normal[0];
    normal[1] = -normal[1];
    normal[2] = -normal[2];
  }
  *depth = -s;

  // compute contact point(s)

  if (code > 6) {
    // An edge from box 1 touches an edge from box 2.
    // find a point pa on the intersecting edge of box 1
    dVector3 pa;
    dReal sign;
    // Copy p1 into pa
    for (i=0; i<3; i++) pa[i] = p1[i]; // why no memcpy?
    // Get world position of p2 into pa
    for (j=0; j<3; j++) {
      sign = (dCalcVectorDot3_14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);
      for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
    }

    // find a point pb on the intersecting edge of box 2
    dVector3 pb;
    // Copy p2 into pb
    for (i=0; i<3; i++) pb[i] = p2[i]; // why no memcpy?
    // Get world position of p2 into pb
    for (j=0; j<3; j++) {
      sign = (dCalcVectorDot3_14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);
      for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
    }
    
    dReal alpha,beta;
    dVector3 ua,ub;
    // Get direction of first edge
    for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
    // Get direction of second edge
    for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
    // Get closest points between edges (one at each)
    dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);    
    for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
    for (i=0; i<3; i++) pb[i] += ub[i]*beta;
    // Set the contact point as halfway between the 2 closest points
    for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
    contact[0].depth = *depth;
    *return_code = code;
    return 1;
  }

  // okay, we have a face-something intersection (because the separating
  // axis is perpendicular to a face). define face 'a' to be the reference
  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
  // the incident face (the closest face of the other box).
  // Note: Unmodified parameter values are being used here
  const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
  if (code <= 3) { // One of the faces of box 1 is the reference face
    Ra = R1; // Rotation of 'a'
    Rb = R2; // Rotation of 'b'
    pa = p1; // Center (location) of 'a'
    pb = p2; // Center (location) of 'b'
    Sa = A;  // Side Lenght of 'a'
    Sb = B;  // Side Lenght of 'b'
  }
  else { // One of the faces of box 2 is the reference face
    Ra = R2; // Rotation of 'a'
    Rb = R1; // Rotation of 'b'
    pa = p2; // Center (location) of 'a'
    pb = p1; // Center (location) of 'b'
    Sa = B;  // Side Lenght of 'a'
    Sb = A;  // Side Lenght of 'b'
  }

  // nr = normal vector of reference face dotted with axes of incident box.
  // anr = absolute values of nr.
  /*
	The normal is flipped if necessary so it always points outward from box 'a',
	box 'b' is thus always the incident box
  */
  dVector3 normal2,nr,anr;
  if (code <= 3) {
    normal2[0] = normal[0];
    normal2[1] = normal[1];
    normal2[2] = normal[2];
  }
  else {
    normal2[0] = -normal[0];
    normal2[1] = -normal[1];
    normal2[2] = -normal[2];
  }
  // Rotate normal2 in incident box opposite direction
  dMultiply1_331 (nr,Rb,normal2);
  anr[0] = dFabs (nr[0]);
  anr[1] = dFabs (nr[1]);
  anr[2] = dFabs (nr[2]);

  // find the largest compontent of anr: this corresponds to the normal
  // for the incident face. the other axis numbers of the incident face
  // are stored in a1,a2.
  int lanr,a1,a2;
  if (anr[1] > anr[0]) {
    if (anr[1] > anr[2]) {
      a1 = 0;
      lanr = 1;
      a2 = 2;
    }
    else {
      a1 = 0;
      a2 = 1;
      lanr = 2;
    }
  }
  else {
    if (anr[0] > anr[2]) {
      lanr = 0;
      a1 = 1;
      a2 = 2;
    }
    else {
      a1 = 0;
      a2 = 1;
      lanr = 2;
    }
  }

  // compute center point of incident face, in reference-face coordinates
  dVector3 center;
  if (nr[lanr] < 0) {
    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
  }
  else {
    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
  }

  // find the normal and non-normal axis numbers of the reference box
  int codeN,code1,code2;
  if (code <= 3) codeN = code-1; else codeN = code-4;
  if (codeN==0) {
    code1 = 1;
    code2 = 2;
  }
  else if (codeN==1) {
    code1 = 0;
    code2 = 2;
  }
  else {
    code1 = 0;
    code2 = 1;
  }

  // find the four corners of the incident face, in reference-face coordinates
  dReal quad[8];	// 2D coordinate of incident face (x,y pairs)
  dReal c1,c2,m11,m12,m21,m22;
  c1 = dCalcVectorDot3_14 (center,Ra+code1);
  c2 = dCalcVectorDot3_14 (center,Ra+code2);
  // optimize this? - we have already computed this data above, but it is not
  // stored in an easy-to-index format. for now it's quicker just to recompute
  // the four dot products.
  m11 = dCalcVectorDot3_44 (Ra+code1,Rb+a1);
  m12 = dCalcVectorDot3_44 (Ra+code1,Rb+a2);
  m21 = dCalcVectorDot3_44 (Ra+code2,Rb+a1);
  m22 = dCalcVectorDot3_44 (Ra+code2,Rb+a2);
  {
    dReal k1 = m11*Sb[a1];
    dReal k2 = m21*Sb[a1];
    dReal k3 = m12*Sb[a2];
    dReal k4 = m22*Sb[a2];
    quad[0] = c1 - k1 - k3;
    quad[1] = c2 - k2 - k4;
    quad[2] = c1 - k1 + k3;
    quad[3] = c2 - k2 + k4;
    quad[4] = c1 + k1 + k3;
    quad[5] = c2 + k2 + k4;
    quad[6] = c1 + k1 - k3;
    quad[7] = c2 + k2 - k4;
  }

  // find the size of the reference face
  dReal rect[2];
  rect[0] = Sa[code1];
  rect[1] = Sa[code2];

  // intersect the incident and reference faces
  dReal ret[16];
  int n = intersectRectQuad (rect,quad,ret);
  if (n < 1) return 0;		// this should never happen

  // convert the intersection points into reference-face coordinates,
  // and compute the contact position and depth for each point. only keep
  // those points that have a positive (penetrating) depth. delete points in
  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
  dReal point[3*8];		// penetrating contact points
  dReal dep[8];			// depths for those points
  dReal det1 = dRecip(m11*m22 - m12*m21);
  m11 *= det1;
  m12 *= det1;
  m21 *= det1;
  m22 *= det1;
  int cnum = 0;			// number of penetrating contact points found
  for (j=0; j < n; j++) {
    dReal k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
    dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
    for (i=0; i<3; i++) point[cnum*3+i] =
			  center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
    dep[cnum] = Sa[codeN] - dCalcVectorDot3(normal2,point+cnum*3);
    if (dep[cnum] >= 0) {
      ret[cnum*2] = ret[j*2];
      ret[cnum*2+1] = ret[j*2+1];
      cnum++;
	  if ((cnum | CONTACTS_UNIMPORTANT) == (flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
		  break;
	  }
    }
  }
  if (cnum < 1) { 
	  return 0;	// this should not happen, yet does at times (demo_plane2d single precision).
  }

  // we can't generate more contacts than we actually have
  int maxc = flags & NUMC_MASK;
  if (maxc > cnum) maxc = cnum;
  if (maxc < 1) maxc = 1;	// Even though max count must not be zero this check is kept for backward compatibility as this is a public function

  if (cnum <= maxc) {
    // we have less contacts than we need, so we use them all
    for (j=0; j < cnum; j++) {
      dContactGeom *con = CONTACT(contact,skip*j);
      for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
      con->depth = dep[j];
    }
  }
  else {
    dIASSERT(!(flags & CONTACTS_UNIMPORTANT)); // cnum should be generated not greater than maxc so that "then" clause is executed
    // we have more contacts than are wanted, some of them must be culled.
    // find the deepest point, it is always the first contact.
    int i1 = 0;
    dReal maxdepth = dep[0];
    for (i=1; i<cnum; i++) {
      if (dep[i] > maxdepth) {
	maxdepth = dep[i];
	i1 = i;
      }
    }

    int iret[8];
    cullPoints (cnum,ret,maxc,i1,iret);

    for (j=0; j < maxc; j++) {
      dContactGeom *con = CONTACT(contact,skip*j);
      for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
      con->depth = dep[iret[j]];
    }
    cnum = maxc;
  }

  *return_code = code;
  return cnum;
}