Exemple #1
0
static int edgeIntersectsRect (dVector3 v1, dVector3 v2,
			       dVector3 p1, dVector3 p2, dVector3 p3)
{
  int k;
  dVector3 u1,u2,n,tmp;
  for (k=0; k<3; k++) u1[k] = p3[k]-p1[k];
  for (k=0; k<3; k++) u2[k] = p2[k]-p1[k];
  dReal d1 = dSqrt(dCalcVectorDot3(u1,u1));
  dReal d2 = dSqrt(dCalcVectorDot3(u2,u2));
  dNormalize3 (u1);
  dNormalize3 (u2);
  if (dFabs(dCalcVectorDot3(u1,u2)) > 1e-6) dDebug (0,"bad u1/u2");
  dCalcVectorCross3(n,u1,u2);
  for (k=0; k<3; k++) tmp[k] = v2[k]-v1[k];
  dReal d = -dCalcVectorDot3(n,p1);
  if (dFabs(dCalcVectorDot3(n,p1)+d) > 1e-8) dDebug (0,"bad n wrt p1");
  if (dFabs(dCalcVectorDot3(n,p2)+d) > 1e-8) dDebug (0,"bad n wrt p2");
  if (dFabs(dCalcVectorDot3(n,p3)+d) > 1e-8) dDebug (0,"bad n wrt p3");
  dReal alpha = -(d+dCalcVectorDot3(n,v1))/dCalcVectorDot3(n,tmp);
  for (k=0; k<3; k++) tmp[k] = v1[k]+alpha*(v2[k]-v1[k]);
  if (dFabs(dCalcVectorDot3(n,tmp)+d) > 1e-6) dDebug (0,"bad tmp");
  if (alpha < 0) return 0;
  if (alpha > 1) return 0;
  for (k=0; k<3; k++) tmp[k] -= p1[k];
  dReal a1 = dCalcVectorDot3(u1,tmp);
  dReal a2 = dCalcVectorDot3(u2,tmp);
  if (a1<0 || a2<0 || a1>d1 || a2>d2) return 0;
  return 1;
}
Exemple #2
0
void makeRandomRotation (dMatrix3 R)
{
  dReal *u1 = R, *u2=R+4, *u3=R+8;
  dMakeRandomVector (u1,3,1.0);
  dNormalize3 (u1);
  dMakeRandomVector (u2,3,1.0);
  dReal d = dCalcVectorDot3(u1,u2);
  u2[0] -= d*u1[0];
  u2[1] -= d*u1[1];
  u2[2] -= d*u1[2];
  dNormalize3(u2);
  dCalcVectorCross3(u3,u1,u2);
}
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
//plane plane
bool FindIntersectionPlanePlane(const dReal Plane0[4], const dReal Plane1[4],
	dVector3 LinePos,dVector3 LineDir)
{
    // If Cross(N0,N1) is zero, then either planes are parallel and separated
    // or the same plane.  In both cases, 'false' is returned.  Otherwise,
    // the intersection line is
    //
    //   L(t) = t*Cross(N0,N1) + c0*N0 + c1*N1
    //
    // for some coefficients c0 and c1 and for t any real number (the line
    // parameter).  Taking dot products with the normals,
    //
    //   d0 = Dot(N0,L) = c0*Dot(N0,N0) + c1*Dot(N0,N1)
    //   d1 = Dot(N1,L) = c0*Dot(N0,N1) + c1*Dot(N1,N1)
    //
    // which are two equations in two unknowns.  The solution is
    //
    //   c0 = (Dot(N1,N1)*d0 - Dot(N0,N1)*d1)/det
    //   c1 = (Dot(N0,N0)*d1 - Dot(N0,N1)*d0)/det
    //
    // where det = Dot(N0,N0)*Dot(N1,N1)-Dot(N0,N1)^2.
/*
    Real fN00 = rkPlane0.Normal().SquaredLength();
    Real fN01 = rkPlane0.Normal().Dot(rkPlane1.Normal());
    Real fN11 = rkPlane1.Normal().SquaredLength();
    Real fDet = fN00*fN11 - fN01*fN01;

    if ( Math::FAbs(fDet) < gs_fEpsilon )
        return false;

    Real fInvDet = 1.0f/fDet;
    Real fC0 = (fN11*rkPlane0.Constant() - fN01*rkPlane1.Constant())*fInvDet;
    Real fC1 = (fN00*rkPlane1.Constant() - fN01*rkPlane0.Constant())*fInvDet;

    rkLine.Direction() = rkPlane0.Normal().Cross(rkPlane1.Normal());
    rkLine.Origin() = fC0*rkPlane0.Normal() + fC1*rkPlane1.Normal();
    return true;
*/
	dReal fN00 = dLENGTHSQUARED(Plane0);
    dReal fN01 = dDOT(Plane0,Plane1);
    dReal fN11 = dLENGTHSQUARED(Plane1);
    dReal fDet = fN00*fN11 - fN01*fN01;

    if ( fabs(fDet) < fEPSILON)
        return false;

    dReal fInvDet = 1.0f/fDet;
    dReal fC0 = (fN11*Plane0[3] - fN01*Plane1[3])*fInvDet;
    dReal fC1 = (fN00*Plane1[3] - fN01*Plane0[3])*fInvDet;

    dCROSS(LineDir,=,Plane0,Plane1);
	dNormalize3(LineDir);

	dVector3 Temp0,Temp1;
	dOPC(Temp0,*,Plane0,fC0);
	dOPC(Temp1,*,Plane1,fC1);
	dOP(LinePos,+,Temp0,Temp1);

    return true;
}
Exemple #5
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 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 #7
0
void fixFrictionVector( dBodyID b1, dBodyID b2, dContact& contact )
{
	dBodyGetPointVel(b1, contact.geom.pos[0], contact.geom.pos[1], contact.geom.pos[2], contact.fdir1);
	dVector3 fdir1_b2;
	if (b2)
	{
		dBodyGetPointVel(b2, contact.geom.pos[0], contact.geom.pos[1],	contact.geom.pos[2], fdir1_b2);
		contact.fdir1[0] -= fdir1_b2[0];
		contact.fdir1[1] -= fdir1_b2[1];
		contact.fdir1[2] -= fdir1_b2[2];
	}
	// at this point, contact[i].fdir1 is the relative tangent velocity of the two bodies.
	dCROSS(contact.fdir1, =, contact.fdir1, contact.geom.normal);
	// now, contact[i].fdir1 is perpendicular to both the normal and
	// the relative tangent velocity.
	double length = sqrt(contact.fdir1[0] * contact.fdir1[0]
	       			   + contact.fdir1[1] * contact.fdir1[1]
				       + contact.fdir1[2] * contact.fdir1[2]);
	if (length > 1e-12)
	{
	       // we only use our calculated direction if it has enough precision
	       contact.fdir1[0] /= length;
	       contact.fdir1[1] /= length;
	       contact.fdir1[2] /= length;
	       dNormalize3(contact.fdir1);
	       contact.surface.mode |= dContactFDir1;
	}

}
Exemple #8
0
void dJointSetGearboxAxis2( dJointID j, dReal x, dReal y, dReal z )
{
    dxJointGearbox* joint = static_cast<dxJointGearbox*>(j);
    dUASSERT( joint, "bad joint argument" );

    dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axis2);
    dNormalize3(joint->axis2);
}
Exemple #9
0
int test_sphere_point_depth()
{
  int j;
  dVector3 p,q;
  dMatrix3 R;
  dReal r,d;

  dSimpleSpace space(0);
  dGeomID sphere = dCreateSphere (0,1);
  dSpaceAdd (space,sphere);

  // ********** make a random sphere of radius r at position p

  r = dRandReal()+0.1;
  dGeomSphereSetRadius (sphere,r);
  dMakeRandomVector (p,3,1.0);
  dGeomSetPosition (sphere,p[0],p[1],p[2]);
  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
		      dRandReal()*2-1,dRandReal()*10-5);
  dGeomSetRotation (sphere,R);

  // ********** test center point has depth r

  if (dFabs(dGeomSpherePointDepth (sphere,p[0],p[1],p[2]) - r) > tol) FAILED();

  // ********** test point on surface has depth 0

  for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
  dNormalize3 (q);
  for (j=0; j<3; j++) q[j] = q[j]*r + p[j];
  if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])) > tol) FAILED();

  // ********** test point at random depth

  d = (dRandReal()*2-1) * r;
  for (j=0; j<3; j++) q[j] = dRandReal()-0.5;
  dNormalize3 (q);
  for (j=0; j<3; j++) q[j] = q[j]*(r-d) + p[j];
  if (dFabs(dGeomSpherePointDepth (sphere,q[0],q[1],q[2])-d) > tol) FAILED();

  PASSED();
}
Exemple #10
0
// 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];
            }
        }
    }
}
void
dxJointHinge2::getInfo2( dReal worldFPS, dReal worldERP, const Info2Descr *info )
{
    // get information we need to set the hinge row
    dReal s, c;
    dVector3 q;
    const dxJointHinge2 *joint = this;

    dVector3 ax1, ax2;
    joint->getAxisInfo( ax1, ax2, q, s, c );
    dNormalize3( q );   // @@@ quicker: divide q by s ?

    // set the three ball-and-socket rows (aligned to the suspension axis ax1)
    setBall2( this, worldFPS, worldERP, info, anchor1, anchor2, ax1, susp_erp );

    // set the hinge row
    int s3 = 3 * info->rowskip;
    info->J1a[s3+0] = q[0];
    info->J1a[s3+1] = q[1];
    info->J1a[s3+2] = q[2];
    if ( joint->node[1].body )
    {
        info->J2a[s3+0] = -q[0];
        info->J2a[s3+1] = -q[1];
        info->J2a[s3+2] = -q[2];
    }

    // compute the right hand side for the constrained rotational DOF.
    // axis 1 and axis 2 are separated by an angle `theta'. the desired
    // separation angle is theta0. sin(theta0) and cos(theta0) are recorded
    // in the joint structure. the correcting angular velocity is:
    //   |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize
    //                      = (erp*fps) * (theta0-theta)
    // (theta0-theta) can be computed using the following small-angle-difference
    // approximation:
    //   theta0-theta ~= tan(theta0-theta)
    //                 = sin(theta0-theta)/cos(theta0-theta)
    //                 = (c*s0 - s*c0) / (c*c0 + s*s0)
    //                 = c*s0 - s*c0         assuming c*c0 + s*s0 ~= 1
    // where c = cos(theta), s = sin(theta)
    //       c0 = cos(theta0), s0 = sin(theta0)

    dReal k = worldFPS * worldERP;
    info->c[3] = k * ( c0 * s - joint->s0 * c );

    // if the axis1 hinge is powered, or has joint limits, add in more stuff
    int row = 4 + limot1.addLimot( this, worldFPS, info, 4, ax1, 1 );

    // if the axis2 hinge is powered, add in more stuff
    limot2.addLimot( this, worldFPS, info, row, ax2, 1 );

    // set parameter for the suspension
    info->cfm[0] = susp_cfm;
}
Exemple #12
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 #13
0
void dJointPlanarSetPlaneNormal(dJointID joint, dVector3 planeNormal) {
    dUASSERT( joint, "bad joint argument" );
    checktype( joint, Plane2D );
    dxPlanarJoint* planarJoint = (dxPlanarJoint*) joint;

    dUASSERT(dLENGTHSQUARED(planeNormal) > 0.000001, "plane normal cannot have zero length");

    dCopyVector3(planarJoint->planeNormal, planeNormal);
    dNormalize3(planarJoint->planeNormal);

    planarJoint->updatePlane();
}
Exemple #14
0
void dJointSetTransmissionAxis2( dJointID j, dReal x, dReal y, dReal z )
{    
    dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
    dUASSERT( joint, "bad joint argument" );
    dUASSERT(joint->mode = dTransmissionIntersectingAxes,
             "can't set individual axes in current mode" );

    if (joint->node[1].body) {
        dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axes[1]);
        dNormalize3(joint->axes[1]);
    }
    
    joint->update = 1;
}
Exemple #15
0
void testPlaneSpace()
{
  HEADER;
  dVector3 n,p,q;
  int bad = 0;
  for (int i=0; i<1000; i++) {
    dMakeRandomVector (n,3,1.0);
    dNormalize3 (n);
    dPlaneSpace (n,p,q);
    if (fabs(dCalcVectorDot3(n,p)) > tol) bad = 1;
    if (fabs(dCalcVectorDot3(n,q)) > tol) bad = 1;
    if (fabs(dCalcVectorDot3(p,q)) > tol) bad = 1;
    if (fabs(dCalcVectorDot3(p,p)-1) > tol) bad = 1;
    if (fabs(dCalcVectorDot3(q,q)-1) > tol) bad = 1;
  }
  printf ("\t%s\n", bad ? "FAILED" : "passed");
}
// check for separation between box edge and cylinder circle edge
int _cldTestEdgeCircleAxis( sCylinderBoxData& cData,
							const dVector3 &vCenterPoint, 
							const dVector3 &vVx0, const dVector3 &vVx1, 
							int iAxis ) 
{
	// calculate direction of edge
	dVector3 vDirEdge;
	dVector3Subtract(vVx1,vVx0,vDirEdge);
	dNormalize3(vDirEdge);
	// starting point of edge 
	dVector3 vEStart;
	dVector3Copy(vVx0,vEStart);;

	// calculate angle cosine between cylinder axis and edge
	dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis);

	// if edge is perpendicular to cylinder axis
	if(dFabs(fdot2) < 1e-5f) 
	{
		// this can't be separating axis, because edge is parallel to circle plane
		return 1;
	}

	// find point of intersection between edge line and circle plane
	dVector3 vTemp1;
	dVector3Subtract(vCenterPoint,vEStart,vTemp1);
	dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis);
	dVector3 vpnt;
	vpnt[0]= vEStart[0] + vDirEdge[0] * (fdot1/fdot2);
	vpnt[1]= vEStart[1] + vDirEdge[1] * (fdot1/fdot2);
	vpnt[2]= vEStart[2] + vDirEdge[2] * (fdot1/fdot2);

	// find tangent vector on circle with same center (vCenterPoint) that
	// touches point of intersection (vpnt)
	dVector3 vTangent;
	dVector3Subtract(vCenterPoint,vpnt,vTemp1);
	dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent);
	
	// find vector orthogonal both to tangent and edge direction
	dVector3 vAxis;
	dVector3Cross(vTangent,vDirEdge,vAxis);

	// use that vector as separating axis
	return _cldTestAxis( cData, vAxis, iAxis );
}
Exemple #17
0
int test_plane_point_depth()
{
  int j;
  dVector3 n,p,q,a,b;	// n = plane normal
  dReal d;

  dSimpleSpace space(0);
  dGeomID plane = dCreatePlane (0,0,0,1,0);
  dSpaceAdd (space,plane);

  // ********** make a random plane

  for (j=0; j<3; j++) n[j] = dRandReal() - 0.5;
  dNormalize3 (n);
  d = dRandReal() - 0.5;
  dGeomPlaneSetParams (plane,n[0],n[1],n[2],d);
  dPlaneSpace (n,p,q);

  // ********** test point on plane has depth 0

  a[0] = dRandReal() - 0.5;
  a[1] = dRandReal() - 0.5;
  a[2] = 0;
  for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
  if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2])) >= tol) FAILED();

  // ********** test arbitrary depth point

  a[0] = dRandReal() - 0.5;
  a[1] = dRandReal() - 0.5;
  a[2] = dRandReal() - 0.5;
  for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
  if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) + a[2]) >= tol)
    FAILED();

  // ********** test depth-1 point

  a[0] = dRandReal() - 0.5;
  a[1] = dRandReal() - 0.5;
  a[2] = -1;
  for (j=0; j<3; j++) b[j] = a[0]*p[j] + a[1]*q[j] + (a[2]+d)*n[j];
  if (dFabs(dGeomPlanePointDepth (plane,b[0],b[1],b[2]) - 1) >= tol) FAILED();

  PASSED();
}
// intersection test between edge and circle
bool _cldTestCircleToEdgeAxis(sData& cData,
                              const dVector3 &v0, const dVector3 &v1, const dVector3 &v2,
                              const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1,
                              const dVector3 &vVx0, const dVector3 &vVx1, int iAxis)
{
    // calculate direction of edge
    dVector3 vkl;
    dVector3Subtract( vVx1 , vVx0 , vkl);
    dNormalize3(vkl);
    // starting point of edge
    dVector3 vol;
    dVector3Copy(vVx0,vol);

    // calculate angle cosine between cylinder axis and edge
    dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1);

    // if edge is perpendicular to cylinder axis
    if(dFabs(fdot2)<REAL(1e-5))
    {
        // this can't be separating axis, because edge is parallel to circle plane
        return true;
    }

    // find point of intersection between edge line and circle plane
    dVector3 vTemp;
    dVector3Subtract(vCenterPoint,vol,vTemp);
    dReal fdot1 = dVector3Dot(vTemp,vCylinderAxis1);
    dVector3 vpnt;// = vol + vkl * (fdot1/fdot2);
    vpnt[0] = vol[0] + vkl[0] * fdot1/fdot2;
    vpnt[1] = vol[1] + vkl[1] * fdot1/fdot2;
    vpnt[2] = vol[2] + vkl[2] * fdot1/fdot2;

    // find tangent vector on circle with same center (vCenterPoint) that touches point of intersection (vpnt)
    dVector3 vTangent;
    dVector3Subtract(vCenterPoint,vpnt,vTemp);
    dVector3Cross(vTemp,vCylinderAxis1,vTangent);

    // find vector orthogonal both to tangent and edge direction
    dVector3 vAxis;
    dVector3Cross(vTangent,vkl,vAxis);

    // use that vector as separating axis
    return _cldTestAxis( cData ,v0, v1, v2, vAxis, iAxis );
}
Exemple #19
0
void dJointSetTransmissionAxis( dJointID j, dReal x, dReal y, dReal z )
{
    dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
    int i;
    
    dUASSERT( joint, "bad joint argument" );
    dUASSERT(joint->mode == dTransmissionParallelAxes ||
             joint->mode == dTransmissionChainDrive ,
             "axes must be set individualy in current mode" );

    for (i = 0 ; i < 2 ; i += 1) {
        if (joint->node[i].body) {
            dBodyVectorFromWorld(joint->node[i].body, x, y, z, joint->axes[i]);
            dNormalize3(joint->axes[i]);
        }
    }

    joint->update = 1;
}
// 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];
        }
        dCROSS( ax[1], = , ax[2], ax[0] );
        dNormalize3( ax[1] );
    }
