dcylinder.cc

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

    // find vertex of the box  deepest along normal 
    dVector3 pb;
    for (i=0; i<3; i++) pb[i] = p2[i];
    sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
    for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4];
    sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
    for (i=0; i<3; i++) pb[i] += sign * B2 * R2[i*4+1];
    sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
    for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2];


    dReal alpha,beta;
    dVector3 ua,ub;
    for (i=0; i<3; i++) ua[i] = R1[1 + i*4];
    for (i=0; i<3; i++) ub[i] = R2[*code-8 + i*4];

    lineClosestApproach (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;

    for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
    contact[0].depth = *depth;
    return 1;
  }


  	if(*code==4){
		for (i=0; i<3; i++) contact[0].pos[i] = pb[i];
		contact[0].depth = *depth;
		return 1;
				}
  

  dVector3 vertex;
  if (*code == 0) {
   
    dReal sign;
    for (i=0; i<3; i++) vertex[i] = p2[i];
    sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
    for (i=0; i<3; i++) vertex[i] += sign * B1 * R2[i*4];
    sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
    for (i=0; i<3; i++) vertex[i] += sign * B2 * R2[i*4+1];
    sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
    for (i=0; i<3; i++) vertex[i] += sign * B3 * R2[i*4+2];
  }
  else {
   
    dReal sign,cos1,cos3,factor;
    for (i=0; i<3; i++) vertex[i] = p1[i];
    cos1 = dDOT14(normal,R1+0) ;
	cos3 = dDOT14(normal,R1+2);
	factor=sqrtf(cos1*cos1+cos3*cos3);
	factor= factor ? factor : 1.f;
	cos1/=factor;
	cos3/=factor;
    for (i=0; i<3; i++) vertex[i] += cos1 * radius * R1[i*4];

    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
    for (i=0; i<3; i++) vertex[i] += sign * hlz * R1[i*4+1];
   
    for (i=0; i<3; i++) vertex[i] += cos3 * radius * R1[i*4+2];
  }
  for (i=0; i<3; i++) contact[0].pos[i] = vertex[i];
  contact[0].depth = *depth;
  return 1;
}

//****************************************************************************

extern "C" int dCylCyl (const dVector3 p1, const dMatrix3 R1,
			const dReal radius1,const dReal lz1, const dVector3 p2,
			const dMatrix3 R2, const dReal radius2,const dReal lz2,
			dVector3 normal, dReal *depth, int *code,
			int maxc, dContactGeom *contact, int skip)
{
  dVector3 p,pp1,pp2,normalC;
  const dReal *normalR = 0;
  dReal hlz1,hlz2,s,s2;
  int i,invert_normal;

  // 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 (pp1,R1,p);		// get pp1 = p relative to body 1
  dMULTIPLY1_331 (pp2,R2,p);
  // get side lengths / 2
  hlz1 = lz1*REAL(0.5);
  hlz2 = lz2*REAL(0.5); 

 dReal proj,cos,sin,cos1,cos3;



#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;

  cos=dFabs(dDOT44(R1+1,R2+1));
  sin=sqrtf(1.f-(cos>1.f ? 1.f : cos));

  TEST (pp1[1],(hlz1 + radius2*sin + hlz2*cos ),R1+1,0);//pp

  TEST (pp2[1],(radius1*sin + hlz1*cos + hlz2),R2+1,1);



  // note: cross product axes need to be scaled when s is computed.
 
#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); \
    } 
 

dVector3 tAx,Ax,pa,pb;

//cross between cylinders' axes
dCROSS144(Ax,=,R1+1,R2+1);
dNormalize3(Ax);
TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius1+radius2,Ax[0],Ax[1],Ax[2],6);


