dcylinder.cc

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

#include <ode/ode.h>
#include "dCylinder.h"
// given a pointer `p' to a dContactGeom, return the dContactGeom at
// p + skip bytes.

#define sqrtf sqrt

struct dxCylinder {	// cylinder
  dReal radius,lz;	// radius, length along y axis //
};

int dCylinderClassUser = -1;

#define NUMC_MASK (0xffff)

#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))

/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////circleIntersection//////////////////////////////////////////////////
//this does following:
//takes two circles as normals to planes n1,n2, center points cp1,cp2,and radiuses r1,r2
//finds line on which circles' planes intersect
//finds four points O1,O2 - intersection between the line and sphere with center cp1 radius r1
//					O3,O4 - intersection between the line and sphere with center cp2 radius r2
//returns false if there is no intersection
//computes distances O1-O3, O1-O4, O2-O3, O2-O4
//in "point" returns mean point between intersection points with smallest distance
/////////////////////////////////////////////////////////////////////////////////////////////////
inline bool circleIntersection(const dReal* n1,const dReal* cp1,dReal r1,const dReal* n2,const dReal* cp2,dReal r2,dVector3 point){
dReal c1=dDOT14(cp1,n1);
dReal c2=dDOT14(cp2,n2);
dReal cos=dDOT44(n1,n2);
dReal cos_2=cos*cos;
dReal sin_2=1-cos_2;
dReal p1=(c1-c2*cos)/sin_2;
dReal p2=(c2-c1*cos)/sin_2;
dVector3 lp={p1*n1[0]+p2*n2[0],p1*n1[4]+p2*n2[4],p1*n1[8]+p2*n2[8]};
dVector3 n;
dCROSS144(n,=,n1,n2);
dVector3 LC1={lp[0]-cp1[0],lp[1]-cp1[1],lp[2]-cp1[2]};
dVector3 LC2={lp[0]-cp2[0],lp[1]-cp2[1],lp[2]-cp2[2]};
dReal A,B,C,B_A,B_A_2,D;
dReal t1,t2,t3,t4;
A=dDOT(n,n);
B=dDOT(LC1,n);
C=dDOT(LC1,LC1)-r1*r1;
B_A=B/A;
B_A_2=B_A*B_A;
D=B_A_2-C;
if(D<0.f){	//somewhat strange solution 
			//- it is needed to set some 
			//axis to sepparate cylinders
			//when their edges approach
	t1=-B_A+sqrtf(-D);
	t2=-B_A-sqrtf(-D);
//	return false;
	}
else{
t1=-B_A-sqrtf(D);
t2=-B_A+sqrtf(D);
}
B=dDOT(LC2,n);
C=dDOT(LC2,LC2)-r2*r2;
B_A=B/A;
B_A_2=B_A*B_A;
D=B_A_2-C;

if(D<0.f) {
	t3=-B_A+sqrtf(-D);
	t4=-B_A-sqrtf(-D);
//	return false;
	}
else{
t3=-B_A-sqrtf(D);
t4=-B_A+sqrtf(D);
}
dVector3 O1={lp[0]+n[0]*t1,lp[1]+n[1]*t1,lp[2]+n[2]*t1};
dVector3 O2={lp[0]+n[0]*t2,lp[1]+n[1]*t2,lp[2]+n[2]*t2};

dVector3 O3={lp[0]+n[0]*t3,lp[1]+n[1]*t3,lp[2]+n[2]*t3};
dVector3 O4={lp[0]+n[0]*t4,lp[1]+n[1]*t4,lp[2]+n[2]*t4};

dVector3 L1_3={O3[0]-O1[0],O3[1]-O1[1],O3[2]-O1[2]};
dVector3 L1_4={O4[0]-O1[0],O4[1]-O1[1],O4[2]-O1[2]};

dVector3 L2_3={O3[0]-O2[0],O3[1]-O2[1],O3[2]-O2[2]};
dVector3 L2_4={O4[0]-O2[0],O4[1]-O2[1],O4[2]-O2[2]};


dReal l1_3=dDOT(L1_3,L1_3);
dReal l1_4=dDOT(L1_4,L1_4);

dReal l2_3=dDOT(L2_3,L2_3);
dReal l2_4=dDOT(L2_4,L2_4);


if (l1_3<l1_4)
	if(l2_3<l2_4)
		if(l1_3<l2_3)
			{
			//l1_3;
			point[0]=0.5f*(O1[0]+O3[0]);
			point[1]=0.5f*(O1[1]+O3[1]);
			point[2]=0.5f*(O1[2]+O3[2]);
			}
		else{
			//l2_3;
			point[0]=0.5f*(O2[0]+O3[0]);
			point[1]=0.5f*(O2[1]+O3[1]);
			point[2]=0.5f*(O2[2]+O3[2]);
			}
	else
		if(l1_3<l2_4)
			{
			//l1_3;
			point[0]=0.5f*(O1[0]+O3[0]);
			point[1]=0.5f*(O1[1]+O3[1]);
			point[2]=0.5f*(O1[2]+O3[2]);
			}
		else{
			//l2_4;
			point[0]=0.5f*(O2[0]+O4[0]);
			point[1]=0.5f*(O2[1]+O4[1]);
			point[2]=0.5f*(O2[2]+O4[2]);
			}

else
	if(l2_3<l2_4)
		if(l1_4<l2_3)
			{
			//l1_4;
			point[0]=0.5f*(O1[0]+O4[0]);
			point[1]=0.5f*(O1[1]+O4[1]);
			point[2]=0.5f*(O1[2]+O4[2]);
			}
		else{
			//l2_3;
			point[0]=0.5f*(O2[0]+O3[0]);
			point[1]=0.5f*(O2[1]+O3[1]);
			point[2]=0.5f*(O2[2]+O3[2]);
			}
	else
		if(l1_4<l2_4)
			{
			//l1_4;
			point[0]=0.5f*(O1[0]+O4[0]);
			point[1]=0.5f*(O1[1]+O4[1]);
			point[2]=0.5f*(O1[2]+O4[2]);
			}
		else{
			//l2_4;
			point[0]=0.5f*(O2[0]+O4[0]);
			point[1]=0.5f*(O2[1]+O4[1]);
			point[2]=0.5f*(O2[2]+O4[2]);
			}

return true;
}