Exemple #21
0
void dRFromZAxis (dMatrix3 R, dReal ax, dReal ay, dReal az)
{
  dVector3 n,p,q;
  n[0] = ax;
  n[1] = ay;
  n[2] = az;
  dNormalize3 (n);
  dPlaneSpace (n,p,q);
  _R(0,0) = p[0];
  _R(1,0) = p[1];
  _R(2,0) = p[2];
  _R(0,1) = q[0];
  _R(1,1) = q[1];
  _R(2,1) = q[2];
  _R(0,2) = n[0];
  _R(1,2) = n[1];
  _R(2,2) = n[2];
  _R(0,3) = REAL(0.0);
  _R(1,3) = REAL(0.0);
  _R(2,3) = REAL(0.0);
}
Exemple #22
0
void testNormalize3()
{
  HEADER;
  int i,j,bad=0;
  dVector3 n1,n2;
  for (i=0; i<1000; i++) {
    dMakeRandomVector (n1,3,1.0);
    for (j=0; j<3; j++) n2[j]=n1[j];
    dNormalize3 (n2);
    if (dFabs(dCalcVectorDot3(n2,n2) - 1.0) > tol) bad |= 1;
    if (dFabs(n2[0]/n1[0] - n2[1]/n1[1]) > tol) bad |= 2;
    if (dFabs(n2[0]/n1[0] - n2[2]/n1[2]) > tol) bad |= 4;
    if (dFabs(n2[1]/n1[1] - n2[2]/n1[2]) > tol) bad |= 8;
    if (dFabs(dCalcVectorDot3(n2,n1) - dSqrt(dCalcVectorDot3(n1,n1))) > tol) bad |= 16;
    if (bad) {
      printf ("\tFAILED (code=%x)\n",bad);
      return;
    }
  }
  printf ("\tpassed\n");
}
Exemple #23
0
void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz,
                  dReal dx, dReal dy, dReal dz)
{
    dUASSERT (g && g->type == dRayClass,"argument not a ray");
    g->recomputePosr();
    dReal* rot = g->final_posr->R;
    dReal* pos = g->final_posr->pos;
    dVector3 n;
    pos[0] = px;
    pos[1] = py;
    pos[2] = pz;

    n[0] = dx;
    n[1] = dy;
    n[2] = dz;
    dNormalize3(n);
    rot[0*4+2] = n[0];
    rot[1*4+2] = n[1];
    rot[2*4+2] = n[2];
    dGeomMoved (g);
}
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 #25
0
static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius,
                              dContactGeom *contact, int mode)
{
    dVector3 q;
    q[0] = ray->final_posr->pos[0] - sphere_pos[0];
    q[1] = ray->final_posr->pos[1] - sphere_pos[1];
    q[2] = ray->final_posr->pos[2] - sphere_pos[2];
    dReal B = dCalcVectorDot3_14(q,ray->final_posr->R+2);
    dReal C = dCalcVectorDot3(q,q) - radius*radius;
    // note: if C <= 0 then the start of the ray is inside the sphere
    dReal k = B*B - C;
    if (k < 0) return 0;
    k = dSqrt(k);
    dReal alpha;
    if (mode && C >= 0) {
        alpha = -B + k;
        if (alpha < 0) return 0;
    }
    else {
        alpha = -B - k;
        if (alpha < 0) {
            alpha = -B + k;
            if (alpha < 0) return 0;
        }
    }
    if (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];
    dReal nsign = (C < 0 || mode) ? REAL(-1.0) : REAL(1.0);
    contact->normal[0] = nsign*(contact->pos[0] - sphere_pos[0]);
    contact->normal[1] = nsign*(contact->pos[1] - sphere_pos[1]);
    contact->normal[2] = nsign*(contact->pos[2] - sphere_pos[2]);
    dNormalize3 (contact->normal);
    contact->depth = alpha;
    return 1;
}
void TestOneTriangleVsCylinder(   sData& cData,
                                  const dVector3 &v0,
                                  const dVector3 &v1,
                                  const dVector3 &v2,
                                  const bool bDoubleSided)
{

    // calculate triangle normal
    dVector3Subtract( v2 , v1 ,cData.vE1);
    dVector3 vTemp;
    dVector3Subtract( v0 , v1 ,vTemp);
    dVector3Cross(cData.vE1 , vTemp , cData.vNormal );

    dNormalize3( cData.vNormal);

    // create plane from triangle
    //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 );
    dReal plDistance = -dVector3Dot(v0, cData.vNormal);
    dVector4 plTrianglePlane;
    dConstructPlane( cData.vNormal,plDistance,plTrianglePlane);

    // calculate sphere distance to plane
    dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(cData.vCylinderPos , plTrianglePlane);

    // Sphere must be over positive side of triangle
    if(fDistanceCylinderCenterToPlane < 0 && !bDoubleSided)
    {
        // if not don't generate contacts
        return;
    }

    dVector3 vPnt0;
    dVector3 vPnt1;
    dVector3 vPnt2;

    if (fDistanceCylinderCenterToPlane < REAL(0.0) )
    {
        // flip it
        dVector3Copy(v0 , vPnt0);
        dVector3Copy(v1 , vPnt2);
        dVector3Copy(v2 , vPnt1);
    }
    else
    {
        dVector3Copy(v0 , vPnt0);
        dVector3Copy(v1 , vPnt1);
        dVector3Copy(v2 , vPnt2);
    }

    cData.fBestDepth = MAX_REAL;

    // do intersection test and find best separating axis
    if(!_cldTestSeparatingAxes(cData , vPnt0, vPnt1, vPnt2) )
    {
        // if not found do nothing
        return;
    }

    // if best separation axis is not found
    if ( cData.iBestAxis == 0 )
    {
        // this should not happen (we should already exit in that case)
        dIASSERT(false);
        // do nothing
        return;
    }

    dReal fdot = dVector3Dot( cData.vContactNormal , cData.vCylinderAxis );

    // choose which clipping method are we going to apply
    if (dFabs(fdot) < REAL(0.9) )
    {
        if (!_cldClipCylinderEdgeToTriangle(cData ,vPnt0, vPnt1, vPnt2))
        {
            return;
        }
    }
    else
    {
        _cldClipCylinderToTriangle(cData ,vPnt0, vPnt1, vPnt2);
    }

}
Exemple #27
0
// simulation loop
static void simLoop (int pause)
{
  static bool todo = false;
  if ( todo ) { // DEBUG
    static int cnt = 0;
    ++cnt;

    if (cnt == 5)
      command ( 'q' );
    if (cnt == 10)
      dsStop();
  }




  if (!pause) {
    double simstep = 0.01; // 10ms simulation steps
    double dt = dsElapsedTime();

    int nrofsteps = (int) ceilf (dt/simstep);
    if (!nrofsteps)
      nrofsteps = 1;

    for (int i=0; i<nrofsteps && !pause; i++) {
      dSpaceCollide (space,0,&nearCallback);
      dWorldStep (world, simstep);

      dJointGroupEmpty (contactgroup);
    }

    update();


    dReal radius, length;

    dsSetTexture (DS_WOOD);

    drawBox (geom[W], 1,1,0);


    drawBox (geom[EXT], 0,1,0);

    dVector3 anchorPos;



    dReal ang1 = 0;
    dReal ang2 = 0;
    dVector3 axisP, axisR1, axisR2;

    if ( dJointTypePU == type ) {
      dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
      ang1 = pu->getAngle1();
      ang2 = pu->getAngle2();
      pu->getAxis1 (axisR1);
      pu->getAxis2 (axisR2);
      pu->getAxisP (axisP);

      dJointGetPUAnchor (pu->id(), anchorPos);
    }
    else if ( dJointTypePR == type ) {
      dPRJoint *pr = dynamic_cast<dPRJoint *> (joint);
      pr->getAxis1 (axisP);
      pr->getAxis2 (axisR1);

      dJointGetPRAnchor (pr->id(), anchorPos);
    }


    // Draw the axisR
    if ( geom[INT] ) {
      dsSetColor (1,0,1);
      dVector3 l;
      dGeomBoxGetLengths (geom[INT], l);

      const dReal *rotBox = dGeomGetRotation (geom[W]);

      dVector3 pos;
      for (int i=0; i<3; ++i)
        pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i];
      dsDrawBox (pos, rotBox, l);
    }

    dsSetTexture (DS_CHECKERED);
    if ( geom[AXIS1] ) {
      dQuaternion q, qAng;
      dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
      dGeomGetQuaternion (geom[AXIS1], q);

      dQuaternion qq;
      dQMultiply1 (qq, qAng, q);
      dMatrix3 R;
      dQtoR (qq,R);


      dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS1]), &radius, &length);
      dsSetColor (1,0,0);
      dsDrawCylinder (anchorPos, R, length, radius);
    }

    if ( dJointTypePU == type && geom[AXIS2] ) {
      //dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);

      dQuaternion q, qAng, qq, qq1;
      dGeomGetQuaternion (geom[AXIS2], q);

      dQFromAxisAndAngle (qAng, 0, 1, 0, ang2);
      dQMultiply1 (qq, qAng, q);


      dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);

      dQMultiply1 (qq1, qAng, qq);


      dMatrix3 R;
      dQtoR (qq1,R);


      dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS2]), &radius, &length);
      dsSetColor (0,0,1);
      dsDrawCylinder (anchorPos, R, length, radius);
    }

    dsSetTexture (DS_WOOD);

    // Draw the anchor
    if ( geom[ANCHOR] ) {
      dsSetColor (1,1,1);
      dVector3 l;
      dGeomBoxGetLengths (geom[ANCHOR], l);

      const dReal *rotBox = dGeomGetRotation (geom[D]);
      const dReal *posBox = dGeomGetPosition (geom[D]);

      dVector3 e;
      for (int i=0; i<3; ++i)
        e[i] = posBox[i] - anchorPos[i];
      dNormalize3 (e);

      dVector3 pos;
      for (int i=0; i<3; ++i)
        pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i];
      dsDrawBox (pos, rotBox, l);
    }

    drawBox (geom[D], 1,1,0);
  }
}
// clip and generate contacts
static void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) {

  // if we have edge/edge intersection
  if ( iBestAxis > 4 ) {

    dVector3 vub,vPb,vPa;

    SET(vPa,vHullBoxPos);

    // calculate point on box edge
    for( int i=0; i<3; i++) {
      dVector3 vRotCol;
      GETCOL(mHullBoxRot,i,vRotCol);
      dReal fSign = dDOT(vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f;

      vPa[0] += fSign * vBoxHalfSize[i] * vRotCol[0];
      vPa[1] += fSign * vBoxHalfSize[i] * vRotCol[1];
      vPa[2] += fSign * vBoxHalfSize[i] * vRotCol[2];
    }

    int iEdge = (iBestAxis-5)%3;

    // decide which edge is on triangle
    if ( iEdge == 0 ) {
      SET(vPb,v0);
      SET(vub,vE0);
    } else if ( iEdge == 1) {
      SET(vPb,v2);
      SET(vub,vE1);
    } else {
      SET(vPb,v1);
      SET(vub,vE2);
    }
    

    // setup direction parameter for face edge
    dNormalize3(vub);

    dReal fParam1, fParam2;

    // setup direction parameter for box edge
    dVector3 vua;
    int col=(iBestAxis-5)/3;
    GETCOL(mHullBoxRot,col,vua);

    // find two closest points on both edges
    _cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 );
    vPa[0] += vua[0]*fParam1;
    vPa[1] += vua[1]*fParam1;
    vPa[2] += vua[2]*fParam1;

    vPb[0] += vub[0]*fParam2; 
    vPb[1] += vub[1]*fParam2; 
    vPb[2] += vub[2]*fParam2; 

    // calculate collision point
    dVector3 vPntTmp;
    ADD(vPa,vPb,vPntTmp);

    vPntTmp[0]*=0.5f;
    vPntTmp[1]*=0.5f;
    vPntTmp[2]*=0.5f;

    // generate contact point between two closest points
#ifdef ORIG
    if (ctContacts < (iFlags & 0x0ffff)) {
    dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);
    Contact->depth = fBestDepth;
    SET(Contact->normal,vBestNormal);
    SET(Contact->pos,vPntTmp);
    Contact->g1 = Geom1;
    Contact->g2 = Geom2;
    ctContacts++;
    }
#endif
    GenerateContact(iFlags, ContactGeoms, iStride,  Geom1, Geom2,
                    vPntTmp, vBestNormal, fBestDepth, ctContacts);



  // if triangle is the referent face then clip box to triangle face
  } else if ( iBestAxis == 1 ) {
    
    
    dVector3 vNormal2;
    vNormal2[0]=-vBestNormal[0];
    vNormal2[1]=-vBestNormal[1];
    vNormal2[2]=-vBestNormal[2];

    
    // vNr is normal in box frame, pointing from triangle to box
    dMatrix3 mTransposed;
    mTransposed[0*4+0]=mHullBoxRot[0*4+0];
    mTransposed[0*4+1]=mHullBoxRot[1*4+0];
    mTransposed[0*4+2]=mHullBoxRot[2*4+0];

    mTransposed[1*4+0]=mHullBoxRot[0*4+1];
    mTransposed[1*4+1]=mHullBoxRot[1*4+1];
    mTransposed[1*4+2]=mHullBoxRot[2*4+1];

    mTransposed[2*4+0]=mHullBoxRot[0*4+2];
    mTransposed[2*4+1]=mHullBoxRot[1*4+2];
    mTransposed[2*4+2]=mHullBoxRot[2*4+2];

    dVector3 vNr;
    vNr[0]=mTransposed[0*4+0]*vNormal2[0]+  mTransposed[0*4+1]*vNormal2[1]+  mTransposed[0*4+2]*vNormal2[2];
    vNr[1]=mTransposed[1*4+0]*vNormal2[0]+  mTransposed[1*4+1]*vNormal2[1]+  mTransposed[1*4+2]*vNormal2[2];
    vNr[2]=mTransposed[2*4+0]*vNormal2[0]+  mTransposed[2*4+1]*vNormal2[1]+  mTransposed[2*4+2]*vNormal2[2];
  

    dVector3 vAbsNormal;
    vAbsNormal[0] = dFabs( vNr[0] );
    vAbsNormal[1] = dFabs( vNr[1] );
    vAbsNormal[2] = dFabs( vNr[2] );

    // get closest face from box
    int iB0, iB1, iB2;
    if (vAbsNormal[1] > vAbsNormal[0]) {
      if (vAbsNormal[1] > vAbsNormal[2]) {
        iB1 = 0;  iB0 = 1;  iB2 = 2;
      } else {
        iB1 = 0;  iB2 = 1;  iB0 = 2;
      }
    } else {

      if (vAbsNormal[0] > vAbsNormal[2]) {
        iB0 = 0;  iB1 = 1;  iB2 = 2;
      } else {
        iB1 = 0;  iB2 = 1;  iB0 = 2;
      }
    }

    // Here find center of box face we are going to project
    dVector3 vCenter;
    dVector3 vRotCol;
    GETCOL(mHullBoxRot,iB0,vRotCol);
    
    if (vNr[iB0] > 0) {
        vCenter[0] = vHullBoxPos[0] - v0[0] - vBoxHalfSize[iB0] * vRotCol[0];
      vCenter[1] = vHullBoxPos[1] - v0[1] - vBoxHalfSize[iB0] * vRotCol[1];
      vCenter[2] = vHullBoxPos[2] - v0[2] - vBoxHalfSize[iB0] * vRotCol[2];
    } else {
      vCenter[0] = vHullBoxPos[0] - v0[0] + vBoxHalfSize[iB0] * vRotCol[0];
      vCenter[1] = vHullBoxPos[1] - v0[1] + vBoxHalfSize[iB0] * vRotCol[1];
      vCenter[2] = vHullBoxPos[2] - v0[2] + vBoxHalfSize[iB0] * vRotCol[2];
    }  

    // Here find 4 corner points of box
    dVector3 avPoints[4];

    dVector3 vRotCol2;
    GETCOL(mHullBoxRot,iB1,vRotCol);
    GETCOL(mHullBoxRot,iB2,vRotCol2);

    for(int x=0;x<3;x++) {
        avPoints[0][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
        avPoints[1][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]);
        avPoints[2][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
        avPoints[3][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]);
    }


    // clip Box face with 4 planes of triangle (1 face plane, 3 egde planes)
    dVector3 avTempArray1[9];
    dVector3 avTempArray2[9];
    dVector4 plPlane;

    int iTempCnt1=0;
    int iTempCnt2=0;

    // zeroify vectors - necessary?
    for(int i=0; i<9; i++) {
      avTempArray1[i][0]=0;
      avTempArray1[i][1]=0;
      avTempArray1[i][2]=0;

      avTempArray2[i][0]=0;
      avTempArray2[i][1]=0;
      avTempArray2[i][2]=0;
    }


    // Normal plane
    dVector3 vTemp;
    vTemp[0]=-vN[0];
    vTemp[1]=-vN[1];
    vTemp[2]=-vN[2];
    dNormalize3(vTemp);
    CONSTRUCTPLANE(plPlane,vTemp,0);

    _cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane  );
    

    // Plane p0
    dVector3 vTemp2;
    SUBTRACT(v1,v0,vTemp2);
    dCROSS(vTemp,=,vN,vTemp2);
    dNormalize3(vTemp);
    CONSTRUCTPLANE(plPlane,vTemp,0);

    _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane  );


    // Plane p1
    SUBTRACT(v2,v1,vTemp2);
    dCROSS(vTemp,=,vN,vTemp2);
    dNormalize3(vTemp);
    SUBTRACT(v0,v2,vTemp2);
    CONSTRUCTPLANE(plPlane,vTemp,dDOT(vTemp2,vTemp));

    _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane  );


    // Plane p2
    SUBTRACT(v0,v2,vTemp2);
    dCROSS(vTemp,=,vN,vTemp2);
    dNormalize3(vTemp);
    CONSTRUCTPLANE(plPlane,vTemp,0);

    _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane  );


    // END of clipping polygons



    // for each generated contact point
    for ( int i=0; i<iTempCnt2; i++ ) {
      // calculate depth
      dReal fTempDepth = dDOT(vNormal2,avTempArray2[i]);

      // clamp depth to zero
      if (fTempDepth > 0) {
        fTempDepth = 0;
      }

      dVector3 vPntTmp;
      ADD(avTempArray2[i],v0,vPntTmp);

#ifdef ORIG
    if (ctContacts < (iFlags & 0x0ffff)) {
          dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride);

          Contact->depth = -fTempDepth;
          SET(Contact->normal,vBestNormal);
          SET(Contact->pos,vPntTmp);
          Contact->g1 = Geom1;
          Contact->g2 = Geom2;
          ctContacts++;
    }
