/*! Calculates the angle betwen the main radius curvature on body1 and the
main radius of curvature on body 2.
*/
int SoftContact::CalcRelPhi()
{
vec3 temp, t;
vec3 R11, R12; //directions of relative curvatures in world frame
temp.x() = 1;
temp.y() = 0.0;
temp.z() = 0.0;
t = fitRot * temp;
R11 = frame.affine() * (t);
R11 = body1->getTran().affine() * (R11);
temp.x() = 1;
temp.y() = 0.0;
temp.z() = 0.0;
t = ((SoftContact *)getMate())->fitRot * temp;
R12 = ((SoftContact *)getMate())->frame.affine() * (t);
R12 = body2->getTran().affine() * (R12);
relPhi = acos((R11.dot(R12)) / (R11.norm() * R12.norm()));
((SoftContact *)getMate())->relPhi = relPhi ;
//fprintf( stderr, "Rel Phi %f", relPhi );
return 0;
}
/*! Sets up friction edges as a 3D friction ellipsoid. All the computations for
fitting analytical surfaces to the two bodies should already have been
completed.
*/
int SoftContact::setUpFrictionEdges(bool dynamicsOn )
{
if( !getMate() ) {
fprintf(stderr,"Trying to set up friction edges for a contact with no mate!!\n");
return 1;
}
DBGP("Setting up SOFT contact friction edges");
double eccen[3];
// magnitude of tangential friction
eccen[0]=1;
eccen[1]=1;
// magnitude of max frictional torque
double torquedivN;
// relative radii of curvature at the contact
CalcRprimes();
if (!dynamicsOn) {
// if dynamics are not on, compute it based on some random applied force
torquedivN = CalcContact_Mattress( 5 );
eccen[2] = torquedivN*1000;
} else {
// if dynamics are on, compute it based on the applied force reported by the LCP
double nForce = dynamicForce[2];
//need to figure out what all the numbers in the dynamic force mean
//I think it is a wrench, and hope it is in the contacts coordinate frame
//and that the units are in newtons, chekc dynamics.cpp iterateDynamics where
//it is created
torquedivN = CalcContact_Mattress( nForce );
eccen[2] = torquedivN*1000;
}
//various possible approximations for the friction ellipsoid
/*
int numDirs[9] = {1,3,5,7,8,7,5,3,1};
double phi[9] = {M_PI_2, M_PI_2*0.6, M_PI_2*0.3, M_PI_2*0.15, 0.0,
-M_PI_2*0.15, -M_PI_2*0.3, -M_PI_2*0.6, -M_PI_2};
return Contact::setUpFrictionEdges(9,numDirs,phi,eccen);
*/
/*
int numDirs[9] = {1,8,8,8,8,8,8,8,1};
double phi[9] = {M_PI_2, M_PI_2*0.66, M_PI_2*0.33, M_PI_2*0.165, 0.0,
-M_PI_2*0.165, -M_PI_2*0.33, -M_PI_2*0.66, -M_PI_2};
return Contact::setUpFrictionEdges(9,numDirs,phi,eccen);
*/
int numDirs[5] = {1,5,8,5,1};
double phi[5] = {M_PI_2, M_PI_2*0.50, 0.0, -M_PI_2*0.50, -M_PI_2};
return Contact::setUpFrictionEllipsoid( 5, numDirs, phi, eccen );
}
/*! Calculates the angle betwen the main radius curvature on body1 and the
main radius of curvature on body 2.
*/
int SoftContact::CalcRelPhi( )
{
vec3 temp, t;
vec3 R11, R12; //directions of relative curvatures in world frame
temp.set( 1, 0 , 0);
t = fitRot * temp;
R11 = t * frame;
R11 = R11 * body1->getTran();
temp.set( 1, 0 , 0);
t = ((SoftContact *)getMate())->fitRot * temp;
R12 = t * ((SoftContact *)getMate())->frame;
R12 = R12 * body2->getTran();
relPhi = acos( (R11%R12)/(R11.len()*R12.len()) );
((SoftContact *)getMate())->relPhi = relPhi ;
//fprintf( stderr, "Rel Phi %f", relPhi );
return 0;
}
/*! Calculates the axes of the ellipse of contact using the mattress
model and a given normal force (for dynamics its the normal component
of the dynamic contact force). Sets major axis and minor axis.
Returns the max torque available.
See Contact Mechanics, KL Johnson Chapter 4
*/
double SoftContact::CalcContact_Mattress( double nForce )
{
if (r1prime < 0) {
DBGP("Degenerate soft contact");
r1prime = 20;
}
if (r2prime < 0) {
DBGP("Degenerate soft contact");
r2prime = 20;
}
//hardwired height of mattress = 3.0 mm
//r primes are in mm
double h = 0.003;
double delta = sqrt( nForce*h/(MAX(body1->getYoungs(), body2->getYoungs())
* M_PI * sqrt(r1prime*0.001*r2prime*0.001)) ); //meters
majorAxis = sqrt( 2*delta*r1prime*0.001 ); //meters
minorAxis = sqrt( 2*delta*r2prime*0.001 );
DBGP("Axes: " << majorAxis << " " << minorAxis);
//axes are given in meters!!!!
//rprimes and the radii of curvature are calculated in mm
SoftContact *m = (SoftContact *)getMate();
m->majorAxis = majorAxis;
m->minorAxis = minorAxis;
//returns max available torque proportional to given normal force
//max torque=*K/h*mu*4*pi*(ab)^1.5/15 where a and b are
//the axis of the contact ellipse
//this just returns the max torque divided by the normal force times mu
//which is 8sqrt(ab)/15
//the pressure distrib is K*delta/h/2*pi*ab
//std::cerr<<"Ma ma "<< majorAxis << minorAxis << "\n";
return 8*sqrt( majorAxis*minorAxis )/15; //in meters
}
/*! Calculates the relative radii of curvature of the contact, using the
radii of curvature of both bodies and the angle between them.
See Johnson, Contact Mechanics Chapter 4 page 85 for calculations.
*/
int SoftContact::CalcRprimes()
{
if( !(getMate()) ) {
DBGA("Contact doesn't have mate, not calculating curvature...");
return 1;
}
SoftContact *m = (SoftContact *)getMate();
CalcRelPhi();
DBGP("Body 1" << getBody1()->getName().latin1() << " Body 2 " << m->getBody1()->getName().latin1());
//the less than zero curvature is for flat objects
//1/r goes to zero for flat objects because the curvature
//is infinite
double x,y,w,z;
if( r1 < 0.0 )
w = 0.0;
else
w = 1/r1;
if( r2 < 0.0 )
x = 0.0;
else
x = 1/r2;
if( m->r1 < 0.0 )
y = 0.0;
else
y = 1/m->r1;
if( m->r2 < 0.0 )
z = 0.0;
else
z = 1/m->r2;
DBGP("x: " << x << " y: " << y << " w: " << w << " z: " << z);
double ApB = 0.5*( w + x + y + z );
DBGP("Apb: " << ApB);
double AmB = 0.5*sqrt( (w - x)*(w - x) + (y - z)*(y - z) +
2*cos(2*relPhi)*(w - x)*(y - z) );
DBGP("Amb: " << AmB << " relPhi: " << relPhi);
if( AmB > ApB ) {
printf( "Invalid relative curvature, ending calculation...\n" );
return 1;
}
//by definition r1prime >= r2prime
if (ApB + AmB != 0)
r1prime = 1/( ApB + AmB );
else
r1prime = -1.0;
if( ApB == AmB)
r2prime = -1.0;
else
r2prime = 1/(ApB - AmB );
m->r1prime = r1prime;
m->r2prime = r2prime;
//fprintf(stderr,"original: %f %f and %f %f\n",r1,r2,m->r1, m->r2);
//fprintf(stderr,"Relative radii: %f %f equiv: %f\n",r1prime, r2prime, sqrt(r1prime*r2prime) );
return 0;
}
