dcylinder.cc

来自「机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。」· CC 代码 · 共 1,448 行 · 第 1/3 页

#define TEST(expr1,expr2,norm,cc) \
  s2 = dFabs(expr1) - (expr2); \
  if (s2 > 0) return 0; \
  if (s2 > s) { \
    s = s2; \
    normalR = norm; \
    invert_normal = ((expr1) < 0); \
    code = (cc); \
  }

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

  // separating axis cyl ax 

  TEST (dDOT14(p,R+1),sphereRadius+hl,R+1,2);
  // note: cross product axes need to be scaled when s is computed.
  // normal (n1,n2,n3) is relative to 
#undef TEST
#define TEST(expr1,expr2,n1,n2,n3,cc) \
  s2 = dFabs(expr1) - (expr2); \
  if (s2 > 0) return 0; \
  if (s2 > s) { \
      s = s2; \
	  normalR = 0; \
      normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \
      invert_normal = ((expr1) < 0); \
      code = (cc); \
    } 
 
//making ax which is perpendicular to cyl1 ax to sphere center//
 
dReal proj,cos,sin,cos1,cos3;
dVector3 Ax;
	proj=dDOT14(p2,R+1)-dDOT14(p1,R+1);

	Ax[0]=p2[0]-p1[0]-R[1]*proj;
	Ax[1]=p2[1]-p1[1]-R[5]*proj;
	Ax[2]=p2[2]-p1[2]-R[9]*proj;
dNormalize3(Ax);
TEST(dDOT(p,Ax),sphereRadius+cylRadius,Ax[0],Ax[1],Ax[2],9);


Ax[0]=p[0];
Ax[1]=p[1];
Ax[2]=p[2];
dNormalize3(Ax);

	dVector3 pa;
    dReal sign, factor;
    for (i=0; i<3; i++) pa[i] = p1[i];

  	cos1 = dDOT14(Ax,R+0);
	cos3 = dDOT14(Ax,R+2) ;
	factor=sqrtf(cos1*cos1+cos3*cos3);
	cos1/=factor;
	cos3/=factor;
    for (i=0; i<3; i++) pa[i] += cos1 * cylRadius * R[i*4];
    sign = (dDOT14(normal,R+1) > 0) ? REAL(1.0) : REAL(-1.0);
    for (i=0; i<3; i++) pa[i] += sign * hl * R[i*4+1];
    for (i=0; i<3; i++) pa[i] += cos3 * cylRadius  * R[i*4+2];

Ax[0]=p2[0]-pa[0];
Ax[1]=p2[1]-pa[1];
Ax[2]=p2[2]-pa[2];
dNormalize3(Ax);

 cos=dFabs(dDOT14(Ax,R+1));
 cos1=dDOT14(Ax,R+0);
 cos3=dDOT14(Ax,R+2);
 sin=sqrtf(cos1*cos1+cos3*cos3);
TEST(dDOT(p,Ax),sphereRadius+cylRadius*sin+hl*cos,Ax[0],Ax[1],Ax[2],14);


#undef TEST

  if (normalR) {
    normal[0] = normalR[0];
    normal[1] = normalR[4];
    normal[2] = normalR[8];
  }
  else {

	normal[0] = normalC[0];
	normal[1] = normalC[1];
	normal[2] = normalC[2];
		}
  if (invert_normal) {
    normal[0] = -normal[0];
    normal[1] = -normal[1];
    normal[2] = -normal[2];
  }
   // compute contact point(s)
contact->depth=-s;
contact->normal[0]=-normal[0];
contact->normal[1]=-normal[1];
contact->normal[2]=-normal[2];
contact->g1=const_cast<dxGeom*> (o1);
contact->g2=const_cast<dxGeom*> (o2);
contact->pos[0]=p2[0]-normal[0]*sphereRadius;
contact->pos[1]=p2[1]-normal[1]*sphereRadius;
contact->pos[2]=p2[2]-normal[2]*sphereRadius;
return 1;
}



int dCollideCylB (dxGeom *o1, dxGeom *o2, int flags,
		dContactGeom *contact, int skip)
{
  dVector3 normal;
  dReal depth;
  int code;
  dReal cylRadius,cylLength;
  dVector3 boxSides;
  dGeomCylinderGetParams(o1,&cylRadius,&cylLength);
  dGeomBoxGetLengths(o2,boxSides);
  int num = dCylBox(dGeomGetPosition(o1),dGeomGetRotation(o1),cylRadius,cylLength, 
					dGeomGetPosition(o2),dGeomGetRotation(o2),boxSides,
					normal,&depth,&code,flags & NUMC_MASK,contact,skip);
  for (int i=0; i<num; i++) {
    CONTACT(contact,i*skip)->normal[0] = -normal[0];
    CONTACT(contact,i*skip)->normal[1] = -normal[1];
    CONTACT(contact,i*skip)->normal[2] = -normal[2];
    CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
  }
  return num;
}