#endif
    GenerateContact(iFlags, ContactGeoms, iStride,  Geom1, Geom2,
                    vPntTmp, vBestNormal, -fTempDepth, ctContacts);
    }

    //dAASSERT(ctContacts>0);

  // if box face is the referent face, then clip triangle on box face
  } else { // 2 <= if iBestAxis <= 4
Exemple #29
0
int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags,
		       dContactGeom *contact, int skip)
{
  dIASSERT (skip >= (int)sizeof(dContactGeom));
  dIASSERT (o1->type == dSphereClass);
  dIASSERT (o2->type == dBoxClass);
  dIASSERT ((flags & NUMC_MASK) >= 1);
  
  // this is easy. get the sphere center `p' relative to the box, and then clip
  // that to the boundary of the box (call that point `q'). if q is on the
  // boundary of the box and |p-q| is <= sphere radius, they touch.
  // if q is inside the box, the sphere is inside the box, so set a contact
  // normal to push the sphere to the closest box face.

  dVector3 l,t,p,q,r;
  dReal depth;
  int onborder = 0;

  dxSphere *sphere = (dxSphere*) o1;
  dxBox *box = (dxBox*) o2;

  contact->g1 = o1;
  contact->g2 = o2;

  p[0] = o1->final_posr->pos[0] - o2->final_posr->pos[0];
  p[1] = o1->final_posr->pos[1] - o2->final_posr->pos[1];
  p[2] = o1->final_posr->pos[2] - o2->final_posr->pos[2];

  l[0] = box->side[0]*REAL(0.5);
  t[0] = dDOT14(p,o2->final_posr->R);
  if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; }
  if (t[0] >  l[0]) { t[0] =  l[0]; onborder = 1; }

  l[1] = box->side[1]*REAL(0.5);
  t[1] = dDOT14(p,o2->final_posr->R+1);
  if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; }
  if (t[1] >  l[1]) { t[1] =  l[1]; onborder = 1; }

  t[2] = dDOT14(p,o2->final_posr->R+2);
  l[2] = box->side[2]*REAL(0.5);
  if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; }
  if (t[2] >  l[2]) { t[2] =  l[2]; onborder = 1; }

  if (!onborder) {
    // sphere center inside box. find closest face to `t'
    dReal min_distance = l[0] - dFabs(t[0]);
    int mini = 0;
    for (int i=1; i<3; i++) {
      dReal face_distance = l[i] - dFabs(t[i]);
      if (face_distance < min_distance) {
	min_distance = face_distance;
	mini = i;
      }
    }
    // contact position = sphere center
    contact->pos[0] = o1->final_posr->pos[0];
    contact->pos[1] = o1->final_posr->pos[1];
    contact->pos[2] = o1->final_posr->pos[2];
    // contact normal points to closest face
    dVector3 tmp;
    tmp[0] = 0;
    tmp[1] = 0;
    tmp[2] = 0;
    tmp[mini] = (t[mini] > 0) ? REAL(1.0) : REAL(-1.0);
    dMULTIPLY0_331 (contact->normal,o2->final_posr->R,tmp);
    // contact depth = distance to wall along normal plus radius
    contact->depth = min_distance + sphere->radius;
    return 1;
  }

  t[3] = 0;			//@@@ hmmm
  dMULTIPLY0_331 (q,o2->final_posr->R,t);
  r[0] = p[0] - q[0];
  r[1] = p[1] - q[1];
  r[2] = p[2] - q[2];
  depth = sphere->radius - dSqrt(dDOT(r,r));
  if (depth < 0) return 0;
  contact->pos[0] = q[0] + o2->final_posr->pos[0];
  contact->pos[1] = q[1] + o2->final_posr->pos[1];
  contact->pos[2] = q[2] + o2->final_posr->pos[2];
  contact->normal[0] = r[0];
  contact->normal[1] = r[1];
  contact->normal[2] = r[2];
  dNormalize3 (contact->normal);
  contact->depth = depth;
  return 1;
}
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){
	dIASSERT (Stride >= (int)sizeof(dContactGeom));
	dIASSERT (g1->type == dTriMeshClass);
	dIASSERT (SphereGeom->type == dSphereClass);
	dIASSERT ((Flags & NUMC_MASK) >= 1);

	dxTriMesh* TriMesh = (dxTriMesh*)g1;

	// Init
	const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh);
	const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh);

	TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache();
	SphereCollider& Collider = pccColliderCache->_SphereCollider;

	const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom);
	dReal Radius = dGeomSphereGetRadius(SphereGeom);

	// Sphere
	Sphere Sphere;
	Sphere.mCenter.x = Position[0];
	Sphere.mCenter.y = Position[1];
	Sphere.mCenter.z = Position[2];
	Sphere.mRadius = Radius;

	Matrix4x4 amatrix;

	// TC results
	if (TriMesh->doSphereTC) {
		dxTriMesh::SphereTC* sphereTC = 0;
		for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){
			if (TriMesh->SphereTCCache[i].Geom == SphereGeom){
				sphereTC = &TriMesh->SphereTCCache[i];
				break;
			}
		}

		if (!sphereTC){
			TriMesh->SphereTCCache.push(dxTriMesh::SphereTC());

			sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1];
			sphereTC->Geom = SphereGeom;
		}
		
		// Intersect
		Collider.SetTemporalCoherence(true);
		Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null, 
						 &MakeMatrix(TLPosition, TLRotation, amatrix));
	}
	else {
		Collider.SetTemporalCoherence(false);
		Collider.Collide(pccColliderCache->defaultSphereCache, Sphere, TriMesh->Data->BVTree, null, 
						 &MakeMatrix(TLPosition, TLRotation, amatrix));
 	}

	if (! Collider.GetContactStatus()) {
		// no collision occurred
		return 0;
	}

	// get results
	int TriCount = Collider.GetNbTouchedPrimitives();
	const int* Triangles = (const int*)Collider.GetTouchedPrimitives();

	if (TriCount != 0){
		if (TriMesh->ArrayCallback != null){
			TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount);
		}

		int OutTriCount = 0;
		for (int i = 0; i < TriCount; i++){
			if (OutTriCount == (Flags & NUMC_MASK)){
				break;
			}

			const int TriIndex = Triangles[i];

			dVector3 dv[3];
			if (!Callback(TriMesh, SphereGeom, TriIndex))
				continue;
			
			FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv);

			dVector3& v0 = dv[0];
			dVector3& v1 = dv[1];
			dVector3& v2 = dv[2];

			dVector3 vu;
			vu[0] = v1[0] - v0[0];
			vu[1] = v1[1] - v0[1];
			vu[2] = v1[2] - v0[2];
			vu[3] = REAL(0.0);

			dVector3 vv;
			vv[0] = v2[0] - v0[0];
			vv[1] = v2[1] - v0[1];
			vv[2] = v2[2] - v0[2];
			vv[3] = REAL(0.0);

			// Get plane coefficients
			dVector4 Plane;
			dCROSS(Plane, =, vu, vv);

			// Even though all triangles might be initially valid, 
			// a triangle may degenerate into a segment after applying 
			// space transformation.
			if (!dSafeNormalize3(Plane)) {
				continue;
			}

			/* If the center of the sphere is within the positive halfspace of the
				* triangle's plane, allow a contact to be generated.
				* If the center of the sphere made it into the positive halfspace of a
				* back-facing triangle, then the physics update and/or velocity needs
				* to be adjusted (penetration has occured anyway).
				*/
		  
			dReal side = dDOT(Plane,Position) - dDOT(Plane, v0);

			if(side < REAL(0.0)) {
				continue;
			}

			dReal Depth;
			dReal u, v;
			if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){
				continue;	// Sphere doesn't hit triangle
			}

			if (Depth < REAL(0.0)){
				continue; // Negative depth does not produce a contact
			}

			dVector3 ContactPos;

			dReal w = REAL(1.0) - u - v;
			ContactPos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v);
			ContactPos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v);
			ContactPos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v);

			// Depth returned from GetContactData is depth along 
			// contact point - sphere center direction
			// we'll project it to contact normal
			dVector3 dir;
			dir[0] = Position[0]-ContactPos[0];
			dir[1] = Position[1]-ContactPos[1];
			dir[2] = Position[2]-ContactPos[2];
			dReal dirProj = dDOT(dir, Plane) / dSqrt(dDOT(dir, dir));
			
			// Since Depth already had a requirement to be non-negative,
			// negative direction projections should not be allowed as well,
			// as otherwise the multiplication will result in negative contact depth.
			if (dirProj < REAL(0.0)){
				continue; // Zero contact depth could be ignored
			}

			dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride);

			Contact->pos[0] = ContactPos[0];
			Contact->pos[1] = ContactPos[1];
			Contact->pos[2] = ContactPos[2];
			Contact->pos[3] = REAL(0.0);

			// Using normal as plane (reversed)
			Contact->normal[0] = -Plane[0];
			Contact->normal[1] = -Plane[1];
			Contact->normal[2] = -Plane[2];
			Contact->normal[3] = REAL(0.0);

			Contact->depth = Depth * dirProj;
			//Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance
			
