void base::rotate()
{
dReal kx, ky, kz;
if(body)
get_euler(dBodyGetRotation(body), kx, ky, kz);
else
get_euler(dGeomGetRotation(geom), kx, ky, kz);
rotate_angle = -kz*57; //from rad to deg
if((int)rotate_angle == -179)
rotate_angle=179;
}
Datum spheretrans_in(PG_FUNCTION_ARGS)
{
SEuler * se = ( SEuler * ) MALLOC ( sizeof ( SEuler ) ) ;
char * c = PG_GETARG_CSTRING(0);
unsigned char etype[3];
int i;
void sphere_yyparse( void );
init_buffer ( c );
sphere_yyparse();
if ( get_euler ( &se->phi, &se->theta, &se->psi, etype ) ){
for ( i=0; i<3; i++ ){
switch ( i ){
case 0: se->phi_a = etype[i] ; break;
case 1: se->theta_a = etype[i] ; break;
case 2: se->psi_a = etype[i] ; break;
}
}
spheretrans_check(se);
} else {
reset_buffer();
FREE( se );
se = NULL;
elog ( ERROR , "spheretrans_in: parse error" );
}
reset_buffer();
PG_RETURN_POINTER( se );
}
void unit_vehicle::update(int dt, world &w)
{
if (hp <= 0)
return;
if (m_first_update)
{
m_first_update = false;
if (!m_follow.expired())
{
auto f = m_follow.lock();
vec3 diff = get_pos() - f->get_pos();
if (diff.length() < m_params.formation_radius)
m_formation_offset = f->get_rot().rotate_inv(diff);
else
m_formation_offset.set(0, 0, -15.0f);
}
m_ground = (get_pos().y - w.get_height(get_pos().x, get_pos().z)) < 5.0f;
if (m_ground)
{
//ToDo: gear anim
}
else
{
if (m_params.speed_min > 0.01f)
m_vel = get_rot().rotate(vec3::forward() * m_params.speed_cruise);
m_dpos = vec3();
}
return;
}
float kdt = dt * 0.001f;
vec3 target_dir;
bool has_target = false;
if (m_target.expired())
{
if (m_ai > ai_default && m_target_search != search_none)
{
const bool air = m_ai == ai_air_to_air || m_ai == ai_air_multirole,
ground = m_ai == ai_air_to_ground || m_ai == ai_air_multirole;
float min_dist = 10000.0f;
for (int i = 0; i < w.get_planes_count() + w.get_units_count(); ++i)
{
object_ptr t;
if (i < w.get_planes_count())
{
auto p = w.get_plane(i);
if (is_ally(p, w))
continue;
t = p;
}
else
{
if (m_target_search == search_player)
continue;
auto u = w.get_unit(i - w.get_planes_count());
if (is_ally(u))
continue;
t = u;
}
if (!t->is_targetable(air, ground))
continue;
const float dist = (t->get_pos() - get_pos()).length();
if(dist >= min_dist)
continue;
m_target = t;
min_dist = dist;
}
}
}
else
{
if (m_target.lock()->hp <= 0)
m_target.reset();
}
bool not_path_not_follow = false;
if (!m_path.empty())
{
has_target = true;
target_dir = m_path.front() - get_pos();
if (target_dir.length() < m_vel.length() * kdt * m_params.target_radius)
{
if (m_path_loop)
m_path.push_back(m_path.front());
m_path.pop_front();
if (m_path.empty())
has_target = false;
else
target_dir = m_path.front() - get_pos();
}
}
else if (!m_follow.expired())
{
has_target = true;
auto f = m_follow.lock();
target_dir = f->get_pos() + f->get_rot().rotate(m_formation_offset) - (get_pos() + get_vel() * kdt);
//+ f->get_vel() * kdt //player's plane is already updated, ToDo
}
else
not_path_not_follow = true;
if (!m_target.expired())
{
auto t = m_target.lock();
const auto dir = t->get_pos() + t->get_rot().rotate(vec3(0, 0, -20.0)) - (get_pos() + get_vel() * kdt);
if (not_path_not_follow)
{
target_dir = dir;
has_target = true;
}
for (auto &wp: m_weapons)
{
if (wp.cooldown > 0)
{
wp.cooldown -= dt;
continue;
}
if (dir.length() > wp.params.lockon_range)
continue;
wp.cooldown = wp.params.reload_time;
auto m = w.add_missile(wp.params.id.c_str(), wp.mdl);
if (!m)
continue;
m->phys->pos = get_pos();
m->phys->rot = get_rot();
m->phys->vel = get_vel();
m->phys->target_dir = dir;
m->target = m_target;
}
}
if (has_target)
{
const float eps=1.0e-6f;
const float target_dist = target_dir.length();
const vec3 v=target_dir / target_dist;
const float xz_sqdist=v.x*v.x+v.z*v.z;
const auto rot = get_rot();
const auto pyr = rot.get_euler();
const nya_math::angle_rad new_yaw=(xz_sqdist>eps*eps)? (atan2(v.x,v.z)) : pyr.y;
nya_math::angle_rad new_pitch=(fabsf(v.y)>eps)? (-atan2(v.y,sqrtf(xz_sqdist))) : 0.0f;
nya_math::angle_rad new_roll;
if (!m_follow.expired())
new_roll = m_follow.lock()->get_rot().get_euler().z;
vec3 pos = get_pos() + m_vel * kdt;
const float h = w.get_height(pos.x, pos.z);
if ((new_pitch > 0.0f && pos.y <= h + m_params.height_min) || (new_pitch < 0.0f && pos.y >= h + m_params.height_max))
new_pitch = 0.0f;
const auto ideal_rot = quat(new_pitch, new_yaw, new_roll);
const float angle_diff = rot.rotate(vec3::forward()).angle(target_dir).get_deg();
if (fabsf(angle_diff) > 0.001f)
{
const float turn_k = nya_math::clamp((180.0f / angle_diff) * m_params.turn_speed * kdt, 0.0, 1.0);
m_render->mdl.set_rot(quat::slerp(rot, ideal_rot, turn_k));
}
float speed = m_vel.length();
float want_speed = target_dist / kdt;
if (!m_follow.expired())
{
const float target_speed = m_follow.lock()->get_vel().length();
float decel_time = (speed - target_speed) / m_params.decel;
if (target_dist / (speed + m_params.accel * kdt) < decel_time)
want_speed = target_speed;
}
want_speed = nya_math::clamp(want_speed, m_params.speed_min, std::min(m_params.speed_max, m_speed_limit));
speed = tend(speed, want_speed, m_params.accel * kdt, m_params.decel * kdt);
m_vel = get_rot().rotate(vec3(0.0, 0.0, speed));
}
else if (m_params.speed_min < 0.01f)
{
float speed = m_vel.length();
if (speed > 0.0f)
{
speed = tend(speed, 0.0f, m_params.accel * kdt, m_params.decel * kdt);
m_vel = get_rot().rotate(vec3(0.0, 0.0, speed));
}
}
vec3 pos = get_pos() + m_vel * kdt;
const float h = w.get_height(pos.x, pos.z);
pos.y = nya_math::clamp(pos.y, h + m_params.height_min, h + m_params.height_max); //ToDo
set_pos(pos);
}
/*** P control tries to add a force that reduces the distance between
the current joint angles and the desired value in the lookup table THETA.
***/
void Pcontrol()
{
dReal kp = 2.0; //effects how quickly it tries to acheive the desired angle (original=2)
// dReal kp = 10.0; //effects how quickly it tries to acheive the desired angle
dReal fMax = 100.0; //fMax is the max for it can apply trying to acheive the desired angle
//#define ANG_VELOCITY
#ifndef ANG_VELOCITY
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
dReal tmp = dJointGetHingeAngle(leg[segment][i][j].joint);
dReal diff = THETA[segment][i][j] - tmp;
dReal u;
u = kp * diff;
//cout << "\n\n***" << u << "***\n\n\n\n" << endl;
dJointSetHingeParam(leg[segment][i][j].joint, dParamVel,
u);
dJointSetHingeParam(leg[segment][i][j].joint, dParamFMax,
fMax);
}
}
#else //when using angular velocity
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
//dReal tmp = dJointGetHingeAngle(leg[segment][i][j].joint);
//dReal diff = THETA[segment][i][j] - tmp;
//dReal u;
//u = kp * diff;
dReal desiredValue = THETA[segment][i][j];
desiredValue *= 2.25;
//cout << "\n\n***" << desiredValue << "***\n\n\n\n" << endl;
dJointSetHingeParam(leg[segment][i][j].joint, dParamVel,
desiredValue);
dJointSetHingeParam(leg[segment][i][j].joint, dParamFMax,
fMax);
}
}
#endif
#ifdef USE_NLEG_ROBOT
if(segment < BODY_SEGMENTS-1) {
// applying torso joint force
dReal tmp = dJointGetHingeAngle(torso[segment].joint); //joints might start in segment 2
dReal diff = torsoJointDesiredAngle[segment] - tmp;
dReal u;
u = kp * diff;
dJointSetHingeParam(torso[segment].joint, dParamVel, u);
dJointSetHingeParam(torso[segment].joint, dParamFMax, fMax);
}
#endif
}
}
/*** Control of walking ***/
void walk()
{
// int numSheets;
//
// #ifdef HIDDEN_LAYER
// numSheets = 3;
// #else
// numSheets = 2;
// #endif
timeSteps++;
//jmc added to print out joint angles
if(printJointAngles){
static bool doOnce = true;
if(doOnce){
doOnce=false;
//remove the file if it is there already
ofstream jangle_file;
jangle_file.open ("jointAngles.txt", ios::trunc );
jangle_file.close();
}
}
//clear out the network
//LHZ - When Recurrence is turned on, we multiply the input values by the recurrent values that were in
//the input nodes to begin with, if they are all 0, the network will never take a value. So we start it with
//all 1's, since this will act as if ther was no recurrence for the first update.
substrate_pointer->reinitialize();
if(experimentType != 33) //If it is a NEAT experiment, the substrate can have an arbitrary number of layers, so we need to update 1+ExtraActivationUpdates num times
{
substrate_pointer->dummyActivation(); //dummyActivation sets a flag so that the first update does not add on ExtraActivationUdates num updates
}
dReal roll, pitch, yaw;
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
roll = pitch = yaw = 0.0;
const dReal * torsoRotation = dBodyGetRotation(torso[segment].body);
get_euler(torsoRotation, roll, pitch, yaw);
#if LEGSWING_ODE_DEBUG
printf("roll: %f, pitch: %f, yaw: %f\n", roll, pitch, yaw);
#endif
for (int leg_no = 0; leg_no < num_legs; leg_no++) {
for (int joint_no = 0; joint_no < num_joints; joint_no++) {
dReal jangle = dJointGetHingeAngle(leg[segment][leg_no][joint_no].joint) - jointError[segment][leg_no][joint_no]; // hidding joint error for sensor
int currDirectionPositive;
if(jangle-lastJangle[segment][leg_no][joint_no] > 0.00001) currDirectionPositive =1; //we use a small positive number to ignore tinsy values
else currDirectionPositive =0;
//printf("jangle: %e, lastJangle_pneat[leg_no][joint_no]: %e, currDir: %i\n", jangle, lastJangle_pneat[leg_no][joint_no], currDirectionPositive);
if(printJointAngles){
ofstream jangle_file;
jangle_file.open ("jointAngles.txt", ios::app );
jangle_file << jangle << " ";
if(leg_no==num_legs-1 and joint_no==num_joints-1) jangle_file << endl;
jangle_file.close();
}
if(currDirectionPositive and !lastJangleDirectionPositve[segment][leg_no][joint_no]) numDirectionChanges++;
if(!currDirectionPositive and lastJangleDirectionPositve[segment][leg_no][joint_no]) numDirectionChanges++;
//if(visualize_pneat) printf("numDirectionChanges_pneat %i \n", numDirectionChanges_pneat);
lastJangleDirectionPositve[segment][leg_no][joint_no] =currDirectionPositive;
lastJangle[segment][leg_no][joint_no] = jangle;
int xInt =joint_no; //which node we will be setting or getting. x & y are within the sheet, z specifies which sheet (0=input, 1=hidden, 2=output)
int yInt =leg_no+(segment*2);
int zInt =0;
#if LEGSWING_ODE_DEBUG
printf("About to set input (x: %i,y: %i,z: %i) to joint angle: %f \n", xInt, yInt, zInt, jangle);
#endif
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)],jangle);
//<start> if inputting lastDesiredAngle
//xInt = xInt+1; //bump it one, cause the last desired angle goes into the input to the right of the current angle for that joint
//#if LEGSWING_ODE_DEBUG
//printf("about to set input (x: %i,y: %i,z: %i) to last desired joint angle: %f \n", xInt, yInt, zInt, newDesiredAngle[leg_no][joint_no]);
//#endif
//substrate_pointer->setValue(
// nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)],
// newDesiredAngle[segment][leg_no][joint_no]
// );
}
//set whether the leg is touching the ground
int xInt = 3; //putting touch just to the right of the 4th legs lastAngle
int yInt = leg_no+(segment*2);
int zInt = 0;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], thisLegIsTouching[segment][leg_no]);
}
#ifndef LEGSWING_ODE_DEBUG
assert(num_legs==4); //warning: only works with four legs
printf("Legs touching: FrontLeft (%i), BackLeft (%i), BackRight (%i), FrontRight (%i)\n", thisLegIsTouching[segment][0],thisLegIsTouching[segment][1], thisLegIsTouching[segment][2], thisLegIsTouching[segment][3]);
#endif
//printf("timeStepDivisor_pneat: %f\n", timeStepDivisor_pneat);
// use if evolving sine period
// if(fabs(timeStepDivisor_pneat<1)) sineOfTimeStep =0; //prevent divide by zero errors, and allow them to silence the sine wave
// else sineOfTimeStep = sin(dReal(timeSteps_pneat)/timeStepDivisor_pneat)*M_PI;
dReal sineOfTimeStep = sin(dReal(timeSteps)/50.0)*M_PI;
#ifdef USE_EXTRA_NODE
dReal negSineOfTimeStep = (-1.0 * sineOfTimeStep);
#endif
if(visualize and sineOfTimeStep>.9999*M_PI) printf("****************************************************\n");
if(visualize and sineOfTimeStep<-.9999*M_PI) printf("---------------------------------------------------\n");
//printf("sineOfTimeStep: %f\n", sineOfTimeStep);
#ifndef USE_NLEG_ROBOT
//<start> use to spread the PRYS horizontally
//insert the roll, pitch and yaw to the 7th-9th columns of the 1st row
//int yInt = 0;
//int zInt = 0;
//dReal insertValue;
//
//for(int q=0;q<3;q++){
// int xInt = 7+q;
// if (q==0) insertValue = roll;
// else if (q==1) insertValue = pitch;
// else insertValue = yaw;
// substrate_pointer->setValue(
// nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)],
// insertValue
// );
//}
//<ent> use to spread the PRYS horizontally
//<start> to spread PRYS vertically
//insert the roll, pitch and yaw to the 1st/0th-4th/3rd row of the 5th/4th columns
int xInt = 4;
int zInt = 0;
dReal insertValue;
for(int q=0;q<3;q++){
int yInt = q;
if (q==0) insertValue = roll;
else if (q==1) insertValue = pitch;
else insertValue = yaw;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], insertValue);
}
//<end>
//insert a sine wave based on timeSteps
xInt = 4;
int yInt = 3;
zInt = 0;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], sineOfTimeStep);
#else // USE_NLEG_ROBOT
//<start> to spread PRYS vertically
//insert the roll, pitch and yaw to the 1st/0th-4th/3rd row of the 5th/4th columns
int xInt = 4;
int zInt = 0;
dReal insertValue;
//roll
int yInt = segment*2;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], roll);
//pitch
yInt = (segment*2)+1;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], pitch);
//yaw
xInt = 5;
yInt = segment*2;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], yaw);
//<end>
if(segment < 1)
{
//insert a sine wave based on timeSteps
yInt = segment*2+1;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], sineOfTimeStep);
} else
{
dReal torsoAngle = dJointGetHingeAngle(torso[segment-1].joint); //TODO: does torso zero have a valid joint?
yInt = segment*2+1;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], torsoAngle);
}
#endif
#if SUBSTRATE_DEBUG
cout << "State of the nodes pre-update\n";
for (int z=0;z<numSheets;z++)
{
cout << "Printing Layer " << z << "************************************************" << endl;
for (int y1=0;y1<numNodesYglobal;y1++)
{
for (int x1=0;x1<numNodesXglobal;x1++)
{
//cout << "(x,y,z): (" << x1 << "," << y1 << "," << z << "): " ;
cout << setw(12) << setiosflags(ios::right) << fixed <<
double(substrate_pointer->getValue(
nameLookupGlobal[HCUBE::Node(x1,y1,z)]
));
}
cout << endl;
}
}
cout << "Updating Substrate\n";
//CREATE_PAUSE(string("Line Number: ")+toString(__LINE__));
#endif
//have to update the network once for each layer of *links* (or NumLayers -1)
#if SUBSTRATE_DEBUG
for (int a=0;a< numSheets - 1 ;a++)
{
substrate_pointer->update(1);
cout << "State of the nodes post-update: " << a << endl;
for (int z=0;z<numSheets;z++)
{
cout << "Printing Layer " << z << endl;
for (int y1=0;y1<numNodesYglobal;y1++)
{
for (int x1=0;x1<numNodesXglobal;x1++)
{
//cout << "(x,y,z): (" << x1 << "," << y1 << "," << z << "): " ;
cout << double(substrate_pointer->getValue(nameLookupGlobal[HCUBE::Node(x1,y1,z)]));
cout << " ";
}
cout << endl;
}
}
}
#else
substrate_pointer->update(numSheets-1);
#endif
}
for(int segment = 0; segment < BODY_SEGMENTS; ++segment){
for (int leg_no = 0; leg_no < num_legs; leg_no++) {
for (int joint_no = 0; joint_no < num_joints; joint_no++) {
int xInt =joint_no*2; //which node we will be setting or getting. x & y are within the sheet, z specifies which sheet (0=input, 1=hidden, 2=output)
int yInt =leg_no;
int zInt = numSheets-1; //2 (or numSheets - 1) to get the output
newDesiredAngle[segment][leg_no][joint_no] = M_PI*double(substrate_pointer->getValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)]));
#if LEGSWING_ODE_DEBUG
printf("newDesiredAngle is: %f\n", newDesiredAngle[segment][leg_no][joint_no]);
#endif
THETA[segment][leg_no][joint_no] = newDesiredAngle[segment][leg_no][joint_no] + jointError[segment][leg_no][joint_no]; // adding joint error
}
}
#ifdef USE_NLEG_ROBOT
if(segment < BODY_SEGMENTS-1) {
//get torso joint angle
int xInt = 5;
int yInt = segment*2+1;
int zInt = numSheets - 1;
torsoJointDesiredAngle[segment] = M_PI*double(substrate_pointer->getValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)]));
}
#endif
//get the timeStepDivisor
// xInt =10; //which node we will be setting or getting. x & y are within the sheet, z specifies which sheet (0=input, 1=hidden, 2=output)
// yInt =1;
// zInt = 2; //2 to get the output
// timeStepDivisor = 100.0*double(substrate_pointer->getValue(
// nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)]));
//if(visualize)printf("timeStepDivisor!: %f\n", timeStepDivisor);
const dReal * torsoPosition = dBodyGetPosition(torso[segment].body);
if(fabs(torsoPosition[0]) > maxXdistanceFrom0[segment]){
maxXdistanceFrom0[segment] = fabs(torsoPosition[0]);
#ifndef LEGSWING_ODE_DEBUG
printf("new maxX::::::::::::::::::::::::::::::::::::::::::::::: %f\n", maxXdistanceFrom0);
#endif
}
if(fabs(torsoPosition[1]) > maxYdistanceFrom0[segment]){
maxYdistanceFrom0[segment] = fabs(torsoPosition[1]);
#ifndef LEGSWING_ODE_DEBUG
printf("new maxY:!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! %f\n", maxYdistanceFrom0);
#endif
}
currentXdistanceFrom0[segment] = fabs(torsoPosition[0]);
currentYdistanceFrom0[segment] = fabs(torsoPosition[1]);
}
Pcontrol();
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
for(int z=0;z<num_legs;z++) thisLegIsTouching[segment][z]=0; //reset these flags before checking the new state of things
}
if(fitness_function == 2) {
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
totalForce[0] += dBodyGetForce(leg[segment][i][j].body)[0];
totalForce[1] += dBodyGetForce(leg[segment][i][j].body)[1];
totalForce[2] += dBodyGetForce(leg[segment][i][j].body)[2];
}
}
totalForce[0] += dBodyGetForce(torso[segment].body)[0];
totalForce[1] += dBodyGetForce(torso[segment].body)[1];
totalForce[2] += dBodyGetForce(torso[segment].body)[2];
}
}
}
