void RagdollDemo::clientMoveAndDisplay() {
#ifdef GRAPHICS
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
extern GLDebugDrawer gDebugDrawer;
#endif
if (m_dynamicsWorld) {
double delta_x = brain->get_output(0);
delta_x *= MOTOR_SCALE;
//printf("delta_x: %f\n", delta_x);
btVector3 agent_position = move_kinematic_body(agent, btVector3(delta_x, 0, 0));
agent->setActivationState(ACTIVE_TAG);
btVector3 target_a_position = move_kinematic_body(target_a, btVector3(target_a_dx, 0, target_a_dz));
btVector3 target_b_position = move_kinematic_body(target_b, btVector3(target_b_dx, 0, target_b_dz));
target_a->setActivationState(ACTIVE_TAG);
target_b->setActivationState(ACTIVE_TAG);
if (create_draw_file) {
draw_fh << agent_position.getX() << " " << agent_position.getZ() << " ";
draw_fh << target_a_position.getX() << " " << target_a_position.getZ() << " ";
draw_fh << target_b_position.getX() << " " << target_b_position.getZ() << std::endl;
}
for (int i = 0; i < NUM_RAYS; i++) {
double angle = (-RAY_ANGLE / 2) + (i * (RAY_ANGLE / (NUM_RAYS - 1)));
btVector3 ray_offset(RAY_LENGTH * sin(angle), 0, RAY_LENGTH * cos(angle));
btVector3 ray_end = agent_position - ray_offset;
int touch = raycast(agent_position, ray_end);
btVector3 ray = ray_end - agent_position;
double sensor = (1.0 - (ray.length() / RAY_LENGTH)) * 10;
ray_sensors[i] = sensor;
#ifdef GRAPHICS
btVector3 color = touch ? btVector3(0, .8, 0) : btVector3(0, 0, 0);
gDebugDrawer.drawLine(agent_position, ray_end, color);
#endif
}
if (!target_a_passed && (target_a_position.getZ() >= AGENT_Z)) {
target_a_passed = true;
target_a_distance = fabs(agent_position.getX() - target_a_position.getX());
}
if (!target_b_passed && (target_b_position.getZ() >= AGENT_Z)) {
target_b_passed = true;
target_b_distance = fabs(agent_position.getX() - target_b_position.getX());
}
if (target_a_passed && target_b_passed) {
double fitness = target_a_distance + target_b_distance;
std::ofstream fh;
fh.open(ERROR_FILENAME);
fh << fitness;
fh.close();
if (create_draw_file) {
draw_fh.close();
}
exit(0);
}
frame_count++;
m_dynamicsWorld->stepSimulation(TIME_STEP);
brain->step(.01);
#ifdef TEST_SPEED
printf("%d %f\n", frame_count, agent_position.getX());
#endif
//optional but useful: debug drawing
m_dynamicsWorld->debugDrawWorld();
} //world exists
#ifdef GRAPHICS
renderme();
glFlush();
glutSwapBuffers();
#endif
}
void RaytestDemo::castRays()
{
static float up = 0.f;
static float dir = 1.f;
//add some simple animation
if (!m_idle)
{
up+=0.01*dir;
if (btFabs(up)>2)
{
dir*=-1.f;
}
btTransform tr = m_dynamicsWorld->getCollisionObjectArray()[1]->getWorldTransform();
static float angle = 0.f;
angle+=0.01f;
tr.setRotation(btQuaternion(btVector3(0,1,0),angle));
m_dynamicsWorld->getCollisionObjectArray()[1]->setWorldTransform(tr);
}
///step the simulation
if (m_dynamicsWorld)
{
m_dynamicsWorld->updateAabbs();
m_dynamicsWorld->computeOverlappingPairs();
btVector3 red(1,0,0);
btVector3 blue(0,0,1);
///all hits
{
btVector3 from(-30,1+up,0);
btVector3 to(30,1,0);
sDebugDraw.drawLine(from,to,btVector4(0,0,0,1));
btCollisionWorld::AllHitsRayResultCallback allResults(from,to);
allResults.m_flags |= btTriangleRaycastCallback::kF_KeepUnflippedNormal;
//kF_UseGjkConvexRaytest flag is now enabled by default, use the faster but more approximate algorithm
allResults.m_flags |= btTriangleRaycastCallback::kF_UseSubSimplexConvexCastRaytest;
m_dynamicsWorld->rayTest(from,to,allResults);
for (int i=0;i<allResults.m_hitFractions.size();i++)
{
btVector3 p = from.lerp(to,allResults.m_hitFractions[i]);
sDebugDraw.drawSphere(p,0.1,red);
}
}
///first hit
{
btVector3 from(-30,1.2,0);
btVector3 to(30,1.2,0);
sDebugDraw.drawLine(from,to,btVector4(0,0,1,1));
btCollisionWorld::ClosestRayResultCallback closestResults(from,to);
closestResults.m_flags |= btTriangleRaycastCallback::kF_FilterBackfaces;
m_dynamicsWorld->rayTest(from,to,closestResults);
if (closestResults.hasHit())
{
btVector3 p = from.lerp(to,closestResults.m_closestHitFraction);
sDebugDraw.drawSphere(p,0.1,blue);
sDebugDraw.drawLine(p,p+closestResults.m_hitNormalWorld,blue);
}
}
}
}