#if !defined MERGECONTACTS	// Merge all contacts into 1
            Contact->g1 = TriMesh;
            Contact->g2 = SphereGeom;

            Contact->side2 = -1;
#endif // Otherwise assigned later

            Contact->side1 = TriIndex;

			OutTriCount++;
		}
#if defined MERGECONTACTS	// Merge all contacts into 1
		if (OutTriCount > 0){
			dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride);
            Contact->g1 = TriMesh;
            Contact->g2 = SphereGeom;
            Contact->side2 = -1;

			if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT)){
			    dVector3 pos;
                pos[0] = Contact->pos[0];
                pos[1] = Contact->pos[1];
                pos[2] = Contact->pos[2];

                dVector3 normal;
                normal[0] = Contact->normal[0] * Contact->depth;
                normal[1] = Contact->normal[1] * Contact->depth;
                normal[2] = Contact->normal[2] * Contact->depth;
                
                int TriIndex = Contact->side1;

				for (int i = 1; i < OutTriCount; i++){
					dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride);
					
					pos[0] += TempContact->pos[0];
					pos[1] += TempContact->pos[1];
					pos[2] += TempContact->pos[2];
					
					normal[0] += TempContact->normal[0] * TempContact->depth;
					normal[1] += TempContact->normal[1] * TempContact->depth;
					normal[2] += TempContact->normal[2] * TempContact->depth;

                    TriIndex = (TriMesh->TriMergeCallback) ? TriMesh->TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1;
				}
			
                Contact->side1 = TriIndex;

                Contact->pos[0] = pos[0] / OutTriCount;
				Contact->pos[1] = pos[1] / OutTriCount;
				Contact->pos[2] = pos[2] / OutTriCount;
				
				// Remember to divide in square space.
				Contact->depth = dSqrt(dDOT(normal, normal) / OutTriCount);

				if (Contact->depth > dEpsilon) { // otherwise the normal is too small
                    dVector3Copy(Contact->normal, normal);
					dNormalize3(Contact->normal);
				} // otherwise original Contact's normal would be used and it should be already normalized
			}

			return 1;
		}
		else return 0;