int dCollideCylCyl (dxGeom *o1, dxGeom *o2, int flags,
		dContactGeom *contact, int skip)
{
  dVector3 normal;
  dReal depth;
  int code;
dReal cylRadius1,cylRadius2;
dReal cylLength1,cylLength2;
dGeomCylinderGetParams(o1,&cylRadius1,&cylLength1);
dGeomCylinderGetParams(o1,&cylRadius2,&cylLength2);
int num = dCylCyl (dGeomGetPosition(o1),dGeomGetRotation(o1),cylRadius1,cylLength1,
				   dGeomGetPosition(o2),dGeomGetRotation(o2),cylRadius2,cylRadius2,
				     normal,&depth,&code,flags & NUMC_MASK,contact,skip);

  for (int i=0; i<num; i++) {
    CONTACT(contact,i*skip)->normal[0] = -normal[0];
    CONTACT(contact,i*skip)->normal[1] = -normal[1];
    CONTACT(contact,i*skip)->normal[2] = -normal[2];
    CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
  }
  return num;
}

struct dxPlane {
  dReal p[4];
};


int dCollideCylPlane 
	(
	dxGeom *o1, dxGeom *o2, int flags,
			  dContactGeom *contact, int skip){
  dIASSERT (skip >= (int)sizeof(dContactGeom));
  dIASSERT (dGeomGetClass(o1) == dCylinderClassUser);
  dIASSERT (dGeomGetClass(o2) == dPlaneClass);
  contact->g1 = const_cast<dxGeom*> (o1);
  contact->g2 = const_cast<dxGeom*> (o2);
  
 unsigned int ret = 0;

 dReal radius;
 dReal hlz;
 dGeomCylinderGetParams(o1,&radius,&hlz);
 hlz /= 2;
 
 const dReal *R	=	dGeomGetRotation(o1);// rotation of cylinder
 const dReal* p	=	dGeomGetPosition(o1);
 dVector4 n;		// normal vector
 dReal pp;
 dGeomPlaneGetParams (o2, n);
 pp=n[3];
 dReal cos1,sin1;
  cos1=dFabs(dDOT14(n,R+1));

cos1=cos1<REAL(1.) ? cos1 : REAL(1.); //cos1 may slightly exeed 1.f
sin1=sqrtf(REAL(1.)-cos1*cos1);
//////////////////////////////

dReal sidePr=cos1*hlz+sin1*radius;

dReal dist=-pp+dDOT(n,p);
dReal outDepth=sidePr-dist;

if(outDepth<0.f) return 0;

dVector3 pos;


/////////////////////////////////////////// from geom.cpp dCollideBP
  dReal Q1 = dDOT14(n,R+0);
  dReal Q2 = dDOT14(n,R+1);
  dReal Q3 = dDOT14(n,R+2);
  dReal factor =sqrtf(Q1*Q1+Q3*Q3);
  factor= factor ? factor :1.f;
  dReal A1 = radius *		Q1/factor;
  dReal A2 = hlz*Q2;
  dReal A3 = radius *		Q3/factor;

  pos[0]=p[0];
  pos[1]=p[1];
  pos[2]=p[2];

  pos[0]-= A1*R[0];
  pos[1]-= A1*R[4];
  pos[2]-= A1*R[8];

  pos[0]-= A3*R[2];
  pos[1]-= A3*R[6];
  pos[2]-= A3*R[10];

  pos[0]-= A2>0 ? hlz*R[1]:-hlz*R[1];
  pos[1]-= A2>0 ? hlz*R[5]:-hlz*R[5];
  pos[2]-= A2>0 ? hlz*R[9]:-hlz*R[9];
  
 

  contact->pos[0] = pos[0];
  contact->pos[1] = pos[1];
  contact->pos[2] = pos[2];
   contact->depth = outDepth;
  ret=1;
 
if(dFabs(Q2)>M_SQRT1_2){

  CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A1*R[0];
  CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A1*R[4];
  CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A1*R[8];
  CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q1*2.f*A1);

  if(CONTACT(contact,ret*skip)->depth>0.f)
  ret++;
  
  
  CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A3*R[2];
  CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A3*R[6];
  CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A3*R[10];
  CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q3*2.f*A3);

  if(CONTACT(contact,ret*skip)->depth>0.f) ret++;
} else {

  CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*(A2>0 ? hlz*R[1]:-hlz*R[1]);
  CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*(A2>0 ? hlz*R[5]:-hlz*R[5]);
  CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*(A2>0 ? hlz*R[9]:-hlz*R[9]);
  CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q2*2.f*A2);

  if(CONTACT(contact,ret*skip)->depth>0.f) ret++;
}



 for (unsigned int i=0; i<ret; i++) {
    CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
	CONTACT(contact,i*skip)->normal[0] =n[0];
	CONTACT(contact,i*skip)->normal[1] =n[1];
	CONTACT(contact,i*skip)->normal[2] =n[2];
  }
  return ret;  
}

