void inertia_triangle(double *v0, double *v1, double *v2,
double mass, double *inertia)
{
double s[3][3] = {{2.0, 1.0, 1.0}, {1.0, 2.0, 1.0}, {1.0, 1.0, 2.0}};
double v[3][3],sv[3][3],vtsv[3][3];
double vvv[3],v1mv0[3],v2mv0[3],normal[3];
v[0][0] = v0[0]; v[0][1] = v0[1]; v[0][2] = v0[2];
v[1][0] = v1[0]; v[1][1] = v1[1]; v[1][2] = v1[2];
v[2][0] = v2[0]; v[2][1] = v2[1]; v[2][2] = v2[2];
times3(s,v,sv);
transpose_times3(v,sv,vtsv);
double sum = lensq3(v0) + lensq3(v1) + lensq3(v2);
vvv[0] = v0[0] + v1[0] + v2[0];
vvv[1] = v0[1] + v1[1] + v2[1];
vvv[2] = v0[2] + v1[2] + v2[2];
sum += lensq3(vvv);
sub3(v1,v0,v1mv0);
sub3(v2,v0,v2mv0);
cross3(v1mv0,v2mv0,normal);
double a = len3(normal);
double inv24 = mass/24.0;
inertia[0] = inv24*a*(sum-vtsv[0][0]);
inertia[1] = inv24*a*(sum-vtsv[1][1]);
inertia[2] = inv24*a*(sum-vtsv[2][2]);
inertia[3] = -inv24*a*vtsv[1][2];
inertia[4] = -inv24*a*vtsv[0][2];
inertia[5] = -inv24*a*vtsv[0][1];
}
TEST(Tag2Sub3, read_write) {
const std::string raw = unhexlify(makehex(dt, 8));
OpenPGP::Subpacket::Tag2::Sub3 sub3(raw);
TAG2_SUB3_EQ(sub3);
EXPECT_EQ(sub3.raw(), raw);
}
TEST( AndOp, MatchesElementThreeClauses ) {
BSONObj baseOperand1 = BSON( "$lt" << "z1" );
BSONObj baseOperand2 = BSON( "$gt" << "a1" );
BSONObj match = BSON( "a" << "r1" );
BSONObj notMatch1 = BSON( "a" << "z1" );
BSONObj notMatch2 = BSON( "a" << "a1" );
BSONObj notMatch3 = BSON( "a" << "r" );
auto_ptr<ComparisonMatchExpression> sub1( new ComparisonMatchExpression() );
ASSERT( sub1->init( "a", ComparisonMatchExpression::LT, baseOperand1[ "$lt" ] ).isOK() );
auto_ptr<ComparisonMatchExpression> sub2( new ComparisonMatchExpression() );
ASSERT( sub2->init( "a", ComparisonMatchExpression::GT, baseOperand2[ "$gt" ] ).isOK() );
auto_ptr<RegexMatchExpression> sub3( new RegexMatchExpression() );
ASSERT( sub3->init( "a", "1", "" ).isOK() );
AndMatchExpression andOp;
andOp.add( sub1.release() );
andOp.add( sub2.release() );
andOp.add( sub3.release() );
ASSERT( andOp.matches( match ) );
ASSERT( !andOp.matches( notMatch1 ) );
ASSERT( !andOp.matches( notMatch2 ) );
ASSERT( !andOp.matches( notMatch3 ) );
}
TEST( NorOp, MatchesThreeClauses ) {
BSONObj baseOperand1 = BSON( "$gt" << 10 );
BSONObj baseOperand2 = BSON( "$lt" << 0 );
BSONObj baseOperand3 = BSON( "b" << 100 );
auto_ptr<ComparisonMatchExpression> sub1( new ComparisonMatchExpression() );
ASSERT( sub1->init( "a", ComparisonMatchExpression::GT, baseOperand1[ "$gt" ] ).isOK() );
auto_ptr<ComparisonMatchExpression> sub2( new ComparisonMatchExpression() );
ASSERT( sub2->init( "a", ComparisonMatchExpression::LT, baseOperand2[ "$lt" ] ).isOK() );
auto_ptr<ComparisonMatchExpression> sub3( new ComparisonMatchExpression() );
ASSERT( sub3->init( "b", ComparisonMatchExpression::EQ, baseOperand3[ "b" ] ).isOK() );
NorMatchExpression norOp;
norOp.add( sub1.release() );
norOp.add( sub2.release() );
norOp.add( sub3.release() );
ASSERT( !norOp.matches( BSON( "a" << -1 ), NULL ) );
ASSERT( !norOp.matches( BSON( "a" << 11 ), NULL ) );
ASSERT( norOp.matches( BSON( "a" << 5 ), NULL ) );
ASSERT( !norOp.matches( BSON( "b" << 100 ), NULL ) );
ASSERT( norOp.matches( BSON( "b" << 101 ), NULL ) );
ASSERT( norOp.matches( BSONObj(), NULL ) );
ASSERT( !norOp.matches( BSON( "a" << 11 << "b" << 100 ), NULL ) );
}
TEST( AndOp, MatchesThreeClauses ) {
BSONObj baseOperand1 = BSON( "$gt" << 1 );
BSONObj baseOperand2 = BSON( "$lt" << 10 );
BSONObj baseOperand3 = BSON( "$lt" << 100 );
auto_ptr<ComparisonMatchExpression> sub1( new ComparisonMatchExpression() );
ASSERT( sub1->init( "a", ComparisonMatchExpression::GT, baseOperand1[ "$gt" ] ).isOK() );
auto_ptr<ComparisonMatchExpression> sub2( new ComparisonMatchExpression() );
ASSERT( sub2->init( "a", ComparisonMatchExpression::LT, baseOperand2[ "$lt" ] ).isOK() );
auto_ptr<ComparisonMatchExpression> sub3( new ComparisonMatchExpression() );
ASSERT( sub3->init( "b", ComparisonMatchExpression::LT, baseOperand3[ "$lt" ] ).isOK() );
AndMatchExpression andOp;
andOp.add( sub1.release() );
andOp.add( sub2.release() );
andOp.add( sub3.release() );
ASSERT( andOp.matches( BSON( "a" << 5 << "b" << 6 ), NULL ) );
ASSERT( !andOp.matches( BSON( "a" << 5 ), NULL ) );
ASSERT( !andOp.matches( BSON( "b" << 6 ), NULL ) );
ASSERT( !andOp.matches( BSON( "a" << 1 << "b" << 6 ), NULL ) );
ASSERT( !andOp.matches( BSON( "a" << 10 << "b" << 6 ), NULL ) );
}
/*
* multiply the top of the matrix stack by a view-pointing transformation
* with the eyepoint at e, looking at point l, with u at the top of the screen.
* The coordinate system is deemed to be right-handed.
* The generated transformation transforms this view into a view from
* the origin, looking in the positive y direction, with the z axis pointing up,
* and x to the right.
*/
void look(Space *t, Point3 e, Point3 l, Point3 u){
Matrix m, inv;
Point3 r;
l=unit3(sub3(l, e));
u=unit3(vrem3(sub3(u, e), l));
r=cross3(l, u);
/* make the matrix to transform from (rlu) space to (xyz) space */
ident(m);
m[0][0]=r.x; m[0][1]=r.y; m[0][2]=r.z;
m[1][0]=l.x; m[1][1]=l.y; m[1][2]=l.z;
m[2][0]=u.x; m[2][1]=u.y; m[2][2]=u.z;
ident(inv);
inv[0][0]=r.x; inv[0][1]=l.x; inv[0][2]=u.x;
inv[1][0]=r.y; inv[1][1]=l.y; inv[1][2]=u.y;
inv[2][0]=r.z; inv[2][1]=l.z; inv[2][2]=u.z;
ixform(t, m, inv);
move(t, -e.x, -e.y, -e.z);
}
void
testTortureExecute (void)
{
if (sub1 (20) != 35)
ASSERT (0);
if (sub2 (20) != 45)
ASSERT (0);
if (sub3 (20) != -5)
ASSERT (0);
if (sub4 (20) != 5)
ASSERT (0);
return;
}
void main() {
debug();
Describe("RGBAImage<byte>", []() {
It("should be able to read data from bmp file", []() {
RGBAImage<byte> image("TestResources/rgbw_2x2.bmp");
Expect(image.GetPixel(0, 0)).ToBe(RGBAColor<byte>(255, 0, 0, 255));
Expect(image.GetPixel(1, 0)).ToBe(RGBAColor<byte>(0, 255, 0, 255));
Expect(image.GetPixel(0, 1)).ToBe(RGBAColor<byte>(0, 0, 255, 255));
Expect(image.GetPixel(1, 1)).ToBe(RGBAColor<byte>(255, 255, 255, 255));
});
Describe("when read lena.bmp finished", []() {
RGBAImage<byte> image("TestResources/Lena.bmp");
It("should be able to get width and height", [&image]() {
Expect(image.Height()).ToBe(512);
Expect(image.Width()).ToBe(512);
});
It("should be able to get pixel", [&image]() {
Expect(image.GetPixel(0, 0)).ToBe(RGBAColor<byte>(225, 138, 128, 255));
});
It("should be able to indexof sub image", [&image]() {
RGBAImage<byte> sub1("TestResources/IndexOf/sub1.bmp");
RGBAImage<byte> sub2("TestResources/IndexOf/sub2.bmp");
RGBAImage<byte> sub3("TestResources/IndexOf/sub3.bmp");
Expect(image.IndexOf(sub1)).ToBe(Coord<short>(0, 0));
Expect(image.IndexOf(sub2)).ToBe(Coord<short>(1, 500));
Expect(image.IndexOf(sub3)).ToBe(Coord<short>(122, 237));
});
});
});
Benchmark("read lene.bmp", []() {
RGBAImage<byte> image("TestResources/Lena.bmp");
});
Benchmark("indexof lene.bmp", []() {
RGBAImage<byte> sub3("TestResources/IndexOf/sub3.bmp");
RGBAImage<byte> image("TestResources/Lena.bmp");
image.IndexOf(sub3);
});
}
main()
{
if (sub1 (0x80000000ULL))
abort ();
if (sub2 (0x80000000ULL))
abort ();
if (sub3 (0x80000000ULL))
abort ();
if (sub4 (0x80000000ULL))
abort ();
exit (0);
}
void
testTortureExecute (void)
{
#if !defined(__SDCC_ds390) && !defined(__SDCC_pic14) && !defined(__SDCC_pic16)
if (sub1 (0x80000000ULL))
ASSERT (0);
if (sub2 (0x80000000ULL))
ASSERT (0);
if (sub3 (0x80000000ULL))
ASSERT (0);
if (sub4 (0x80000000ULL))
ASSERT (0);
return;
#endif
}
void
program_prog(
int argc,
char const* argv[])
{
common cmn(argc, argv);
common_write write(cmn);
int num = fem::int0;
sub1(num);
write(6, star), "num after sub1:", num;
int n = 1;
sub2(num, n);
write(6, star), "num after sub2", num;
arr_1d<2, int> nums(fem::fill0);
sub1(nums);
write(6, star), "nums after sub1:", nums(1), nums(2);
n = 2;
sub2(nums, n);
write(6, star), "nums after sub2:", nums(1), nums(2);
sub3(nums, n);
write(6, star), "nums after sub3:", nums(1), nums(2);
}
void lookat(vec3 eye, vec3 center, vec3 up, float* camera) {
vec3 z = normal3(sub3(eye, center));
vec3 x = normal3(cross3(up,z));
vec3 y = normal3(cross3(z,x));
float Minv[4][4];
float Tr[4][4];
mat_identity(&Minv[0][0]);
mat_identity(&Tr[0][0]);
Minv[0][0] = x.x;
Minv[1][0] = y.x;
Minv[2][0] = z.x;
Tr[0][3] = -center.x;
Minv[0][1] = x.y;
Minv[1][1] = y.y;
Minv[2][1] = z.y;
Tr[1][3] = -center.y;
Minv[0][2] = x.z;
Minv[1][2] = y.z;
Minv[2][2] = z.z;
Tr[2][3] = -center.z;
matmul(&Minv[0][0], &Tr[0][0], camera);
}
TEST( OrOp, MatchesThreeClauses ) {
BSONObj baseOperand1 = BSON( "$gt" << 10 );
BSONObj baseOperand2 = BSON( "$lt" << 0 );
BSONObj baseOperand3 = BSON( "b" << 100 );
auto_ptr<ComparisonMatchExpression> sub1( new GTMatchExpression() );
ASSERT( sub1->init( "a", baseOperand1[ "$gt" ] ).isOK() );
auto_ptr<ComparisonMatchExpression> sub2( new LTMatchExpression() );
ASSERT( sub2->init( "a", baseOperand2[ "$lt" ] ).isOK() );
auto_ptr<ComparisonMatchExpression> sub3( new EqualityMatchExpression() );
ASSERT( sub3->init( "b", baseOperand3[ "b" ] ).isOK() );
OrMatchExpression orOp;
orOp.add( sub1.release() );
orOp.add( sub2.release() );
orOp.add( sub3.release() );
ASSERT( orOp.matchesBSON( BSON( "a" << -1 ), NULL ) );
ASSERT( orOp.matchesBSON( BSON( "a" << 11 ), NULL ) );
ASSERT( !orOp.matchesBSON( BSON( "a" << 5 ), NULL ) );
ASSERT( orOp.matchesBSON( BSON( "b" << 100 ), NULL ) );
ASSERT( !orOp.matchesBSON( BSON( "b" << 101 ), NULL ) );
ASSERT( !orOp.matchesBSON( BSONObj(), NULL ) );
ASSERT( orOp.matchesBSON( BSON( "a" << 11 << "b" << 100 ), NULL ) );
}
void main()
{
time_t t;
int i;
char s[35];
clrscr();
time(&t);
strcpy(s,ctime(&t));
//*//
printf("\nSTD Time:\n\n");
printf("%s\n",ctime(&t));
//*/
//TEST Input Section
/*//
s[11]='2'; s[12]='3'; s[14]='1'; s[15]='0'; s[17]='3'; s[18]='5';
printf("\nOld:\n");
printf("%c\t",s[11]);
printf("%c:\t",s[12]);
printf("%c\t",s[14]);
printf("%c:\t",s[15]);
printf("%c\t",s[17]);
printf("%c\n",s[18]);
//*/
//ascii()//0=48:1=49:2=50:3=51:4=52:5=53:6=54:7=55:8=56:9=57
//Extra Space Intro
for(i=27;i>=22;i--)
{
s[i]=s[i-2];
s[26]=s[24];
}
//Space intro btw am/pm field and yr field
for(i=27;i>=21;i--)
{
s[i]=s[i-1];
s[21]=' ';
}
//default definition as AM
s[20]='A';
s[21]='M';
//---------------------CONVERSION---------------------------
//conversion of hours field to invert time
//PM conversion
if(s[11]==49&&s[12]>=50)
s[20]='P';
else if(s[11]==50)
s[20]='P';
//Hours conversion//without edge value tapering
if(s[11]==48&&s[12]<=52)
{
s[11]='2';
s[12]=sub3(s[12]);
}
else if(s[11]==48&&s[12]>52&&s[12]<=57)
{
s[11]='1';
s[12]=sub4(s[12]);
}
else if(s[11]==49&&s[12]<=52)
{
s[12]=sub3(s[12]);
}
else if(s[11]==49&&s[12]>52&&s[12]<=57)
{
s[11]='0';
s[12]=sub4(s[12]);
}
else if(s[11]==50&&s[12]<=52)
{
s[11]='0';
s[12]=sub3(s[12]);
}
//*//
//12 format conversion//without edge value tapering
//case xx:00:00
if(s[11]==50&&s[12]==52)
{
s[11]='1';
s[12]='2';
}
else if(s[11]==50&&s[12]>=50)
{
s[11]='1';
s[12]=sub5(s[12]);
}
else if(s[11]==50&&s[12]>=48&&s[12]<50)
{
s[11]='0';
s[12]=sub6(s[12]);
}
else if(s[11]==49&&s[12]>=51&&s[12]<=57)
{
s[11]='0';
s[12]=sub5(s[12]);
}
/*//
printf("\n\n12 converted:\n\n");
printf("%c\t",s[11]);
printf("%c:\t",s[12]);
printf("%c\t",s[14]);
printf("%c:\t",s[15]);
printf("%c\t",s[17]);
printf("%c\n",s[18]);
//*/
//----------------------------------------------------------
//minutes conversion
if(s[14]!=48)
{
s[15]=sub1(s[15]);
s[14]=sub2(s[14],s[15]);
}
else if(s[14]==48&&s[15]!=48)
{
s[14]='5';
s[15]=sub1(s[15]);
}
/*//
printf("\n\nMinutes Converted:\n\n");
printf("%c\t",s[11]);
printf("%c:\t",s[12]);
printf("%c\t",s[14]);
printf("%c:\t",s[15]);
printf("%c\t",s[17]);
printf("%c\n",s[18]);
//*/
/*
else
{
s[15]='5';
s[14]=sub2(s[14],s[15]);
}
//*/
//seconds conversion
if(s[17]!=48)
{
s[18]=sub1(s[18]);
s[17]=sub2(s[17],s[18]);
}
else if(s[17]==48&&s[18]!=48)
{
s[17]='5';
s[18]=sub1(s[18]);
}
/*//
printf("\n\nSeconds Converted:\n\n");
printf("%c\t",s[11]);
printf("%c:\t",s[12]);
printf("%c\t",s[14]);
printf("%c:\t",s[15]);
printf("%c\t",s[17]);
printf("%c\n",s[18]);
//*/
//----------------------------------------------------------
//edge tapering
//case xx:xx:00
if(s[18]==48&&s[17]==48)
{
if(!(s[11]==48&&s[12]==48))
{
if(s[14]!=48||s[15]!=48)
{
if(s[11]==49&&s[12]>48)
s[12]=sub7(s[12]);
else if(s[11]==49&&s[12]==48)
{s[11]='0';s[12]='9';}
else if(s[11]==48&&s[12]>=49)
s[12]=sub7(s[12]);
}
}
/*//
printf("\n\nPost tapering case xx:xx:00:\n\n");
printf("%c\t",s[11]);
printf("%c:\t",s[12]);
printf("%c\t",s[14]);
printf("%c:\t",s[15]);
printf("%c\t",s[17]);
printf("%c\n",s[18]);
//*/
goto output;
}
//case xx:00:xx
if(s[14]==48&&s[15]==48)
{
if(s[17]!=48||s[18]!=48)
{
s[14]='5'; //printf("\nValue of s[14]:%c",s[14]);
s[15]='9'; //printf("\nValue of s[15]:%c",s[15]);
///*
if(s[11]==49&&s[12]>48)
s[12]=sub7(s[12]);
else if(s[11]==49&&s[12]==48)
{s[11]='0';s[12]='9';}
else if(s[11]==48&&s[12]>=49)
s[12]=sub7(s[12]);
}
/*//
printf("\n\nPost tapering case xx:00:xx:\n\n");
printf("%c\t",s[11]);
printf("%c:\t",s[12]);
printf("%c\t",s[14]);
printf("%c:\t",s[15]);
printf("%c\t",s[17]);
printf("%c\n",s[18]);
//*/
goto output;
}
//case xx:xx:xx//prev cases excluded
if(s[17]!=48||s[18]!=48)
{
if(s[14]==53)
{
s[15]=sub7(s[15]);
if(s[11]==49&&s[12]>48)
s[12]=sub7(s[12]);
else if(s[11]==49&&s[12]==48)
{s[11]='0';s[12]='9';}
else if(s[11]==48&&s[12]>=49)
s[12]=sub7(s[12]);
if(s[15]==48)
{
s[15]='9';
s[14]=sub7(s[14]);
}
}
else if(s[14]!=53)
{
if(s[15]==48)
{
s[15]='9';
s[14]=sub7(s[14]);
if(s[11]==49&&s[12]>48)
s[12]=sub7(s[12]);
else if(s[11]==49&&s[12]==48)
{s[11]='0';s[12]='9';}
else if(s[11]==48&&s[12]>=49)
s[12]=sub7(s[12]);
}
else
{
s[15]=sub7(s[15]);
if(s[11]==49&&s[12]>48)
s[12]=sub7(s[12]);
else if(s[11]==49&&s[12]==48)
{s[11]='0';s[12]='9';}
else if(s[11]==48&&s[12]>=49)
s[12]=sub7(s[12]);
}
}
/*//
if(s[14]!=48||s[15]!=48)
{
s[15]=sub1(s[15]);
s[15]=sub7(s[15]);
{
if(s[11]==49&&s[12]>48)
s[12]=sub7(s[12]);
else if(s[11]==49&&s[12]==48)
{s[11]='0';s[12]='9';}
else if(s[11]==48&&s[12]>=49&&s[15]!=48)
{s[12]=sub7(s[12]);}
}
if(s[15]==48)
{
//s[14]=sub7(s[14]);
s[15]='9';
}
}
if(s[17]==48)
{
s[17]='5';
s[18]=sub1(s[18]);
}
//*/
/*//
printf("\n\nPost tapering case xx:xx:xx:\n\n");
printf("%c\t",s[11]);
printf("%c:\t",s[12]);
printf("%c\t",s[14]);
printf("%c:\t",s[15]);
printf("%c\t",s[17]);
printf("%c\n",s[18]);
//*/
}
//----------------OUTPUT Section----------------------------
output:
printf("\n\n");
printf("Invert TIME:\n\n");
for(i=0;i<=26;i++)
{
printf("%c",s[i]);
}
getch();
}
void basic_gear()
{
char rcsid[] = "junk";
#define NUM_WHEELS 4
// char gear_strings[NUM_WHEELS][12]={"nose","right main", "left main", "tail skid"};
/*
* Aircraft specific initializations and data goes here
*/
static int num_wheels = NUM_WHEELS; /* number of wheels */
static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations,full extension */
{
{ .422, 0., .29 }, /*nose*/ /* in feet */
{ 0.026, 0.006, .409 }, /*right main*/
{ 0.026, -.006, .409 }, /*left main*/
{ -1.32, 0, .17 } /*tail skid */
};
// static DATA gear_travel[NUM_WHEELS] = /*in Z-axis*/
// { -0.5, 2.5, 2.5, 0};
static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
{ 2., .65, .65, 1. };
static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
{ 1., .3, .3, .5 };
static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
{ 0., 0., 0., 0. }; /* 0 = none, 1 = full */
static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
{ 0., 0., 0., 0}; /* radians, +CW */
/*
* End of aircraft specific code
*/
/*
* Constants & coefficients for tyres on tarmac - ref [1]
*/
/* skid function looks like:
*
* mu ^
* |
* max_mu | +
* | /|
* sliding_mu | / +------
* | /
* | /
* +--+------------------------>
* | | | sideward V
* 0 bkout skid
* V V
*/
static int it_rolls[NUM_WHEELS] = { 1,1,1,0};
static DATA sliding_mu[NUM_WHEELS] = { 0.5, 0.5, 0.5, 0.3};
static DATA rolling_mu[NUM_WHEELS] = { 0.01, 0.01, 0.01, 0.0};
static DATA max_brake_mu[NUM_WHEELS] ={ 0.0, 0.6, 0.6, 0.0};
static DATA max_mu = 0.8;
static DATA bkout_v = 0.1;
static DATA skid_v = 1.0;
/*
* Local data variables
*/
DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/
// DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
// DATA altitude_local_v[3]; /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
// DATA altitude_body_v[3]; /*altitude vector in body (X,Y,Z)*/
DATA temp3a[3];
// DATA temp3b[3];
DATA tempF[3];
DATA tempM[3];
DATA reaction_normal_force; /* wheel normal (to rwy) force */
DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
DATA forward_mu, sideward_mu; /* friction coefficients */
DATA beta_mu; /* breakout friction slope */
DATA forward_wheel_force, sideward_wheel_force;
int i; /* per wheel loop counter */
/*
* Execution starts here
*/
beta_mu = max_mu/(skid_v-bkout_v);
clear3( F_gear_v ); /* Initialize sum of forces... */
clear3( M_gear_v ); /* ...and moments */
/*
* Put aircraft specific executable code here
*/
percent_brake[1] = Brake_pct[0];
percent_brake[2] = Brake_pct[1];
caster_angle_rad[0] =
(0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
for (i=0;i<num_wheels;i++) /* Loop for each wheel */
{
/* printf("%s:\n",gear_strings[i]); */
/*========================================*/
/* Calculate wheel position w.r.t. runway */
/*========================================*/
/* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
/* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
/* then converting to local (North-East-Down) axes... */
multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
/* Runway axes correction - third element is Altitude, not (-)Z... */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
/* Add wheel offset to cg location in local axes */
add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
/* remove Runway axes correction so right hand rule applies */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
/*============================*/
/* Calculate wheel velocities */
/*============================*/
/* contribution due to angular rates */
cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
/* transform into local axes */
multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
/* plus contribution due to cg velocities */
add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
clear3(f_wheel_local_v);
reaction_normal_force=0;
if( HEIGHT_AGL_WHEEL < 0. )
/*the wheel is underground -- which implies ground contact
so calculate reaction forces */
{
/*===========================================*/
/* Calculate forces & moments for this wheel */
/*===========================================*/
/* Add any anticipation, or frame lead/prediction, here... */
/* no lead used at present */
/* Calculate sideward and forward velocities of the wheel
in the runway plane */
cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
+ v_wheel_local_v[1]*sin_wheel_hdg_angle;
v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
- v_wheel_local_v[0]*sin_wheel_hdg_angle;
/* Calculate normal load force (simple spring constant) */
reaction_normal_force = 0.;
reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
- v_wheel_local_v[2]*spring_damping[i];
/* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
if (reaction_normal_force > 0.) reaction_normal_force = 0.;
/* to prevent damping component from swamping spring component */
/* Calculate friction coefficients */
if(it_rolls[i])
{
forward_mu = (max_brake_mu[i] - rolling_mu[i])*percent_brake[i] + rolling_mu[i];
abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
sideward_mu = sliding_mu[i];
if (abs_v_wheel_sideward < skid_v)
sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
}
else
{
forward_mu=sliding_mu[i];
sideward_mu=sliding_mu[i];
}
/* Calculate foreward and sideward reaction forces */
forward_wheel_force = forward_mu*reaction_normal_force;
sideward_wheel_force = sideward_mu*reaction_normal_force;
if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
/* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
*/
/* Rotate into local (N-E-D) axes */
f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
- sideward_wheel_force*sin_wheel_hdg_angle;
f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
+ sideward_wheel_force*cos_wheel_hdg_angle;
f_wheel_local_v[2] = reaction_normal_force;
/* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
/* Calculate moments from force and offsets in body axes */
cross3( d_wheel_cg_body_v, tempF, tempM );
/* Sum forces and moments across all wheels */
add3( tempF, F_gear_v, F_gear_v );
add3( tempM, M_gear_v, M_gear_v );
}
/* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
/* printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear); */
/* printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
}
}
int LocalNonsmoothNewtonSolver(FrictionContactProblem* localproblem, double * R, int *iparam, double *dparam)
{
double mu = localproblem->mu[0];
double * qLocal = localproblem->q;
double * MLocal = localproblem->M->matrix0;
double Tol = dparam[0];
double itermax = iparam[0];
int i, j, k, inew;
// store the increment
double dR[3] = {0., 0., 0.};
// store the value fo the function
double F[3] = {0., 0., 0.};
// Store the (sub)-gradient of the function
double A[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
double B[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
// Value of AW+B
double AWplusB[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
// Compute values of Rho (should be here ?)
double rho[3] = {1., 1., 1.};
#ifdef OPTI_RHO
computerho(localproblem, rho);
#endif
// compute the velocity
double velocity[3] = {0., 0., 0.};
for (i = 0; i < 3; i++) velocity[i] = MLocal[i + 0 * 3] * R[0] + qLocal[i]
+ MLocal[i + 1 * 3] * R[1] +
+ MLocal[i + 2 * 3] * R[2] ;
for (inew = 0 ; inew < itermax ; ++inew) // Newton iteration
{
//Update function and gradient
Function(R, velocity, mu, rho, F, A, B);
#ifndef AC_Generated
#ifndef NDEBUG
double Fg[3] = {0., 0., 0.};
double Ag[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
double Bg[9] = {0., 0., 0., 0., 0., 0., 0., 0., 0.};
double AWpB[9];
assert(*rho > 0. && *(rho + 1) > 0. && *(rho + 2) > 0.);
#ifdef AC_STD
frictionContact3D_AlartCurnierFunctionGenerated(R, velocity, mu, rho, Fg, Ag, Bg);
#endif
#ifdef AC_JeanMoreau
frictionContact3D_AlartCurnierJeanMoreauFunctionGenerated(R, velocity, mu, rho, Fg, Ag, Bg);
#endif
sub3(F, Fg);
sub3x3(A, Ag);
sub3x3(B, Bg);
assert(hypot3(Fg) <= 1e-7);
assert(hypot9(Ag) <= 1e-7);
assert(hypot9(Bg) <= 1e-7);
cpy3x3(A, Ag);
cpy3x3(B, Bg);
mm3x3(A, MLocal, AWpB);
add3x3(B, AWpB);
#endif
#endif
// compute -(A MLocal +B)
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
AWplusB[i + 3 * j] = 0.0;
for (k = 0; k < 3; k++)
{
AWplusB[i + 3 * j] -= A[i + 3 * k] * MLocal[k + j * 3];
}
AWplusB[i + 3 * j] -= B[i + 3 * j];
}
}
#ifdef AC_STD
#ifndef NDEBUG
scal3x3(-1., AWpB);
sub3x3(AWplusB, AWpB);
assert(hypot9(AWpB) <= 1e-7);
#endif
#endif
// Solve the linear system
solv3x3(AWplusB, dR, F);
// upate iterates
R[0] += dR[0];
R[1] += dR[1];
R[2] += dR[2];
// compute new residue
for (i = 0; i < 3; i++) velocity[i] = MLocal[i + 0 * 3] * R[0] + qLocal[i]
+ MLocal[i + 1 * 3] * R[1] +
+ MLocal[i + 2 * 3] * R[2] ;
Function(R, velocity, mu, rho, F, NULL, NULL);
dparam[1] = 0.5 * (F[0] * F[0] + F[1] * F[1] + F[2] * F[2]) / (1.0 + sqrt(R[0] * R[0] + R[1] * R[1] + R[2] * R[2])) ; // improve with relative tolerance
/* dparam[2] =0.0;
FrictionContact3D_unitary_compute_and_add_error( R , velocity,mu, &(dparam[2]));*/
if (verbose > 1) printf("----------------------------------- LocalNewtonSolver number of iteration = %i error = %.10e \n", inew, dparam[1]);
if (dparam[1] < Tol)
{
/* printf("----------------------------------- LocalNewtonSolver number of iteration = %i error = %.10e \t error2 = %.10e \n",inew,dparam[1], dparam[2]); */
return 0;
}
}// End of the Newton iteration
/* printf("----------------------------------- LocalNewtonSolver number of iteration = %i error = %.10e \t error2 = %.10e \n",inew,dparam[1], dparam[2]); */
return 1;
}
void uiuc_gear()
{
/*
* Aircraft specific initializations and data goes here
*/
static DATA percent_brake[MAX_GEAR] = /* percent applied braking */
{ 0., 0., 0., 0.,
0., 0., 0., 0.,
0., 0., 0., 0.,
0., 0., 0., 0.
}; /* 0 = none, 1 = full */
static DATA caster_angle_rad[MAX_GEAR] = /* steerable tires - in */
{ 0., 0., 0., 0.,
0., 0., 0., 0.,
0., 0., 0., 0.,
0., 0., 0., 0.
}; /* radians, +CW */
/*
* End of aircraft specific code
*/
/*
* Constants & coefficients for tyres on tarmac - ref [1]
*/
/* skid function looks like:
*
* mu ^
* |
* max_mu | +
* | /|
* sliding_mu | / +------
* | /
* | /
* +--+------------------------>
* | | | sideward V
* 0 bkout skid
* V V
*/
static int it_rolls[MAX_GEAR] =
{ 1, 1, 1, 0,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0
};
static DATA sliding_mu[MAX_GEAR] =
{ 0.5, 0.5, 0.5, 0.3,
0.3, 0.3, 0.3, 0.3,
0.3, 0.3, 0.3, 0.3,
0.3, 0.3, 0.3, 0.3
};
static DATA max_brake_mu[MAX_GEAR] =
{ 0.0, 0.6, 0.6, 0.0,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0
};
static DATA max_mu = 0.8;
static DATA bkout_v = 0.1;
static DATA skid_v = 1.0;
/*
* Local data variables
*/
DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/
// DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
// DATA altitude_local_v[3]; /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
// DATA altitude_body_v[3]; /*altitude vector in body (X,Y,Z)*/
DATA temp3a[3];
// DATA temp3b[3];
DATA tempF[3];
DATA tempM[3];
DATA reaction_normal_force; /* wheel normal (to rwy) force */
DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
DATA forward_mu, sideward_mu; /* friction coefficients */
DATA beta_mu; /* breakout friction slope */
DATA forward_wheel_force, sideward_wheel_force;
int i; /* per wheel loop counter */
/*
* Execution starts here
*/
beta_mu = max_mu/(skid_v-bkout_v);
clear3( F_gear_v ); /* Initialize sum of forces... */
clear3( M_gear_v ); /* ...and moments */
/*
* Put aircraft specific executable code here
*/
percent_brake[1] = Brake_pct[0];
percent_brake[2] = Brake_pct[1];
caster_angle_rad[0] =
(0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
for (i=0; i<MAX_GEAR; i++) /* Loop for each wheel */
{
// Execute only if the gear has been defined
if (!gear_model[i])
{
// do nothing
continue;
}
else
{
/*========================================*/
/* Calculate wheel position w.r.t. runway */
/*========================================*/
/* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
/* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
sub3( D_gear_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
/* then converting to local (North-East-Down) axes... */
multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
/* Runway axes correction - third element is Altitude, not (-)Z... */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
/* Add wheel offset to cg location in local axes */
add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
/* remove Runway axes correction so right hand rule applies */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
/*============================*/
/* Calculate wheel velocities */
/*============================*/
/* contribution due to angular rates */
cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
/* transform into local axes */
multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
/* plus contribution due to cg velocities */
add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
clear3(f_wheel_local_v);
reaction_normal_force=0;
fgSetBool("/gear/gear[0]/wow", false);
fgSetBool("/gear/gear[1]/wow", false);
fgSetBool("/gear/gear[2]/wow", false);
if( HEIGHT_AGL_WHEEL < 0. )
/*the wheel is underground -- which implies ground contact
so calculate reaction forces */
{
//set the property - weight on wheels
// if (i==0)
// {
// fgSetBool("/gear/gear[0]/wow", true);
// }
// if (i==1)
// {
// fgSetBool("/gear/gear[1]/wow", true);
// }
// if (i==2)
// {
// fgSetBool("/gear/gear[2]/wow", true);
// }
/*===========================================*/
/* Calculate forces & moments for this wheel */
/*===========================================*/
/* Add any anticipation, or frame lead/prediction, here... */
/* no lead used at present */
/* Calculate sideward and forward velocities of the wheel
in the runway plane */
cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
+ v_wheel_local_v[1]*sin_wheel_hdg_angle;
v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
- v_wheel_local_v[0]*sin_wheel_hdg_angle;
/* Calculate normal load force (simple spring constant) */
reaction_normal_force = 0.;
reaction_normal_force = kgear[i]*d_wheel_rwy_local_v[2]
- v_wheel_local_v[2]*cgear[i];
/* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
if (reaction_normal_force > 0.) reaction_normal_force = 0.;
/* to prevent damping component from swamping spring component */
/* Calculate friction coefficients */
if(it_rolls[i])
{
forward_mu = (max_brake_mu[i] - muGear[i])*percent_brake[i] + muGear[i];
abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
sideward_mu = sliding_mu[i];
if (abs_v_wheel_sideward < skid_v)
sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
}
else
{
forward_mu=sliding_mu[i];
sideward_mu=sliding_mu[i];
}
/* Calculate foreward and sideward reaction forces */
forward_wheel_force = forward_mu*reaction_normal_force;
sideward_wheel_force = sideward_mu*reaction_normal_force;
if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
/* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
*/
/* Rotate into local (N-E-D) axes */
f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
- sideward_wheel_force*sin_wheel_hdg_angle;
f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
+ sideward_wheel_force*cos_wheel_hdg_angle;
f_wheel_local_v[2] = reaction_normal_force;
/* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
/* Calculate moments from force and offsets in body axes */
cross3( d_wheel_cg_body_v, tempF, tempM );
/* Sum forces and moments across all wheels */
if (tempF[0] != 0.0 || tempF[1] != 0.0 || tempF[2] != 0.0) {
fgSetBool("/gear/gear[1]/wow", true);
}
add3( tempF, F_gear_v, F_gear_v );
add3( tempM, M_gear_v, M_gear_v );
}
}
/* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
/*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
}
}
int main(int argc, char** argv)
{
try
{
nitf_Error e;
// Foo<int>* foo = new Bar<int>();
// delete foo;
nitf_Field* cField = nitf_Field_construct(100, NITF_BCS_N, &e);
nitf::Field field(cField);
{
nitf::Field field2(cField);
}
std::cout << field.isValid() << std::endl;
nitf_BandInfo* cBandInfo = nitf_BandInfo_construct(&e);
assert(cBandInfo);
nitf::BandInfo info(cBandInfo);
std::cout << (int)info.getNumLUTs() << std::endl;
std::cout << "HERE!!!!!!!!!!!!!!!!!!!!" << std::endl;
nitf::SubWindow sub;
nitf::SubWindow sub2 = sub;
sub.setNumRows(5);
nitf::PixelSkip p(1, 1);
sub.setDownSampler(p);
sub.setDownSampler(p);
sub.setDownSampler(p);
sub.setDownSampler(p);
nitf::PixelSkip p2(1, 1);
sub.setDownSampler(p2);
sub.setDownSampler(p);
nitf_SubWindow* subw = nitf_SubWindow_construct(&e);
nitf::SubWindow sub3(subw);
std::cout << sub.getNumRows() << " == " << sub2.getNumRows() << std::endl;
nitf::List list;
nitf::List list2 = list;
{
nitf::FileHeader header;
nitf::FileHeader header2(header.clone());
//should be not equal
std::cout << "Equal? " << (header == header2) << std::endl;
nitf::FileHeader header3(header2);
//these two should be equal
std::cout << "Equal? " << (header3 == header2) << std::endl;
}
nitf::HashTable hash;
nitf::Extensions extensions;
{
nitf::ImageSegment imageSeg;
nitf::ImageSubheader imageSub;
imageSeg.getSubheader().getImageId() = "Test Image";
std::cout << imageSeg.getSubheader().getImageId().toString() << std::endl;
nitf::Field f = imageSeg.getSubheader().getImageId();
std::cout << f.toString() << std::endl;
nitf::ImageSegment imageSeg2 = imageSeg.clone();
nitf::ImageSubheader imageSub2(imageSub.clone());
extensions = imageSub.getExtendedSection();
}
nitf::TextSegment tSeg;
nitf::TextSubheader tSub;
nitf::GraphicSegment gSeg;
nitf::GraphicSubheader gSub;
nitf::DESegment dSeg;
nitf::DESubheader dSub;
nitf::LabelSegment rSeg;
nitf::LabelSubheader rSub;
nitf::TRE tre("JITCID");
//tre.print();
std::cout << "HERE!!!!!" << std::endl;
nitf::Reader reader;
nitf::Writer writer;
//open a file
sys::OS os;
std::vector< std::string > files;
for (int i = 1; i < argc; ++i)
{
if (!os.exists(argv[i]))
std::cout << "Error -> File does not exist: " << argv[i] << std::endl;
else
files.push_back(argv[i]);
}
for (std::vector< std::string >::iterator it = files.begin(); it != files.end(); ++it)
{
nitf::IOHandle handle(*it);
nitf::Reader rdr;
nitf::Record rec = rdr.read(handle);
std::cout << "CODEWORDS: " << rec.getHeader().getSecurityGroup().getCodewords().toString() << std::endl;
rec.getHeader().getSecurityGroup().getCodewords() = "TEST";
std::cout << "CODEWORDS: " << rec.getHeader().getSecurityGroup().getCodewords().toString() << std::endl;
nitf::FileSecurity security;
rec.getHeader().setSecurityGroup(security);
std::cout << "CODEWORDS: " << rec.getHeader().getSecurityGroup().getCodewords().toString() << std::endl;
std::cout << "Num Images: " << rec.getImages().getSize() << std::endl;
}
nitf_SubWindow_destruct(&subw);
nitf_Field_destruct(&cField);
nitf_BandInfo_destruct(&cBandInfo);
}
catch(except::Exception& ex)
{
std::cerr << "ERROR: " << ex.getMessage() << std::endl;
return 1;
}
return 0;
}
int main()
{
DECL_TIMER(T0);
DECL_TIMER(T1);
int info = 0;
int r = -1;
FILE* file = fopen("./data/ACinputs.dat", "r");
unsigned int dim = 0;
double* reactions;
double* velocities;
double *mus;
double *rhos;
r = fscanf(file, "%d\n", &dim);
assert(r > 0);
if (r <= 0) return(r);
reactions = (double *) malloc(3 * dim * sizeof(double));
velocities = (double *) malloc(3 * dim * sizeof(double));
mus = (double *) malloc(dim * sizeof(double));
rhos = (double *) malloc(3 * dim * sizeof(double));
for (unsigned int i = 0; i < dim * 3 ; ++i)
{
r = fscanf(file, "%lf\n", &reactions[i]);
assert(r > 0);
};
for (unsigned int i = 0; i < dim * 3 ; ++i)
{
r = fscanf(file, "%lf\n", &velocities[i]);
assert(r > 0);
};
for (unsigned int k = 0; k < dim ; ++k)
{
r = fscanf(file, "%lf\n", &mus[k]);
assert(r > 0);
};
for (unsigned int i = 0; i < dim * 3 ; ++i)
{
r = fscanf(file, "%lf\n", &rhos[i]);
assert(r > 0);
};
double F1[3], A1[9], B1[9],
F2[3], A2[9], B2[9];
for (unsigned int k = 0; k < dim; ++k)
{
double* p;
p = F1;
OP3(*p++ = NAN);
p = F2;
OP3(*p++ = NAN);
p = A1;
OP3X3(*p++ = NAN);
p = B1;
OP3X3(*p++ = NAN);
p = B2;
OP3X3(*p++ = NAN);
p = A2;
OP3X3(*p++ = NAN);
START_TIMER(T0);
DO(computeAlartCurnierSTDOld(&reactions[k * 3], &velocities[k * 3], mus[k], &rhos[k * 3], F1, A1, B1));
STOP_TIMER(T0);
START_TIMER(T1);
DO(computeAlartCurnierSTD(&reactions[k * 3], &velocities[k * 3], mus[k], &rhos[k * 3], F1, A1, B1));
STOP_TIMER(T1);
PRINT_ELAPSED(T0);
PRINT_ELAPSED(T1);
#ifdef WITH_TIMERS
printf("T1/T0 = %g\n", ELAPSED(T1) / ELAPSED(T0));
#endif
p = F1;
OP3(info |= isnan(*p++));
assert(!info);
p = A1;
OP3X3(info |= isnan(*p++));
assert(!info);
p = B1;
OP3X3(info |= isnan(*p++));
assert(!info);
frictionContact3D_AlartCurnierFunctionGenerated(&reactions[k * 3], &velocities[k * 3], mus[k], &rhos[k * 3], F2, A2, B2);
p = F1;
OP3(info |= isnan(*p++));
assert(!info);
p = A1;
OP3X3(info |= isnan(*p++));
assert(!info);
p = B1;
OP3X3(info |= isnan(*p++));
assert(!info);
sub3(F1, F2);
sub3x3(A1, A2);
sub3x3(B1, B2);
#define EPS 1e-6
p = F2;
OP3(info |= !(*p++ < EPS));
assert(!info);
p = A2;
OP3X3(info |= !(*p++ < EPS));
assert(!info);
p = B2;
OP3X3(info |= !(*p++ < EPS));
assert(!info);
}
free(reactions);
free(velocities);
free(mus);
free(rhos);
fclose(file);
return (info);
}
