/*** スタート関数 ***/
static void start()
{
float xyz[3] = { 1.7f, -1.4f, 0.4f};
float hpr[3] = { 144.0f, -4.0f, 0.0f};
dsSetViewpoint(xyz,hpr);
dsSetSphereQuality(3);
}
// simulation loop
static void simLoop (int pause)
{
const dReal *pos1,*R1,*pos2,*R2,*pos3,*R3;
//@a sphere
dsSetColor(1,0,0); // set color (red, green, blue�j a value is between 0 and 1
dsSetSphereQuality(3); // set sphere quality. 3 is enough
pos1 = dBodyGetPosition(sphere.body); // get a body position
R1 = dBodyGetRotation(sphere.body); // get a body rotation matrix
dsDrawSphere(pos1,R1,radius); // draw a sphere
// a cylinder
dsSetColorAlpha (0,1,0,1);
pos2 = dBodyGetPosition(cylinder.body);
R2 = dBodyGetRotation(cylinder.body);
dsDrawCylinder(pos2,R2,length,radius); // draw a cylinder
// a capsule
dsSetColorAlpha (1,1,1,1);
pos2 = dBodyGetPosition(capsule.body);
R2 = dBodyGetRotation(capsule.body);
dsDrawCapsule(pos2,R2,length,radius); // draw a capsule
// a box
dsSetColorAlpha (0,0,1,1);
pos3 = dBodyGetPosition(box.body);
R3 = dBodyGetRotation(box.body);
dsDrawBox(pos3,R3,sides); // draw a box
// a ray
dReal posA[3] = {0, 5, 0}, posB[3]= {0, 5, 1.9};
dsDrawLine(posA,posB); // draw a ray
}
static void start() {
//static float xyz[3] = { 0.2,5.2,1.0};
static float xyz[3] = { 1.0,1.5,1.0};
static float hpr[3] = { 270,0,0};
dsSetViewpoint(xyz,hpr); // 視点，視線の設定
dsSetSphereQuality(3); // 球の品質設定
}
void start()
{
static float xyz[3] = {1.5, -1.5, 0.8}; // View position (x, y, z) [m]
static float hpr[3] = {136.0, 0.0, 0.0}; // View direction （head,pitch,roll)
dsSetViewpoint (xyz,hpr); // Set a view point
dsSetSphereQuality(3);
// ********** Pre-compute a simple trajectory wrt Fixed Leg *************************************
double t0 = 0.0, t1 = 2.0; // Time (initial & final)
bool FixedLeg = RIGHT;
Math::TPoint3D Pq1, Pq2, Pf1, Pf2;
Pq1.x=0.0; Pq1.y=0.0; Pq1.z=(L3+L4+L5+L6+L7+L8); Pq1.vx=0; Pq1.vy=0; Pq1.vz=0; // Hips wrt Fixed Leg: Initial Point
Pq2.x=0.0; Pq2.y=-L1; Pq2.z=(L3+L4+L5+L6+L7+L8)-L1/4; Pq2.vx=0; Pq2.vy=0; Pq2.vz=0; // Hips wrt Fixed Leg: Final Point
//Pq2.x=0.0; Pq2.y=0.0; Pq2.z=(L3+L4+L5+L6+L7+L8)-0.05; Pq2.vx=0; Pq2.vy=0; Pq2.vz=0; // Hips: Final Point
Pf1.x=0.0; Pf1.y=2*L1; Pf1.z=0; Pf1.vx=0; Pf1.vy=0; Pf1.vz=0; // Floating Leg wrt Fixed Leg: Initial Point
Pf2.x=0.0; Pf2.y=2*L1; Pf2.z=0; Pf2.vx=0; Pf2.vy=0; Pf2.vz=0; // Floating Leg wrt Fixed Leg: Final Point
Traj::rComputeHipsMotion(PreComputedAngleL, PreComputedAngleR, target_angleL, target_angleR,
Pq1,Pq2,Pf1,Pf2,t0,t1,TLENG,FixedLeg);
printf("\nAngles: Pre-computed \n");
}
IoObject *IoDrawStuff_dsSetSphereQuality(IoDrawStuff *self, IoObject *locals, IoMessage *m)
{
int n;
n = IoMessage_locals_intArgAt_(m, locals, 0);
dsSetSphereQuality(n);
return self;
}
/* ------------------------
* スタート関数
------------------------ */
static void start()
{
float xyz[3] = { 0.2, 2.5, 4.00}; /* カメラ座標 */
float hpr[3] = { 90.0, -90.0, 0.0}; /* カメラ方向 */
dsSetViewpoint(xyz,hpr);
dsSetSphereQuality(3);
}
/*** シミュレーションの初期化 ***/
void dmInit()
{
dInitODE(); // ODEの初期化
world = dWorldCreate(); // 世界の創造
dWorldSetGravity(world, 0.0,0.0,-9.8); // 重力設定
space = dHashSpaceCreate(0); // 衝突用空間の創造
contactgroup = dJointGroupCreate(0); // ジョイントグループの生成
ground = dCreatePlane(space,0,0,1,0); // 地面（平面ジオメトリ）の生成
dsSetSphereQuality(3);
}
/*** Setting of point of view and camera gaze ***/
void start()
{
#ifndef NOVIZ
xyz[0] = 1.0f;
xyz[1] = -1.2f;
xyz[2] = 0.5f; // point of view[m]
hpr[0] = 121.0f;
hpr[1] = -10.0f;
hpr[2] = 0.0f; // gaze[°]
dsSetViewpoint(xyz,hpr); // set point of view and gaze
dsSetSphereQuality(3);
dsSetCapsuleQuality(7); // increase slows visualization
#endif
}
void start()
{
world = dWorldCreate();
dWorldSetGravity (world,0,0,-9.8);
contact_group = dJointGroupCreate(0);
space = dSimpleSpaceCreate (0);
// first, the ground plane
// it has to coincide with the plane we have in drawstuff
ground = dCreatePlane(space, 0, 0, 1, 0);
// now a ball
dMass m;
dMassSetSphere(&m, 0.1, ball_radius);
ball1_geom = dCreateSphere(space, ball_radius);
ball1_body = dBodyCreate(world);
dGeomSetBody(ball1_geom, ball1_body);
dBodySetMass(ball1_body, &m);
ball2_geom = dCreateSphere(space, ball_radius);
ball2_body = dBodyCreate(world);
dGeomSetBody(ball2_geom, ball2_body);
dBodySetMass(ball2_body, &m);
// tracks made out of boxes
dGeomID trk;
dMatrix3 r1, r2, r3;
dVector3 ro = {0, -(0.5*track_gauge + 0.5*track_width), track_elevation};
dMatrix3 s1, s2, s3;
dVector3 so = {0, 0.5*track_gauge + 0.5*track_width, track_elevation};
dRFromAxisAndAngle(r1, 1, 0, 0, track_angle);
dRFromAxisAndAngle(r2, 0, 1, 0, -track_incl);
dMultiply0_333(r3, r2, r1);
dRFromAxisAndAngle(s1, 1, 0, 0, -track_angle);
dRFromAxisAndAngle(s2, 0, 1, 0, -track_incl);
dMultiply0_333(s3, s2, s1);
trk = dCreateBox(space, track_len, track_width, track_height);
dGeomSetPosition(trk, ro[0], ro[1] + balls_sep, ro[2]);
dGeomSetRotation(trk, r3);
trk = dCreateBox(space, track_len, track_width, track_height);
dGeomSetPosition(trk, so[0], so[1] + balls_sep, so[2]);
dGeomSetRotation(trk, s3);
// tracks made out of trimesh
for (unsigned i=0; i<n_box_verts; ++i) {
dVector3 p;
dMultiply0_331(p, s3, box_verts[i]);
dAddVectors3(p, p, so);
dCopyVector3(track_verts[i], p);
}
// trimesh tracks 2, transform all vertices by s3
for (unsigned i=0; i<n_box_verts; ++i) {
dVector3 p;
dMultiply0_331(p, r3, box_verts[i]);
dAddVectors3(p, p, ro);
dCopyVector3(track_verts[n_box_verts + i], p);
}
// copy face indices
for (unsigned i=0; i<n_box_faces; ++i)
for (unsigned j=0; j<3; ++j) // each face index
track_faces[3*i+j] = box_faces[3*i+j];
for (unsigned i=0; i<n_box_faces; ++i)
for (unsigned j=0; j<3; ++j) // each face index
track_faces[3*(i + n_box_faces)+j] = box_faces[3*i+j] + n_box_verts;
mesh_data = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSimple(mesh_data,
track_verts[0], n_track_verts,
track_faces, 3*n_track_faces);
mesh_geom = dCreateTriMesh(space, mesh_data, 0, 0, 0);
resetSim();
// initial camera position
static float xyz[3] = {-5.9414,-0.4804,2.9800};
static float hpr[3] = {32.5000,-10.0000,0.0000};
dsSetViewpoint (xyz,hpr);
dsSetSphereQuality(3);
}
void do_tests (int argc, char **argv)
{
int i,j;
// process command line arguments
if (argc >= 2) {
graphical_test = atoi (argv[1]);
}
if (graphical_test) {
// do one test gaphically and interactively
if (graphical_test < 1 || graphical_test >= MAX_TESTS ||
!testslot[graphical_test].name) {
dError (0,"invalid test number");
}
printf ("performing test: %s\n",testslot[graphical_test].name);
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = &command;
fn.stop = 0;
fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;
dsSetSphereQuality (3);
dsSetCapsuleQuality (8);
dsSimulationLoop (argc,argv,1280,900,&fn);
}
else {
// do all tests noninteractively
for (i=0; i<MAX_TESTS; i++) testslot[i].number = i;
// first put the active tests into a separate array
int n=0;
for (i=0; i<MAX_TESTS; i++) if (testslot[i].name) n++;
TestSlot **ts = (TestSlot**) malloc (n * sizeof(TestSlot*));
j = 0;
for (i=0; i<MAX_TESTS; i++) if (testslot[i].name) ts[j++] = testslot+i;
if (j != n) dDebug (0,"internal");
// do two test batches. the first test batch has far fewer reps and will
// catch problems quickly. if all tests in the first batch passes, the
// second batch is run.
for (i=0; i<n; i++) ts[i]->failcount = 0;
int total_reps=0;
for (int batch=0; batch<2; batch++) {
int reps = (batch==0) ? TEST_REPS1 : TEST_REPS2;
total_reps += reps;
printf ("testing batch %d (%d reps)...\n",batch+1,reps);
// run tests
for (j=0; j<reps; j++) {
for (i=0; i<n; i++) {
current_test = ts[i]->number;
if (ts[i]->test_fn() != 1) ts[i]->failcount++;
}
}
// check for failures
int total_fail_count=0;
for (i=0; i<n; i++) total_fail_count += ts[i]->failcount;
if (total_fail_count) break;
}
// print results
for (i=0; i<n; i++) {
printf ("%3d: %-30s: ",ts[i]->number,ts[i]->name);
if (ts[i]->failcount) {
printf ("FAILED (%.2f%%) at line %d\n",
double(ts[i]->failcount)/double(total_reps)*100.0,
ts[i]->last_failed_line);
}
else {
printf ("ok\n");
}
}
}
}