{
 
    dReal sign, factor;

	//making ax which is perpendicular to cyl1 ax passing across cyl2 position//
		//(project p on cyl1 flat surface )
    for (i=0; i<3; i++) pb[i] = p2[i];
 	//cos1 = dDOT14(p,R1+0);
	//cos3 = dDOT14(p,R1+2) ;
	tAx[0]=pp1[0]*R1[0]+pp1[2]*R1[2];
	tAx[1]=pp1[0]*R1[4]+pp1[2]*R1[6];
	tAx[2]=pp1[0]*R1[8]+pp1[2]*R1[10];
	dNormalize3(tAx);

//find deepest point pb of cyl2 on opposite direction of tAx
 	cos1 = dDOT14(tAx,R2+0);
	cos3 = dDOT14(tAx,R2+2) ;
	factor=sqrtf(cos1*cos1+cos3*cos3);
	cos1/=factor;
	cos3/=factor;
    for (i=0; i<3; i++) pb[i] -= cos1 * radius2 * R2[i*4];

    sign = (dDOT14(tAx,R2+1) > 0) ? REAL(1.0) : REAL(-1.0);
    for (i=0; i<3; i++) pb[i] -= sign * hlz2 * R2[i*4+1];

    for (i=0; i<3; i++) pb[i] -= cos3 * radius2 * R2[i*4+2];

//making perpendicular to cyl1 ax passing across pb
	proj=dDOT14(pb,R1+1)-dDOT14(p1,R1+1);

	Ax[0]=pb[0]-p1[0]-R1[1]*proj;
	Ax[1]=pb[1]-p1[1]-R1[5]*proj;
	Ax[2]=pb[2]-p1[2]-R1[9]*proj;

}

dNormalize3(Ax);


  cos=dFabs(dDOT14(Ax,R2+1));
  cos1=dDOT14(Ax,R2+0);
  cos3=dDOT14(Ax,R2+2);
  sin=sqrtf(cos1*cos1+cos3*cos3);

TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius1+cos*hlz2+sin*radius2,Ax[0],Ax[1],Ax[2],3);



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

 	//making ax which is perpendicular to cyl2 ax passing across cyl1 position//
	//(project p on cyl2 flat surface )
 	//cos1 = dDOT14(p,R2+0);
	//cos3 = dDOT14(p,R2+2) ;
	tAx[0]=pp2[0]*R2[0]+pp2[2]*R2[2];
	tAx[1]=pp2[0]*R2[4]+pp2[2]*R2[6];
	tAx[2]=pp2[0]*R2[8]+pp2[2]*R2[10];
	dNormalize3(tAx);

 	cos1 = dDOT14(tAx,R1+0);
	cos3 = dDOT14(tAx,R1+2) ;
	factor=sqrtf(cos1*cos1+cos3*cos3);
	cos1/=factor;
	cos3/=factor;

//find deepest point pa of cyl2 on direction of tAx
    for (i=0; i<3; i++) pa[i] += cos1 * radius1 * R1[i*4];

    sign = (dDOT14(tAx,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
    for (i=0; i<3; i++) pa[i] += sign * hlz1 * R1[i*4+1];

  
    for (i=0; i<3; i++) pa[i] += cos3 * radius1 * R1[i*4+2];

	proj=dDOT14(pa,R2+1)-dDOT14(p2,R2+1);

	Ax[0]=pa[0]-p2[0]-R2[1]*proj;
	Ax[1]=pa[1]-p2[1]-R2[5]*proj;
	Ax[2]=pa[2]-p2[2]-R2[9]*proj;

}
dNormalize3(Ax);



  cos=dFabs(dDOT14(Ax,R1+1));
  cos1=dDOT14(Ax,R1+0);
  cos3=dDOT14(Ax,R1+2);
  sin=sqrtf(cos1*cos1+cos3*cos3);

TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius2+cos*hlz1+sin*radius1,Ax[0],Ax[1],Ax[2],4);


////test circl