void lineClosestApproach (const dVector3 pa, const dVector3 ua,
				 const dVector3 pb, const dVector3 ub,
				 dReal *alpha, dReal *beta)
{
  dVector3 p;
  p[0] = pb[0] - pa[0];
  p[1] = pb[1] - pa[1];
  p[2] = pb[2] - pa[2];
  dReal uaub = dDOT(ua,ub);
  dReal q1 =  dDOT(ua,p);
  dReal q2 = -dDOT(ub,p);
  dReal d = 1-uaub*uaub;
  if (d <= 0) {
    // @@@ this needs to be made more robust
    *alpha = 0;
    *beta  = 0;
  }
  else {
    d = dRecip(d);
    *alpha = (q1 + uaub*q2)*d;
    *beta  = (uaub*q1 + q2)*d;
  }
}


// @@@ some stuff to optimize here, reuse code in contact point calculations.

extern "C" int dCylBox (const dVector3 p1, const dMatrix3 R1,
			const dReal radius,const dReal lz, const dVector3 p2,
			const dMatrix3 R2, const dVector3 side2,
			dVector3 normal, dReal *depth, int *code,
			int maxc, dContactGeom *contact, int skip)
{
  dVector3 p,pp,normalC;
  const dReal *normalR = 0;
  dReal B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,sQ21,sQ22,sQ23;
  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 (pp,R1,p);		// get pp = p relative to body 1

  // get side lengths / 2
  //A1 =radius; A2 = lz*REAL(0.5); A3 = radius;
  dReal hlz=lz/2.f;
  B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5);

  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(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);

  
  //   * see if the axis separates the box with cylinder. 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.

#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 = cylinder ax u2
 //used when a box vertex touches a flat face of the cylinder
  TEST (pp[1],(hlz + B1*Q21 + B2*Q22 + B3*Q23),R1+1,0);


  // separating axis = box axis v1,v2,v3
  //used when cylinder edge touches box face
  //there is two ways to compute sQ: sQ21=sqrtf(1.f-Q21*Q21); or sQ21=sqrtf(Q23*Q23+Q22*Q22); 
  //if we did not need Q23 and Q22 the first way might be used to quiken the routine but then it need to 
  //check if Q21<=1.f, becouse it may slightly exeed 1.f.

 
  sQ21=sqrtf(Q23*Q23+Q22*Q22);
  TEST (dDOT41(R2+0,p),(radius*sQ21 + hlz*Q21 + B1),R2+0,1);

  sQ22=sqrtf(Q23*Q23+Q21*Q21);
  TEST (dDOT41(R2+1,p),(radius*sQ22 + hlz*Q22 + B2),R2+1,2);

  sQ23=sqrtf(Q22*Q22+Q21*Q21);
  TEST (dDOT41(R2+2,p),(radius*sQ23 + hlz*Q23 + B3),R2+2,3);

 
#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); \
    } 
 


// separating axis is a normal to the cylinder axis passing across the nearest box vertex
//used when a box vertex touches the lateral surface of the cylinder