#elif defined MERGECONTACTNORMALS	// Merge all normals, and distribute between all contacts
		if (OutTriCount != 0){
            if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){
				dVector3 Normal;

                dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride);
				Normal[0] = FirstContact->normal[0] * FirstContact->depth;
				Normal[1] = FirstContact->normal[1] * FirstContact->depth;
				Normal[2] = FirstContact->normal[2] * FirstContact->depth;
				Normal[3] = FirstContact->normal[3] * FirstContact->depth;

				for (int i = 1; i < OutTriCount; i++){
					dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);

					Normal[0] += Contact->normal[0] * Contact->depth;
					Normal[1] += Contact->normal[1] * Contact->depth;
					Normal[2] += Contact->normal[2] * Contact->depth;
					Normal[3] += Contact->normal[3] * Contact->depth;
				}

                dNormalize3(Normal);

				for (int i = 0; i < OutTriCount; i++){
					dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride);

					Contact->normal[0] = Normal[0];
					Contact->normal[1] = Normal[1];
					Contact->normal[2] = Normal[2];
					Contact->normal[3] = Normal[3];
				}
			}

			return OutTriCount;
		}
		else return 0;
#else   // none of MERGECONTACTS and MERGECONTACTNORMALS // Just return

        return OutTriCount;
#endif	// MERGECONTACTS
	}
	else return 0;