//@ this needed to set right normal when cylinders edges intersect
//@ the most precise axis for this test may be found as a line between nearest points of two
//@ circles. But it needs comparatively a lot of computation.
//@ I use a trick which lets not to solve quadric equation. 
//@ In the case when cylinder eidges touches the test below rather accurate.
//@ I still not sure about problems with sepparation but they have not been revealed during testing.
dVector3 point;
{
 dVector3 ca,cb; 
 dReal sign;
 for (i=0; i<3; i++) ca[i] = p1[i];
 for (i=0; i<3; i++) cb[i] = p2[i];
//find two nearest flat rings
 sign = (pp1[1] > 0) ? REAL(1.0) : REAL(-1.0);
 for (i=0; i<3; i++) ca[i] += sign * hlz1 * R1[i*4+1];

 sign = (pp2[1] > 0) ? REAL(1.0) : REAL(-1.0);
 for (i=0; i<3; i++) cb[i] -= sign * hlz2 * R2[i*4+1];

 dVector3 tAx,tAx1;
	circleIntersection(R1+1,ca,radius1,R2+1,cb,radius2,point);

	Ax[0]=point[0]-ca[0];
	Ax[1]=point[1]-ca[1];
	Ax[2]=point[2]-ca[2];

  	cos1 = dDOT14(Ax,R1+0);
	cos3 = dDOT14(Ax,R1+2) ;

	tAx[0]=cos3*R1[0]-cos1*R1[2];
	tAx[1]=cos3*R1[4]-cos1*R1[6];
	tAx[2]=cos3*R1[8]-cos1*R1[10];

	Ax[0]=point[0]-cb[0];
	Ax[1]=point[1]-cb[1];
	Ax[2]=point[2]-cb[2];


 	cos1 = dDOT14(Ax,R2+0);
	cos3 = dDOT14(Ax,R2+2) ;

	tAx1[0]=cos3*R2[0]-cos1*R2[2];
	tAx1[1]=cos3*R2[4]-cos1*R2[6];
	tAx1[2]=cos3*R2[8]-cos1*R2[10];
	dCROSS(Ax,=,tAx,tAx1);
	

 

dNormalize3(Ax);
dReal cyl1Pr,cyl2Pr;

 cos=dFabs(dDOT14(Ax,R1+1));
 cos1=dDOT14(Ax,R1+0);
 cos3=dDOT14(Ax,R1+2);
 sin=sqrtf(cos1*cos1+cos3*cos3);
 cyl1Pr=cos*hlz1+sin*radius1;

 cos=dFabs(dDOT14(Ax,R2+1));
 cos1=dDOT14(Ax,R2+0);
 cos3=dDOT14(Ax,R2+2);
 sin=sqrtf(cos1*cos1+cos3*cos3);
 cyl2Pr=cos*hlz2+sin*radius2;
TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],cyl1Pr+cyl2Pr,Ax[0],Ax[1],Ax[2],5);


}


#undef TEST



  // if we get to this point, the cylinders interpenetrate. compute the normal
  // in global coordinates.
  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];
  }

  *depth = -s;

  // compute contact point(s)

	if(*code==3){
		for (i=0; i<3; i++) contact[0].pos[i] = pb[i];
		contact[0].depth = *depth;
		return 1;
				}

	if(*code==4){
		for (i=0; i<3; i++) contact[0].pos[i] = pa[i];
		contact[0].depth = *depth;
		return 1;
				}

	if(*code==5){
		for (i=0; i<3; i++) contact[0].pos[i] = point[i];
		contact[0].depth = *depth;
		return 1;
				}

