void ENVS::Viewpoint_Get(void) {
dsGetViewpoint(xyz,hpr);
printf("%3.3f %3.3f %3.3f %3.3f %3.3f %3.3f\n",
xyz[0],xyz[1],xyz[2],hpr[0],hpr[1],hpr[2]);
/*** キーボードコマンドの処理関数 ***/
static void command(int cmd)
{
switch (cmd)
{
case '1':
float xyz1[3],hpr1[3];
dsGetViewpoint(xyz1,hpr1);
printf("xyz=%4.2f %4.2f %4.2f ",xyz1[0],xyz1[1],xyz1[2]);
printf("hpr=%6.2f %6.2f %5.2f \n",hpr1[0],hpr1[1],hpr1[2]);
break;
case 'k':
wheel[3].v -= 0.8;
break; // 後進
case 's':
wheel[0].v = wheel[1].v = wheel[2].v = wheel[3].v = 0.0; // 停止
break;
case 'x': // ゴロシュート
{
const dReal *bp = dBodyGetPosition(ball.body);
const dReal *p = dBodyGetPosition(wheel[0].body);
const dReal *R = dBodyGetRotation(base.body);
dReal dist = sqrt(pow(bp[0]-p[0],2)+pow(bp[1]-p[1], 2));
if (dist < 0.3)
{
dReal vmax = POWER * 0.01 * 8.0;
dBodySetLinearVel(ball.body,vmax * R[0],vmax * R[4], 0.0);
// printf("z:vx=%f vy=%f \n",vmax*R[0],vmax*R[4]);
}
}
break;
case 'l': // ループシュート
{
const dReal *bp = dBodyGetPosition(ball.body);
const dReal *p = dBodyGetPosition(wheel[0].body);
const dReal *R = dBodyGetRotation(base.body);
dReal dist = sqrt(pow(bp[0]-p[0],2)+pow(bp[1]-p[1],2));
if (dist < 1.0)
{
dReal vmax45 = POWER * 0.01 * 8.0 / sqrt(2.0);
dBodySetLinearVel(ball.body,vmax45 * R[0],vmax45 * R[4], vmax45);
// printf("z:vx=%f vy=%f \n",vmax*R[0],vmax*R[4]);
}
}
break;
case 'b':
dBodySetPosition(ball.body,0,0,0.14+offset_z);
dBodySetLinearVel(ball.body,0,0,0);
dBodySetAngularVel(ball.body,0,0,0);
break;
}
}
IoObject *IoDrawStuff_dsGetViewpoint(IoDrawStuff *self, IoObject *locals, IoMessage *m)
{
float xyz[3];
float hpr[3];
vec3f v1, v2;
IoSeq *list_xyz = IoMessage_locals_vectorArgAt_(m, locals, 0);
IoSeq *list_hpr = IoMessage_locals_vectorArgAt_(m, locals, 1);
dsGetViewpoint(xyz, hpr);
v1.x = xyz[0]; v1.y = xyz[1]; v1.z = xyz[2];
v2.x = hpr[0]; v2.y = hpr[1]; v2.z = hpr[2];
memcpy(IoSeq_rawBytes(list_xyz), &v1, sizeof(vec3f));
memcpy(IoSeq_rawBytes(list_hpr), &v2, sizeof(vec3f));
return self;
}
void start()
{
// adjust the starting viewpoint a bit
float xyz[3],hpr[3];
dsGetViewpoint (xyz,hpr);
hpr[0] += 7;
dsSetViewpoint (xyz,hpr);
geoms[0]=dCreateConvex (space,
planes,
planecount,
points,
pointcount,
polygons);
dGeomSetPosition (geoms[0],0,0,0.25);
geoms[1]=dCreateConvex (space,
planes,
planecount,
points,
pointcount,
polygons);
dGeomSetPosition (geoms[1],0.25,0.25,0.70);
}
void start()
{
// adjust the starting viewpoint a bit
float xyz[3],hpr[3];
dsGetViewpoint (xyz,hpr);
hpr[0] += 7;
dsSetViewpoint (xyz,hpr);
convex[0]=dCreateConvex (space,
planes,
planecount,
points,
pointcount,
polygons);
convex[1]=dCreateConvex (space,
planes,
planecount,
points,
pointcount,
polygons);
boxes[0]=dCreateBox(space,0.5,0.5,0.5);
boxes[1]=dCreateBox(space,0.5,0.5,0.5);
geoms=convex;
dMatrix3 m1 = { 1,0,0,0,0,1,0,0,0,0,1,0 };
dMatrix3 m2 = { 1,0,0,0,0,1,0,0,0,0,1,0 };
#if 0
dGeomSetPosition (convex[0],
0.0,
0.0,
0.25);
dGeomSetPosition (convex[1],
geom1pos[0],
geom1pos[1],
geom1pos[2]);
dQtoR (geom0quat, m1);
dGeomSetRotation (convex[0],m1);
dQtoR (geom1quat, m2);
dGeomSetRotation (convex[1],m2);
dGeomSetPosition (boxes[0],
0.0,
0.0,
0.25);
dGeomSetPosition (boxes[1],
geom1pos[0],
geom1pos[1],
geom1pos[2]);
dQtoR (geom0quat, m1);
dGeomSetRotation (boxes[0],m1);
dQtoR (geom1quat, m2);
dGeomSetRotation (boxes[1],m2);
#else
dGeomSetPosition (convex[0],
fixed_pos_0[0],
fixed_pos_0[1],
fixed_pos_0[2]);
dGeomSetPosition (convex[1],
fixed_pos_1[0],
fixed_pos_1[1],
fixed_pos_1[2]);
dGeomSetRotation (convex[0],fixed_rot_0);
dGeomSetRotation (convex[1],fixed_rot_1);
dGeomSetPosition (boxes[0],
fixed_pos_0[0],
fixed_pos_0[1],
fixed_pos_0[2]);
dGeomSetPosition (boxes[1],
fixed_pos_1[0],
fixed_pos_1[1],
fixed_pos_1[2]);
dGeomSetRotation (boxes[0],fixed_rot_0);
dGeomSetRotation (boxes[1],fixed_rot_1);
#endif
}
void nearCallback(void *, dGeomID a, dGeomID b)
{
const unsigned max_contacts = 8;
dContact contacts[max_contacts];
if (!dGeomGetBody(a) && !dGeomGetBody(b))
return; // don't handle static geom collisions
int n = dCollide(a, b, max_contacts, &contacts[0].geom, sizeof(dContact));
//clog << "got " << n << " contacts" << endl;
/* Simple contact merging:
* If we have contacts that are too close with the same normal, keep only
* the one with maximum depth.
* The epsilon that defines what "too close" means can be a heuristic.
*/
int new_n = 0;
dReal epsilon = 1e-1; // default
/* If we know one of the geoms is a sphere, we can base the epsilon on the
* sphere's radius.
*/
dGeomID s = 0;
if ((dGeomGetClass(a) == dSphereClass && (s = a)) ||
(dGeomGetClass(b) == dSphereClass && (s = b))) {
epsilon = dGeomSphereGetRadius(s) * 0.3;
}
for (int i=0; i<n; ++i) {
// this block draws the contact points before merging, in red
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(1, 0, 0);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.008);
// let's offset the line a bit to avoid drawing overlap issues
float xyzf[3], hprf[3];
dsGetViewpoint(xyzf, hprf);
dVector3 xyz = {dReal(xyzf[0]), dReal(xyzf[1]), dReal(xyzf[2])};
dVector3 v;
dSubtractVectors3(v, contacts[i].geom.pos, xyz);
dVector3 c;
dCalcVectorCross3(c, v, contacts[i].geom.pos);
dSafeNormalize3(c);
dVector3 pos1;
dAddScaledVectors3(pos1, contacts[i].geom.pos, c, 1, 0.005);
dVector3 pos2;
dAddScaledVectors3(pos2, pos1, contacts[i].geom.normal, 1, 0.05);
dsDrawLine(pos1, pos2);
// end of contacts drawing code
int closest_point = i;
for (int j=0; j<new_n; ++j) {
dReal alignment = dCalcVectorDot3(contacts[i].geom.normal, contacts[j].geom.normal);
if (alignment > 0.99 // about 8 degrees of difference
&&
dCalcPointsDistance3(contacts[i].geom.pos, contacts[j].geom.pos) < epsilon) {
// they are too close
closest_point = j;
//clog << "found close points: " << j << " and " << i << endl;
break;
}
}
if (closest_point != i) {
// we discard one of the points
if (contacts[i].geom.depth > contacts[closest_point].geom.depth)
// the new point is deeper, copy it over closest_point
contacts[closest_point] = contacts[i];
} else
contacts[new_n++] = contacts[i]; // the point is preserved
}
//clog << "reduced from " << n << " to " << new_n << endl;
n = new_n;
for (int i=0; i<n; ++i) {
contacts[i].surface.mode = dContactBounce | dContactApprox1 | dContactSoftERP;
contacts[i].surface.mu = 10;
contacts[i].surface.bounce = 0.2;
contacts[i].surface.bounce_vel = 0;
contacts[i].surface.soft_erp = 1e-3;
//clog << "depth: " << contacts[i].geom.depth << endl;
dJointID contact = dJointCreateContact(world, contact_group, &contacts[i]);
dJointAttach(contact, dGeomGetBody(a), dGeomGetBody(b));
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(0, 0, 1);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.01);
dsSetColor(0, 1, 0);
dVector3 pos2;
dAddScaledVectors3(pos2, contacts[i].geom.pos, contacts[i].geom.normal, 1, 0.1);
dsDrawLine(contacts[i].geom.pos, pos2);
}
//clog << "----" << endl;
}
void Simulation::command(int cmd)
{
dMass mass;
static int view; // which view was set last
static std::string input = ""; // input string ("" initially)
//static dJointID tagging_joint = 0; // joint that keeps one link static
if (cmd >= 'A' && cmd <= 'B')
cmd = cmd + 'a' - 'A'; // lower case
if (mode == KeyMode) {
switch (cmd) {
case keyWriteFrame:
mywriteframes ^= 1;
break;
case keyCreateMainBody:
if (car->body_obj == 0) {
car->createBody(center);
objbin.Add(car->body_obj);
};
break;
// switch mode to input
case keyInputMode:
mode = InputMode;
std::cout << "input: ";
std::cout.flush();
break;
// abandon ship
case keyBreakWheel:
car->breakWheel();
break;
// change speed
case keyZeroSpeed:
s_speed[0] = 0;
s_speed[1] = 0;
break;
case keyRightSpeedIncrease:
s_speed[1] += 0.1;
break;
case keyRightSpeedDecrease:
s_speed[1] -= 0.1;
break;
case keyRightSpeedMax:
s_speed[1] = max_speed;
break;
case keyRightSpeedMin:
s_speed[1] = min_speed;
break;
case keyLeftSpeedIncrease:
s_speed[0] += 0.1;
break;
case keyLeftSpeedDecrease:
s_speed[0] -= 0.1;
break;
case keyLeftSpeedMax:
s_speed[0] = max_speed;
break;
case keyLeftSpeedMin:
s_speed[0] = min_speed;
break;
case keyBothSpeedIncrease:
s_speed[0] += 0.1;
s_speed[1] += 0.1;
break;
case keyBothSpeedDecrease:
s_speed[0] -= 0.1;
s_speed[1] -= 0.1;
break;
case keyBothSpeedMax:
s_speed[0] = max_speed;
s_speed[1] = max_speed;
break;
case keyBothSpeedMin:
s_speed[0] = min_speed;
s_speed[1] = min_speed;
break;
// do the twitch
case keyTwitchToggle:
twitch_flag = 1 - twitch_flag;
break;
case keyTwitchHertzHalf:
twitch_hertz /= 2.0;
break;
case keyTwitchHertzDouble:
twitch_hertz *= 2.0;
break;
// camera views change
case keyAimCamera:
aimCamera(camera_object);
break;
case keyLockCameraToVehicle: // only pos, not orientation so far
lockCamera = 1 - lockCamera;
float xyz[3], hpr[3];
dsGetViewpoint(xyz, hpr);
dBodyGetPosRelPoint(camera_object, xyz[0], xyz[1], xyz[2], cameraRelVec); // returns point in body relative coordinates
break;
case keyIncreaseViewByTwoMod:
view = (view + 2) % set_view(-1);
set_view(view);
break;
case keyIncreaseViewByOneMod:
view = (view + 1) % set_view(-1);
set_view(view);
break;
// show speed and wanted speed (uncorrect when in twitch mode)
case keyDisplayWheels:
for (int i = 0 ; i < 4 ; ++i)
car->wheel_obj[i]->toggleDrawFlag();
case keyPrintSprocketMass:
car->getSprocketMass(&mass);
printf("mass %3.3f %3.3f %3.3f %3.3f\n",
mass.mass, mass.I[0], mass.I[5],
mass.I[10]);
break;
case keyPrintManyVariables:
printf
("s_speed %3.3f | actual %3.3f | hertz %3.3f | time %3.3f | lv %d\n",
s_speed[0], car->getSprocketSpeed(),
twitch_hertz, dStopwatchTime(&timer), view);
break;
case keyEnterMovieMode:
enterMovieMode();
automat = new AutoEventFinish(simTime, event_time, m_myMachine, finish_time, m_exitMachine, camera_object); // create automat
break;
};
if (cmd >= '0' && cmd < ('0' + LINK_PARTS))
Chain::showbit[cmd - '0'] =
1 - Chain::showbit[cmd - '0'];
for (int i = 0; i < 2; ++i)
s_speed[i] =
BRACKET(s_speed[i], min_speed, max_speed);
} else if (mode == InputMode) {
// switch mode to single key
if (cmd == ' ') {
mode = KeyMode;
//process (input);
std::istringstream s(input);
int tagged_link;
s >> tagged_link;
/*
tagged_link = BRACKET(tagged_link, 0, chain_obj[0]->b_num - 1);
if (tagging_joint != 0) // delete old
dJointDestroy(tagging_joint);
tagging_joint = dJointCreateFixed(world, 0);
dJointAttach(tagging_joint, chain_obj[0]->body[tagged_link], 0);
dJointSetFixed(tagging_joint);
*/
std::cout << "\n";
// reset input
input = "";
} else {
void Simulation::simLoop(int pause)
{
static dReal last_capture_time = 0.0;
static dReal capture_dt = 1.0 / 100; // for 100 fps in simulation time
// profiling variables
this->callback_count = this->not_ground_count = this->ground_count = 0;
// use timer to track time (you need to stop the watch to update the time.
// apparently the digits change so fast it can't be read :-)
//dStopwatchStop(&this->timer);
//dStopwatchStart(&this->timer);
// if there is an automat running, run it. if it is done, delete and *zero* it
this->mainMovie();
// capture a picture every capture_dt second
#ifdef ODE_FRAME_PATCH
clearWriteFrames();
#endif
//cout << "debug: " << (dStopwatchTime(&timer)) << "\n";
if (mywriteframes && (simTime - last_capture_time > capture_dt)) {
#ifdef ODE_FRAME_PATCH
setWriteFrames();
#endif
PEXP(this->simTime);
last_capture_time = this->simTime;
}
// apply forces
for (int i = 0; i < 2; ++i) {
if (twitch_flag) {
this->car->setSpeed(i,
(int) ((dStopwatchTime(&timer) *
twitch_hertz)) %
2 ? min_speed : max_speed);
} else {
this->car->setSpeed(i, s_speed[i]);
}
}
// Accounting
// - zero number of contacts in each chain
this->car->chain_obj[0]->ZeroUserData();
this->car->chain_obj[1]->ZeroUserData();
// oh, if this was ruby!
// car.chains.each { |chain| chain.ZeroUserData() }
// time step
if (!pause) {
dSpaceCollide(space, 0, &::nearCallback);
//if (contactgroup->num) PEXP(contactgroup->num);
// Test fast step
if (use_step_fast)
//dWorldStepFast1(world, step, iterations);
dWorldQuickStep(world, step);
else
dWorldStep(world, step);
simTime += step;
// remove all contact joints
dJointGroupEmpty(contactgroup);
}
// profiling
//printf("debug: callback %4d not_ground %4d ground %4d\n", callback_count,
// not_ground_count, ground_count);
// draw
//dsSetTexture (DS_WOOD);
// set camera if it is tracking the body
if (lockCamera) {
float xyz[3], hpr[3];
dVector3 newpoint;
dsGetViewpoint(xyz, hpr);
dBodyGetRelPointPos(camera_object, cameraRelVec[0],
cameraRelVec[1], cameraRelVec[2], newpoint);
xyz[0] = newpoint[0]; xyz[1] = newpoint[1]; xyz[2] = newpoint[2];
dsSetViewpoint(xyz, hpr);
}
if (objbin.first)
objbin.first->DrawChain();
}
// called when a key pressed
void command(int cmd)
{
static float xyz[3];
static float hpr[3];
switch (cmd) {
case 'e':
occasional_error ^= 1;
break;
case 'v':
dsGetViewpoint (xyz, hpr);
xyz[0] += 0.1;
xyz[1] += 0.1;
dsSetViewpoint (xyz,hpr);
break;
case 'r':
xyz[0] = 0.75;
xyz[1] = 0.90;
xyz[2] = 0.5;
hpr[0] = -125.0;
hpr[1] = -20.00;
hpr[2] = 0.0;
dsSetViewpoint (xyz,hpr);
break;
case 'x':
dsGetViewpoint(xyz, hpr);
hpr[0] = 180.0;
hpr[1] = 0.0;
hpr[2] = 0.0;
dsSetViewpoint (xyz,hpr);
break;
case 'y':
dsGetViewpoint(xyz, hpr);
hpr[0] = -90.0;
hpr[1] = 0.0;
hpr[2] = 0.0;
dsSetViewpoint (xyz,hpr);
break;
case 'a':
dsGetViewpoint (xyz, hpr);
hpr[0] = 0.0;
hpr[1] = -90.0;
hpr[2] = 0.0;
xyz[0] = 0.5;
xyz[1] = 0.0;
xyz[2] = 2.0;
dsSetViewpoint (xyz,hpr);
break;
case 'z':
dsGetViewpoint (xyz, hpr);
hpr[0] = 0.0;
hpr[1] = -90.0;
hpr[2] = 0.0;
xyz[0] = 0.0;
xyz[1] = 0.0;
xyz[2] = 2.5;
dsSetViewpoint (xyz,hpr);
break;
case 'q':
xyz[0] = 0.00;
xyz[1] = 0.10;
xyz[2] = 0.5;
hpr[0] = 0.0;
hpr[1] = 0.00;
hpr[2] = -90.0;
dsSetViewpoint (xyz,hpr);
break;
//new
case 'p':
dsGetViewpoint (xyz, hpr);
hpr[0] = -90.0;
hpr[1] = 0.0;
hpr[2] = 0.0;
xyz[0] = .0;
xyz[1] = 1.0;
xyz[2] = 0.4;
dsSetViewpoint (xyz,hpr);
break;
case 'o':
dsGetViewpoint (xyz, hpr);
hpr[0] = 0.0;
hpr[1] = 0.0;
hpr[2] = 0.0;
xyz[0] = -0.9;
xyz[1] = 0.0;
xyz[2] = 0.2;
dsSetViewpoint (xyz,hpr);
break;
case 'i':
dsGetViewpoint (xyz, hpr);
hpr[0] = 90.0;
hpr[1] = 0.0;
hpr[2] = 0.0;
xyz[0] = 0.0;
xyz[1] = -1.0;
xyz[2] = 0.3;
dsSetViewpoint (xyz,hpr);
break;
case 'u':
dsGetViewpoint (xyz, hpr);
hpr[0] = 0.0;
hpr[1] = 0.0;
hpr[2] = 0.0;
xyz[0] = -0.9;
xyz[1] = 0.0;
xyz[2] = 0.02;
dsSetViewpoint (xyz,hpr);
break;
}
}