int dCollideCylRay(dxGeom *o1, dxGeom *o2, int flags,
		   dContactGeom *contact, int skip) {
  dIASSERT (skip >= (int)sizeof(dContactGeom));
  dIASSERT (dGeomGetClass(o1) == dCylinderClassUser);
  dIASSERT (dGeomGetClass(o2) == dRayClass);
  contact->g1 = const_cast<dxGeom*> (o1);
  contact->g2 = const_cast<dxGeom*> (o2);
  dReal radius;
  dReal lz;
  dGeomCylinderGetParams(o1,&radius,&lz);
  dReal lz2=lz*REAL(0.5);
  const dReal *R = dGeomGetRotation(o1); // rotation of the cylinder
  const dReal *p = dGeomGetPosition(o1); // position of the cylinder
  dVector3 start,dir;
  dGeomRayGet(o2,start,dir); // position and orientation of the ray
  dReal length = dGeomRayGetLength(o2);

  // compute some useful info
  dVector3 cs,q,r;
  dReal C,k;
  cs[0] = start[0] - p[0];
  cs[1] = start[1] - p[1];
  cs[2] = start[2] - p[2];
  k = dDOT41(R+1,cs);	// position of ray start along cyl axis (Y)
  q[0] = k*R[0*4+1] - cs[0];
  q[1] = k*R[1*4+1] - cs[1];
  q[2] = k*R[2*4+1] - cs[2];
  C = dDOT(q,q) - radius*radius;
  // if C < 0 then ray start position within infinite extension of cylinder
  // if ray start position is inside the cylinder
  int inside_cyl=0;
  if (C<0 && !(k<-lz2 || k>lz2)) inside_cyl=1;
  // compute ray collision with infinite cylinder, except for the case where
  // the ray is outside the cylinder but within the infinite cylinder
  // (it that case the ray can only hit endcaps)
  if (!inside_cyl && C < 0) {
    // set k to cap position to check
    if (k < 0) k = -lz2; else k = lz2;
  }
  else {
    dReal uv = dDOT41(R+1,dir);
    r[0] = uv*R[0*4+1] - dir[0];
    r[1] = uv*R[1*4+1] - dir[1];
    r[2] = uv*R[2*4+1] - dir[2];
    dReal A = dDOT(r,r);
    dReal B = 2*dDOT(q,r);
    k = B*B-4*A*C;
    if (k < 0) {
      // the ray does not intersect the infinite cylinder, but if the ray is
      // inside and parallel to the cylinder axis it may intersect the end
      // caps. set k to cap position to check.
      if (!inside_cyl) return 0;
      if (uv < 0) k = -lz2; else k = lz2;
    }
    else {
      k = dSqrt(k);
      A = dRecip (2*A);
      dReal alpha = (-B-k)*A;
      if (alpha < 0) {
	alpha = (-B+k)*A;
	if (alpha<0) return 0;
      }
      if (alpha>length) return 0;
      // the ray intersects the infinite cylinder. check to see if the
      // intersection point is between the caps
      contact->pos[0] = start[0] + alpha*dir[0];
      contact->pos[1] = start[1] + alpha*dir[1];
      contact->pos[2] = start[2] + alpha*dir[2];
      q[0] = contact->pos[0] - p[0];
      q[1] = contact->pos[1] - p[1];
      q[2] = contact->pos[2] - p[2];
      k = dDOT14(q,R+1);
      dReal nsign = inside_cyl ? -1 : 1;
      if (k >= -lz2 && k <= lz2) {
	contact->normal[0] = nsign * (contact->pos[0] -
				      (p[0] + k*R[0*4+1]));
	contact->normal[1] = nsign * (contact->pos[1] -
				      (p[1] + k*R[1*4+1]));
	contact->normal[2] = nsign * (contact->pos[2] -
				      (p[2] + k*R[2*4+1]));
	dNormalize3 (contact->normal);
	contact->depth = alpha;
	return 1;
      }
      // the infinite cylinder intersection point is not between the caps.
      // set k to cap position to check.
      if (k < 0) k = -lz2; else k = lz2;
    }
  }
  // check for ray intersection with the caps. k must indicate the cap
  // position to check
  // perform a ray plan interesection
  // R+1 is the plan normal
  q[0] = start[0] - (p[0] + k*R[0*4+1]);
  q[1] = start[1] - (p[1] + k*R[1*4+1]);
  q[2] = start[2] - (p[2] + k*R[2*4+1]);
  dReal alpha = -dDOT14(q,R+1);
  dReal k2 = dDOT14(dir,R+1);
  if (k2==0) return 0; // ray parallel to the plane
  alpha/=k2;
  if (alpha<0 || alpha>length) return 0; // too short
  contact->pos[0]=start[0]+alpha*dir[0];
  contact->pos[1]=start[1]+alpha*dir[1];
  contact->pos[2]=start[2]+alpha*dir[2];
  dReal nsign = (k<0)?-1:1;
  contact->normal[0]=nsign*R[0*4+1];
  contact->normal[1]=nsign*R[1*4+1];
  contact->normal[2]=nsign*R[2*4+1];
  contact->depth=alpha;
  return 1;
}