if (*code == 6) {
	    dVector3 pa;
    dReal sign, cos1,cos3,factor;


    for (i=0; i<3; i++) pa[i] = p1[i];

  	cos1 = dDOT14(normal,R1+0);
	cos3 = dDOT14(normal,R1+2) ;
	factor=sqrtf(cos1*cos1+cos3*cos3);

	cos1/=factor;
	cos3/=factor;
	
    for (i=0; i<3; i++) pa[i] += cos1 * radius1 * R1[i*4];

    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
    for (i=0; i<3; i++) pa[i] += sign * hlz1 * R1[i*4+1];

  
    for (i=0; i<3; i++) pa[i] += cos3 * radius1 * R1[i*4+2];

    // find a point pb on the intersecting edge of cylinder 2
    dVector3 pb;
    for (i=0; i<3; i++) pb[i] = p2[i];
 	cos1 = dDOT14(normal,R2+0);
	cos3 = dDOT14(normal,R2+2) ;
	factor=sqrtf(cos1*cos1+cos3*cos3);

	cos1/=factor;
	cos3/=factor;
	
    for (i=0; i<3; i++) pb[i] -= cos1 * radius2 * R2[i*4];

    sign = (dDOT14(normal,R2+1) > 0) ? REAL(1.0) : REAL(-1.0);
    for (i=0; i<3; i++) pb[i] -= sign * hlz2 * R2[i*4+1];

  
    for (i=0; i<3; i++) pb[i] -= cos3 * radius2 * R2[i*4+2];

	
	dReal alpha,beta;
	dVector3 ua,ub;
	for (i=0; i<3; i++) ua[i] = R1[1 + i*4];
	for (i=0; i<3; i++) ub[i] = R2[1 + i*4];
	lineClosestApproach (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;

    for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
    contact[0].depth = *depth;
    return 1;
  }

  // okay, we have a face-something intersection (because the separating
  // axis is perpendicular to a face).

  // @@@ temporary: make deepest point on the "other" cylinder the contact point.
  // @@@ this kind of works, but we need multiple contact points for stability,
  // @@@ especially for face-face contact.

  dVector3 vertex;
  if (*code == 0) {
    // flat face from cylinder 1 touches a edge/face from cylinder 2.
    dReal sign,cos1,cos3,factor;
    for (i=0; i<3; i++) vertex[i] = p2[i];
    cos1 = dDOT14(normal,R2+0) ;
	cos3 = dDOT14(normal,R2+2);
	factor=sqrtf(cos1*cos1+cos3*cos3);

	cos1/=factor;
	cos3/=factor;
    for (i=0; i<3; i++) vertex[i] -= cos1 * radius2 * R2[i*4];

    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
    for (i=0; i<3; i++) vertex[i] -= sign * hlz2 * R2[i*4+1];
   
    for (i=0; i<3; i++) vertex[i] -= cos3 * radius2 * R2[i*4+2];
  }
  else {
     // flat face from cylinder 2 touches a edge/face from cylinder 1.
    dReal sign,cos1,cos3,factor;
    for (i=0; i<3; i++) vertex[i] = p1[i];
    cos1 = dDOT14(normal,R1+0) ;
	cos3 = dDOT14(normal,R1+2);
	factor=sqrtf(cos1*cos1+cos3*cos3);

	cos1/=factor;
	cos3/=factor;
    for (i=0; i<3; i++) vertex[i] += cos1 * radius1 * R1[i*4];

    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
    for (i=0; i<3; i++) vertex[i] += sign * hlz1 * R1[i*4+1];
   
    for (i=0; i<3; i++) vertex[i] += cos3 * radius1 * R1[i*4+2];
  }
  for (i=0; i<3; i++) contact[0].pos[i] = vertex[i];
  contact[0].depth = *depth;
  return 1;
}

//****************************************************************************


int dCollideCylS (dxGeom *o1, dxGeom *o2, int flags,
		dContactGeom *contact, int skip)
{
 

  dIASSERT (skip >= (int)sizeof(dContactGeom));
  dIASSERT (dGeomGetClass(o2) == dSphereClass);
  dIASSERT (dGeomGetClass(o1) == dCylinderClassUser);
  const dReal* p1=dGeomGetPosition(o1);
  const dReal* p2=dGeomGetPosition(o2);
  const dReal* R=dGeomGetRotation(o1);
  dVector3 p,normalC,normal;
  const dReal *normalR = 0;
  dReal cylRadius;
  dReal hl;
  dGeomCylinderGetParams(o1,&cylRadius,&hl);
  dReal sphereRadius;
  sphereRadius=dGeomSphereGetRadius(o2);
  
  int i,invert_normal;

  // get vector from centers of cyl to shere
  p[0] = p2[0] - p1[0];
  p[1] = p2[1] - p1[1];
  p[2] = p2[2] - p1[2];
 
dReal s,s2;
unsigned char code;