dReal proj,boxProj,cos,sin,cos1,cos3;
dVector3 tAx,Ax,pb;
{
//making Ax which is perpendicular to cyl ax to box position//
proj=dDOT14(p2,R1+1)-dDOT14(p1,R1+1);

Ax[0]=p2[0]-p1[0]-R1[1]*proj;
Ax[1]=p2[1]-p1[1]-R1[5]*proj;
Ax[2]=p2[2]-p1[2]-R1[9]*proj;
dNormalize3(Ax);
//using Ax find box vertex which is nearest to the cylinder axis
	dReal sign;
    
    for (i=0; i<3; i++) pb[i] = p2[i];
    sign = (dDOT14(Ax,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
    for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4];
    sign = (dDOT14(Ax,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(Ax,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
    for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2];

//building axis which is normal to cylinder ax to the nearest box vertex
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);
}

boxProj=dFabs(dDOT14(Ax,R2+0)*B1)+
		dFabs(dDOT14(Ax,R2+1)*B2)+
		dFabs(dDOT14(Ax,R2+2)*B3);

TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(radius+boxProj),Ax[0],Ax[1],Ax[2],4);


//next three test used to handle collisions between cylinder circles and box ages
proj=dDOT14(p1,R2+0)-dDOT14(p2,R2+0);

tAx[0]=-p1[0]+p2[0]+R2[0]*proj;
tAx[1]=-p1[1]+p2[1]+R2[4]*proj;
tAx[2]=-p1[2]+p2[2]+R2[8]*proj;
dNormalize3(tAx);

//now tAx is normal to first ax of the box to cylinder center
//making perpendicular to tAx lying in the plane which is normal to the cylinder axis
//it is tangent in the point where projection of tAx on cylinder's ring intersect edge circle

cos=dDOT14(tAx,R1+0);
sin=dDOT14(tAx,R1+2);
tAx[0]=R1[2]*cos-R1[0]*sin;
tAx[1]=R1[6]*cos-R1[4]*sin;
tAx[2]=R1[10]*cos-R1[8]*sin;


//use cross between tAx and first ax of the box as separating axix 

dCROSS114(Ax,=,tAx,R2+0);
dNormalize3(Ax);

boxProj=dFabs(dDOT14(Ax,R2+1)*B2)+
		dFabs(dDOT14(Ax,R2+0)*B1)+
		dFabs(dDOT14(Ax,R2+2)*B3);

  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],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],5);


//same thing with the second axis of the box
proj=dDOT14(p1,R2+1)-dDOT14(p2,R2+1);

tAx[0]=-p1[0]+p2[0]+R2[1]*proj;
tAx[1]=-p1[1]+p2[1]+R2[5]*proj;
tAx[2]=-p1[2]+p2[2]+R2[9]*proj;
dNormalize3(tAx);


cos=dDOT14(tAx,R1+0);
sin=dDOT14(tAx,R1+2);
tAx[0]=R1[2]*cos-R1[0]*sin;
tAx[1]=R1[6]*cos-R1[4]*sin;
tAx[2]=R1[10]*cos-R1[8]*sin;

dCROSS114(Ax,=,tAx,R2+1);
dNormalize3(Ax);

boxProj=dFabs(dDOT14(Ax,R2+0)*B1)+
		dFabs(dDOT14(Ax,R2+1)*B2)+
		dFabs(dDOT14(Ax,R2+2)*B3);

  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],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],6);

//same thing with the third axis of the box
proj=dDOT14(p1,R2+2)-dDOT14(p2,R2+2);

Ax[0]=-p1[0]+p2[0]+R2[2]*proj;
Ax[1]=-p1[1]+p2[1]+R2[6]*proj;
Ax[2]=-p1[2]+p2[2]+R2[10]*proj;
dNormalize3(tAx);

cos=dDOT14(tAx,R1+0);
sin=dDOT14(tAx,R1+2);
tAx[0]=R1[2]*cos-R1[0]*sin;
tAx[1]=R1[6]*cos-R1[4]*sin;
tAx[2]=R1[10]*cos-R1[8]*sin;

dCROSS114(Ax,=,tAx,R2+2);
dNormalize3(Ax);
boxProj=dFabs(dDOT14(Ax,R2+1)*B2)+
		dFabs(dDOT14(Ax,R2+2)*B3)+
		dFabs(dDOT14(Ax,R2+0)*B1);

  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],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],7);


#undef TEST

// note: cross product axes need to be scaled when s is computed.
// normal (n1,n2,n3) is relative to box 1.

#define TEST(expr1,expr2,n1,n2,n3,cc) \
  s2 = dFabs(expr1) - (expr2); \
  if (s2 > 0) return 0; \
  l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
  if (l > 0) { \
    s2 /= l; \
    if (s2 > s) { \
      s = s2; \
      normalR = 0; \
      normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
      invert_normal = ((expr1) < 0); \
      *code = (cc); \
    } \
  }

//crosses between cylinder axis and box axes
  // separating axis = u2 x (v1,v2,v3)
  TEST(pp[0]*R31-pp[2]*R11,(radius+B2*Q23+B3*Q22),R31,0,-R11,8);
  TEST(pp[0]*R32-pp[2]*R12,(radius+B1*Q23+B3*Q21),R32,0,-R12,9);
  TEST(pp[0]*R33-pp[2]*R13,(radius+B1*Q22+B2*Q21),R33,0,-R13,10);


#undef TEST

  // 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 {
	  if(*code>7) dMULTIPLY0_331 (normal,R1,normalC);
	  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 > 7) {
 //find point on the cylinder pa deepest along normal
    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 * radius * R1[i*4];

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

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