static  dColliderFn * dCylinderColliderFn (int num)
{
  if (num == dBoxClass) return (dColliderFn *) &dCollideCylB;
  else if (num == dSphereClass) return (dColliderFn *) &dCollideCylS;
  else if (num == dCylinderClassUser) return (dColliderFn *) &dCollideCylCyl;
  else if (num == dPlaneClass) return (dColliderFn *) &dCollideCylPlane;
  else if (num == dRayClass) return (dColliderFn *) &dCollideCylRay;
  return 0;
}


static  void dCylinderAABB (dxGeom *geom, dReal aabb[6])
{
  dReal radius,lz;
  dGeomCylinderGetParams(geom,&radius,&lz);
const dReal* R= dGeomGetRotation(geom);
const dReal* pos= dGeomGetPosition(geom);
  dReal xrange =  dFabs (R[0] *radius) +
    REAL(0.5) *dFabs (R[1] * lz) + dFabs (R[2] * radius);

  dReal yrange = dFabs (R[4] *radius) +
    REAL(0.5) * dFabs (R[5] * lz) + dFabs (R[6] * radius);

  dReal zrange =  dFabs (R[8] * radius) +
    REAL(0.5) *dFabs (R[9] * lz) + dFabs (R[10] * radius);

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

dxGeom *dCreateCylinder (dSpaceID space, dReal r, dReal lz)
{
 dAASSERT (r > 0 && lz > 0);
 if (dCylinderClassUser == -1)
  {
    dGeomClass c;
    c.bytes = sizeof (dxCylinder);
    c.collider = &dCylinderColliderFn;
    c.aabb = &dCylinderAABB;
    c.aabb_test = 0;
    c.dtor = 0;
    dCylinderClassUser=dCreateGeomClass (&c);

  }

  dGeomID g = dCreateGeom (dCylinderClassUser);
  if (space) dSpaceAdd (space,g);
  dxCylinder *c = (dxCylinder*) dGeomGetClassData(g);

  c->radius = r;
  c->lz = lz;
  return g;
}



void dGeomCylinderSetParams (dGeomID g, dReal radius, dReal length)
{
  dUASSERT (g && dGeomGetClass(g) == dCylinderClassUser,"argument not a cylinder");
  dAASSERT (radius > 0 && length > 0);
  dxCylinder *c = (dxCylinder*) dGeomGetClassData(g);
  c->radius = radius;
  c->lz = length;
}



void dGeomCylinderGetParams (dGeomID g, dReal *radius, dReal *length)
{
  dUASSERT (g && dGeomGetClass(g) == dCylinderClassUser ,"argument not a cylinder");
  dxCylinder *c = (dxCylinder*) dGeomGetClassData(g);
  *radius = c->radius;
  *length = c->lz;
}

/*
void dMassSetCylinder (dMass *m, dReal density,
		  dReal radius, dReal length)
{
  dAASSERT (m);
  dMassSetZero (m);
  dReal M = length*M_PI*radius*radius*density;
  m->mass = M;
  m->_I(0,0) = M/REAL(4.0) * (ly*ly + lz*lz);
  m->_I(1,1) = M/REAL(12.0) * (lx*lx + lz*lz);
  m->_I(2,2) = M/REAL(4.0) * (lx*lx + ly*ly);

# ifndef dNODEBUG
  checkMass (m);
# endif
}
*/