void frictionContact3D_AlartCurnierFunctionGenerated(
double *reaction,
double *velocity,
double mu,
double *rho,
double *f,
double *A,
double *B)
{
double result[21];
assert(reaction);
assert(velocity);
assert(rho);
SET3(reaction);
SET3(velocity);
SET3(rho);
if (f && A && B)
{
frictionContact3D_AlartCurnierFABGenerated(
*reaction0, *reaction1, *reaction2,
*velocity0, *velocity1, *velocity2,
mu,
*rho0, *rho1, *rho2,
result);
cpy3(result, f);
cpy3x3(result + 3, A);
cpy3x3(result + 12, B);
}
else
{
if (f)
{
frictionContact3D_AlartCurnierFGenerated(
*reaction0, *reaction1, *reaction2,
*velocity0, *velocity1, *velocity2,
mu,
*rho0, *rho1, *rho2,
result);
cpy3(result, f);
}
if (A && B)
{
frictionContact3D_AlartCurnierABGenerated(
*reaction0, *reaction1, *reaction2,
*velocity0, *velocity1, *velocity2,
mu,
*rho0, *rho1, *rho2,
result);
cpy3x3(result, A);
cpy3x3(result + 9, B);
}
}
}
//---------------------------------------------------------------------------
void CGQuadric::m_addTriangle(float x0, float y0, float z0, float nx0, float ny0, float nz0,
float x1, float y1, float z1, float nx1, float ny1, float nz1,
float x2, float y2, float z2, float nx2, float ny2, float nz2)
{
// Increase array storage by 3 vertices.
m_positions = (float*)realloc(m_positions,sizeof(float)*3*(m_vertexCount+3));
m_normals = (float*)realloc(m_normals,sizeof(float)*3*(m_vertexCount+3));
m_colors = (float*)realloc(m_colors,sizeof(float)*4*(m_vertexCount+3));
m_texcoords = (float*)realloc(m_texcoords,sizeof(float)*2*(m_vertexCount+3));
// Get the next free array position.
float *p=m_positions+3*m_vertexCount,
*n=m_normals +3*m_vertexCount,
*c=m_colors +4*m_vertexCount,
*t=m_texcoords+2*m_vertexCount;
// Set the respective values.
#define SET2(p,x,y) { (p)[0]=x; (p)[1]=y; }
#define SET3(p,x,y,z) { (p)[0]=x; (p)[1]=y; (p)[2]=z; }
SET3(p+0,x0,y0,z0) SET3(n+0,nx0,ny0,nz0) SET2(t+0,0,0) memcpy(c+0,m_standardRGBA,sizeof(float)*4);
SET3(p+3,x1,y1,z1) SET3(n+3,nx1,ny1,nz1) SET2(t+2,0,0) memcpy(c+4,m_standardRGBA,sizeof(float)*4);
SET3(p+6,x2,y2,z2) SET3(n+6,nx2,ny2,nz2) SET2(t+4,0,0) memcpy(c+8,m_standardRGBA,sizeof(float)*4);
#undef SET2
#undef SET3
// Increase internal vertex count.
m_vertexCount+=3;
}
extern float *
get_polygon_normal(float normal[3],
int nverts, const float verts[/* nverts */][3])
{
int i;
float tothis[3], toprev[3], cross[3];
/*
* Triangulate the polygon and sum up the nverts-2 triangle normals.
*/
ZEROVEC3(normal);
VMV3(toprev, verts[1], verts[0]); /* 3 subtracts */
for (i = 2; i <= nverts-1; ++i) /* n-2 times... */
{
VMV3(tothis, verts[i], verts[0]); /* 3 subtracts */
VXV3(cross, toprev, tothis); /* 3 subtracts, 6 multiplies */
VPV3(normal, normal, cross); /* 3 adds */
SET3(toprev, tothis);
}
return normal;
}
/* Alart & Curnier version (Radius = mu*max(0,RVN)) */
void computeAlartCurnierSTD(double R[3], double velocity[3], double mu, double rho[3], double F[3], double A[9], double B[9])
{
SET3(R);
SET3(velocity);
SET3(rho);
SET3(F);
SET3X3(A);
SET3X3(B);
double RVN, RVT, RVS;
double RhoN = *rho0;
double RhoT = *rho1;
double Radius, RV, RV1, RV3, GammaTT, GammaTS, GammaST, GammaSS;
RVN = *R0 - RhoN * *velocity0;
RVT = *R1 - RhoT * *velocity1;
RVS = *R2 - RhoT * *velocity2;
RV = sqrt(RVT * RVT + RVS * RVS);
//Radius = mu*R[0];
if (A00)
{
// always true
*A01 = 0.;
*A02 = 0.;
// these should appear under conditions
*A10 = 0.;
*A20 = 0.;
}
if (B00)
{
// always true
*B01 = 0.;
*B02 = 0.;
// these should appear under conditions
*B10 = 0.;
*B20 = 0.;
}
if (RVN > 0.0)
{
#ifdef VERBOSE_DEBUG
printf("Normal part in the cone\n");
#endif
Radius = mu * RVN;
*F0 = RhoN * *velocity0;
if (A00 && B00)
{
*A00 = RhoN;
*B00 = 0.0;
}
}
else
{
#ifdef VERBOSE_DEBUG
printf("Normal part out the cone\n");
#endif
Radius = 0.0;
*F0 = *R0;
if (A00 && B00)
{
*A00 = 0.0;
*B00 = 1.0;
}
}
// Compute the value of the Alart--Curnier Function and its gradient for the tangential part
#ifdef VERBOSE_DEBUG
printf("Radius=%le\n", Radius);
printf("RV=%le\n", RV);
#endif
if (RV <= Radius) // We are in the disk and Radius is positive
{
#ifdef VERBOSE_DEBUG
printf("We are in the disk\n");
#endif
*F1 = RhoT * *velocity1;
*F2 = RhoT * *velocity2;
if (A00 && B00)
{
*A11 = RhoT;
*A12 = 0.0;
*A21 = 0.0;
*A22 = RhoT;
*B10 = 0.0;
*B11 = 0.0;
*B12 = 0.0;
*B20 = 0.0;
*B21 = 0.0;
*B22 = 0.0;
}
}
else if (RV > Radius) // We are out the disk and Radius is postive
{
if (Radius > 0)
{
#ifdef VERBOSE_DEBUG
printf("We are out the disk and Radius is positive\n");
#endif
RV1 = 1.0 / RV;
*F1 = *R1 - Radius * RVT * RV1;
*F2 = *R2 - Radius * RVS * RV1;
if (A00 && B00)
{
RV3 = RV1 * RV1 * RV1;
GammaTT = RV1 - RVT * RVT * RV3;
GammaTS = - RVT * RVS * RV3;
GammaST = GammaTS;
GammaSS = RV1 - RVS * RVS * RV3;
*A10 = mu * RhoN * RVT * RV1;
*A20 = mu * RhoN * RVS * RV1;
*A11 = GammaTT * RhoT * Radius;
*A12 = GammaTS * RhoT * Radius;
*A21 = GammaST * RhoT * Radius;
*A22 = GammaSS * RhoT * Radius;
*B10 = -mu * RVT * RV1;
*B11 = 1.0 - GammaTT * Radius ;
*B12 = - GammaTS * Radius ;
*B20 = -mu * RVS * RV1;
*B21 = - GammaST * Radius;
*B22 = 1.0 - GammaSS * Radius;
}
}
else
{
#ifdef VERBOSE_DEBUG
printf("We are out the disk and Radius is zero\n");
#endif
*F1 = *R1 ;
*F2 = *R2 ;
if (A00 && B00)
{
*A11 = 0.0;
*A12 = 0.0;
*A21 = 0.0;
*A22 = 0.0;
*B10 = 0.0;
*B11 = 1.0;
*B12 = 0.0;
*B20 = 0.0;
*B21 = 0.0;
*B22 = 1.0;
}
}
}
/* else // We are out the disk and Radius is negative */
/* { */
/* #ifdef VERBOSE_DEBUG */
/* printf("We are out the disk and Radius is negative\n"); */
/* #endif */
/* /\*Version original *\/ */
/* F[1] = R[1] ; */
/* F[2] = R[2] ; */
/* if (A && B){ */
/* A[1+3*1]=0.0; */
/* A[1+3*2]=0.0; */
/* A[2+3*1]=0.0; */
/* A[2+3*2]=0.0; */
/* B[1+3*0]=0.0; */
/* B[1+3*1]=1.0; */
/* B[1+3*2]=0.0; */
/* B[2+3*0]=0.0; */
/* B[2+3*1]=0.0; */
/* B[2+3*2]=1.0;} */
/* }
#ifdef VERBOSE_DEBUG
printf("F[0] = %le\t", F[0]);
printf("F[1] = %le\t", F[1]);
printf("F[2] = %le\n", F[2]);
if (A && B)
{
for (int l=0; l<3; l++)
{
for (int k=0; k<3; k++)
{
printf("A[%i+3*%i] = %le\t", l,k, A[l+3*k]);
}
printf("\n");
}
for (int l=0; l<3; l++)
{
for (int k=0; k<3; k++)
{
printf("B[%i+3*%i] = %le\t", l,k, B[l+3*k]);
}
printf("\n");
}
double diago =0.0;
for (int l=0; l<3; l++)
{
for (int k=0; k<3; k++)
{
if (k==l) diago = 1.0;
else diago=0.0;
printf("I-B[%i+3*%i] = %le\t", l,k,diago- B[l+3*k]);
}
printf("\n");
}
}
#endif
*/
}
//gfd*
static void md5_process(mir_md5_state_t *pms, const mir_md5_byte_t *data /*[64]*/)
{
mir_md5_word_t
a = pms->abcd[0], b = pms->abcd[1],
c = pms->abcd[2], d = pms->abcd[3];
mir_md5_word_t t;
/* Define storage for little-endian or both types of CPUs. */
mir_md5_word_t xbuf[16];
const mir_md5_word_t *X;
{
/*
* Determine dynamically whether this is a big-endian or
* little-endian machine, since we can use a more efficient
* algorithm on the latter.
*/
static const int w = 1;
if (*((const mir_md5_byte_t *)&w)) /* dynamic little-endian */
{
/*
* On little-endian machines, we can process properly aligned
* data without copying it.
*/
if (!((data - (const mir_md5_byte_t *)0) & 3)) {
/* data are properly aligned */
X = (const mir_md5_word_t *)data;
} else {
/* not aligned */
memcpy(xbuf, data, 64);
X = xbuf;
}
}
else /* dynamic big-endian */
{
/*
* On big-endian machines, we must arrange the bytes in the
* right order.
*/
const mir_md5_byte_t *xp = data;
int i;
X = xbuf; /* (dynamic only) */
for (i = 0; i < 16; ++i, xp += 4)
xbuf[i] = xp[0] + (xp[1] << 8) + (xp[2] << 16) + (xp[3] << 24);
}
}
#define ROTATE_LEFT(x, n) (((x) << (n)) | ((x) >> (32 - (n))))
/* Round 1. */
/* Let [abcd k s i] denote the operation
a = b + ((a + F(b,c,d) + X[k] + T[i]) <<< s). */
#define F(x, y, z) (((x) & (y)) | (~(x) & (z)))
#define SET1(a, b, c, d, k, s, Ti)\
t = a + F(b,c,d) + X[k] + Ti;\
a = ROTATE_LEFT(t, s) + b
/* Do the following 16 operations. */
SET1(a, b, c, d, 0, 7, T1);
SET1(d, a, b, c, 1, 12, T2);
SET1(c, d, a, b, 2, 17, T3);
SET1(b, c, d, a, 3, 22, T4);
SET1(a, b, c, d, 4, 7, T5);
SET1(d, a, b, c, 5, 12, T6);
SET1(c, d, a, b, 6, 17, T7);
SET1(b, c, d, a, 7, 22, T8);
SET1(a, b, c, d, 8, 7, T9);
SET1(d, a, b, c, 9, 12, T10);
SET1(c, d, a, b, 10, 17, T11);
SET1(b, c, d, a, 11, 22, T12);
SET1(a, b, c, d, 12, 7, T13);
SET1(d, a, b, c, 13, 12, T14);
SET1(c, d, a, b, 14, 17, T15);
SET1(b, c, d, a, 15, 22, T16);
/* Round 2. */
/* Let [abcd k s i] denote the operation
a = b + ((a + G(b,c,d) + X[k] + T[i]) <<< s). */
#define G(x, y, z) (((x) & (z)) | ((y) & ~(z)))
#define SET2(a, b, c, d, k, s, Ti)\
t = a + G(b,c,d) + X[k] + Ti;\
a = ROTATE_LEFT(t, s) + b
/* Do the following 16 operations. */
SET2(a, b, c, d, 1, 5, T17);
SET2(d, a, b, c, 6, 9, T18);
SET2(c, d, a, b, 11, 14, T19);
SET2(b, c, d, a, 0, 20, T20);
SET2(a, b, c, d, 5, 5, T21);
SET2(d, a, b, c, 10, 9, T22);
SET2(c, d, a, b, 15, 14, T23);
SET2(b, c, d, a, 4, 20, T24);
SET2(a, b, c, d, 9, 5, T25);
SET2(d, a, b, c, 14, 9, T26);
SET2(c, d, a, b, 3, 14, T27);
SET2(b, c, d, a, 8, 20, T28);
SET2(a, b, c, d, 13, 5, T29);
SET2(d, a, b, c, 2, 9, T30);
SET2(c, d, a, b, 7, 14, T31);
SET2(b, c, d, a, 12, 20, T32);
/* Round 3. */
/* Let [abcd k s t] denote the operation
a = b + ((a + H(b,c,d) + X[k] + T[i]) <<< s). */
#define H(x, y, z) ((x) ^ (y) ^ (z))
#define SET3(a, b, c, d, k, s, Ti)\
t = a + H(b,c,d) + X[k] + Ti;\
a = ROTATE_LEFT(t, s) + b
/* Do the following 16 operations. */
SET3(a, b, c, d, 5, 4, T33);
SET3(d, a, b, c, 8, 11, T34);
SET3(c, d, a, b, 11, 16, T35);
SET3(b, c, d, a, 14, 23, T36);
SET3(a, b, c, d, 1, 4, T37);
SET3(d, a, b, c, 4, 11, T38);
SET3(c, d, a, b, 7, 16, T39);
SET3(b, c, d, a, 10, 23, T40);
SET3(a, b, c, d, 13, 4, T41);
SET3(d, a, b, c, 0, 11, T42);
SET3(c, d, a, b, 3, 16, T43);
SET3(b, c, d, a, 6, 23, T44);
SET3(a, b, c, d, 9, 4, T45);
SET3(d, a, b, c, 12, 11, T46);
SET3(c, d, a, b, 15, 16, T47);
SET3(b, c, d, a, 2, 23, T48);
/* Round 4. */
/* Let [abcd k s t] denote the operation
a = b + ((a + I(b,c,d) + X[k] + T[i]) <<< s). */
#define I(x, y, z) ((y) ^ ((x) | ~(z)))
#define SET4(a, b, c, d, k, s, Ti)\
t = a + I(b,c,d) + X[k] + Ti;\
a = ROTATE_LEFT(t, s) + b
/* Do the following 16 operations. */
SET4(a, b, c, d, 0, 6, T49);
SET4(d, a, b, c, 7, 10, T50);
SET4(c, d, a, b, 14, 15, T51);
SET4(b, c, d, a, 5, 21, T52);
SET4(a, b, c, d, 12, 6, T53);
SET4(d, a, b, c, 3, 10, T54);
SET4(c, d, a, b, 10, 15, T55);
SET4(b, c, d, a, 1, 21, T56);
SET4(a, b, c, d, 8, 6, T57);
SET4(d, a, b, c, 15, 10, T58);
SET4(c, d, a, b, 6, 15, T59);
SET4(b, c, d, a, 13, 21, T60);
SET4(a, b, c, d, 4, 6, T61);
SET4(d, a, b, c, 11, 10, T62);
SET4(c, d, a, b, 2, 15, T63);
SET4(b, c, d, a, 9, 21, T64);
/* Then perform the following additions. (That is increment each
of the four registers by the value it had before this block
was started.) */
pms->abcd[0] += a;
pms->abcd[1] += b;
pms->abcd[2] += c;
pms->abcd[3] += d;
}
