void Balaenidae::doDynamics(dBodyID body)
{
Vec3f Ft;
Ft[0]=0;Ft[1]=0;Ft[2]=getThrottle();
dBodyAddRelForce(body,Ft[0],Ft[1],Ft[2]);
dBodyAddRelTorque(body,0.0f,Balaenidae::rudder*1000,0.0f);
if (offshoring == 1) {
offshoring=0;
setStatus(Balaenidae::SAILING);
}
else if (offshoring > 0)
{
// Add a retractive force to keep it out of the island.
Vec3f ap = Balaenidae::ap;
setThrottle(0.0);
Vec3f V = ap*(-10000);
dBodyAddRelForce(body,V[0],V[1],V[2]);
offshoring--;
}
// Buyoncy
//if (pos[1]<0.0f)
// dBodyAddRelForce(me,0.0,9.81*20050.0f,0.0);
dReal *v = (dReal *)dBodyGetLinearVel(body);
dVector3 O;
dBodyGetRelPointPos( body, 0,0,0, O);
dVector3 F;
dBodyGetRelPointPos( body, 0,0,1, F);
F[0] = (F[0]-O[0]);
F[1] = (F[1]-O[1]);
F[2] = (F[2]-O[2]);
Vec3f vec3fF;
vec3fF[0] = F[0];vec3fF[1] = F[1]; vec3fF[2] = F[2];
Vec3f vec3fV;
vec3fV[0]= v[0];vec3fV[1] = v[1]; vec3fV[2] = v[2];
speed = vec3fV.magnitude();
VERIFY(speed, me);
vec3fV = vec3fV * 0.02f;
dBodyAddRelForce(body,vec3fV[0],vec3fV[1],vec3fV[2]);
wrapDynamics(body);
}
void GameObject::Update()
{
double reward = 0;
if (!m_hasAgent) {
return;
}
// reorder the game states
if (m_oldGS != nullptr) {
delete m_oldGS;
m_oldGS = nullptr;
}
m_oldGS = m_newGS;
m_newGS = new GameState;
// construct the game state to pass to an agent
GetGameState(*m_newGS);
reward = 1 / std::max(1.d, m_newGS->m_distanceFromDestination);
// Discretize after reward calculation
m_newGS->discretize();
// get the agent for this object
QLearningAgent *agent = m_gw.m_game.GetAgent(m_id);
// Update the behaviors based on the last action
if (m_oldGS != 0) {
agent->update(*m_oldGS, m_lastAction, *m_newGS, reward);
}
if (m_pathToDest.size() == 0) {
Ogre::Vector3 a = GetLocation();
WorldPos curr = {a.x, a.z, 0};
m_gw.MakeMapPath(curr, m_pathToDest);
}
// here is where I'd pass the game state to an agent if we had one
// layout: turn, acceleration
m_lastAction = agent->getAction(*m_newGS);
std::cout << "Action: " << m_lastAction.m_accelerateMagnitude << ", " << m_lastAction.m_turnMagnitude << std::endl;
const dReal *quat = dBodyGetQuaternion(m_body);
// do physics things
if (quat[1] < 0.05f) {
dBodyAddRelForce(m_body, 0, 0, m_lastAction.m_accelerateMagnitude * m_maxForward);
dBodyAddTorque(m_body, 0, m_lastAction.m_turnMagnitude * m_maxTurn, 0);
}
// reset collision accumulator
m_totalDamage += m_collisionAccum;
m_collisionAccum = 0;
}
IoObject *IoODEBody_addRelForce(IoODEBody *self, IoObject *locals, IoMessage *m)
{
const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
IoODEBody_assertValidBody(self, locals, m);
dBodyAddRelForce(BODYID, x, y, z);
return self;
}
void ODE_ForceHandle::updateActuators() {
if(mHostBody != 0 && mActivateForces->get()) {
dBodyID hostBodyId = mHostBody->getRigidBodyID();
if(hostBodyId != 0) {
if(mApplyRelativeForces->get()) {
dBodyAddForce(hostBodyId, mAppliedForce->getX(), mAppliedForce->getY(), mAppliedForce->getZ());
dBodyAddTorque(hostBodyId, mAppliedTorque->getX(), mAppliedTorque->getY(),mAppliedTorque->getZ());
}
else {
dBodyAddRelForce(hostBodyId, mAppliedForce->getX(), mAppliedForce->getY(), mAppliedForce->getZ());
dBodyAddRelTorque(hostBodyId, mAppliedTorque->getX(), mAppliedTorque->getY(),mAppliedTorque->getZ());
}
}
}
}
void RigidBody::addRelativeForce(const ngl::Vec3 &_f)
{
dBodyAddRelForce(m_id,_f.m_x,_f.m_y,_f.m_z);
}
void SParts::addRelForce(dReal fx, dReal fy, dReal fz)
{
dBodyAddRelForce(m_odeobj->body(), fx, fy, fz);
}
void PhysicsBody::addRelForce(const Vec3f &v)
{
dBodyAddRelForce(_BodyID,v.x(), v.y(), v.z());
}
void cPhysicsObject::AddForceRelativeToObjectKg(const physvec_t& forceKg)
{
dBodyAddRelForce(body, forceKg.x, forceKg.y, forceKg.z);
}
void base::hack_2d(void)
{
const dReal *rot = dBodyGetAngularVel (body);
const dReal *quat_ptr;
dReal quat[4], quat_len;
quat_ptr = dBodyGetQuaternion (body);
quat[0] = quat_ptr[0];
quat[1] = 0;
quat[2] = 0;
quat[3] = quat_ptr[3];
quat_len = sqrt (quat[0] * quat[0] + quat[3] * quat[3]);
quat[0] /= quat_len;
quat[3] /= quat_len;
dBodySetQuaternion (body, quat);
dBodySetAngularVel (body, 0, 0, rot[2]);
//Restricting Rotation To One Axis
//The plane2D stops objects rotating along non-2d axes,
//but it does not compensate for drift
const dReal *rot1 = dBodyGetAngularVel( body );
const dReal *quat_ptr1;
dReal quat1[4], quat_len1;
quat_ptr1 = dBodyGetQuaternion( body );
quat1[0] = quat_ptr1[0];
quat1[1] = 0;
quat1[2] = 0;
quat1[3] = quat_ptr1[3];
quat_len1 = sqrt( quat1[0] * quat1[0] + quat1[3] * quat1[3] );
quat1[0] /= quat_len1;
quat1[3] /= quat_len1;
dBodySetQuaternion( body, quat1 );
dBodySetAngularVel( body, 0, 0, rot1[2] );
dMatrix3 R = { 0, 0, 0, 0, 0, 1};
// 0 0 y
if(!rotatebit) //if there is no set rotatebit then don't make rotate in ode
{
quat1[0] = 0;
quat1[1] = 0;
quat1[2] = 0;
quat1[3] = 1;
dBodySetQuaternion( body, quat1 );
dBodySetAngularVel( body, 0, 0, 0 );
}
static dVector3 pointrelvel;
const dReal *odepos;
const dReal *force;
dBodyGetRelPointVel(body,0,0,0,pointrelvel);
odepos=dBodyGetPosition(body);
force = dBodyGetForce (body);
dBodySetForce(body,force[0],force[1],0.000);
if (odepos[2]>=0.001 || odepos[2]<=-0.001 ){
dBodySetPosition (body,odepos[0],odepos[1], 0.000);
dBodyAddRelForce (body,0,0, -(pointrelvel[2]));
dBodySetTorque (body, 0.00, 0.00, 0.000);
}
}
//all keyboard processing
void base::active_control(void)
{
const dReal *odepos;
static int wj;
static int dj;
static int wall_last;
if(bitd!=2){
if(keystates['d']){
move(20, 80);
last=RIGHT;
if(keystates['c'])
dBodyAddRelForce(body,-100,0,0);
}
if(keystates['a']){
move(-20, 80);
last=LEFT;
if(keystates['z'])
dBodyAddRelForce(body,100,0,0);
}
if(keystates[' ']){
up=check_state();
if (up==0){
dBodyAddForce(body,0.0,40.0,0.0); //if stand
wj=0; //can wj
dj=0; //can dj
wall_last=last;
}
if ((last==UL1 || last==UR1) && !wj){
wj=1;
if(last==UL1 && wall_last!=UL1){
dBodyAddForce(body,10.0,200.0,0.0); //from wall
wj=0;
}
else if(last==UR1 && wall_last!=UR1){
dBodyAddForce(body,-10.0,200.0,0.0); //from wall
wj=0;
}
printf("DJW\n");
wall_last=last;
}
else if (up==2 && !dj){
// dBodyAddForce(body,0.0,180.0,0.0); //for dj
dj=1;
}
}
//---------------------------------
float tmp;
up=check_state();
dBodyGetRelPointVel(body,0,0,0,pointrelvel); //get body's speed
if(last>1 && !up) tmp=pointrelvel[0]/speed;
else if(!up)tmp=0.03; //stand anim speed
else tmp=0.4; //jump anim speed
if(tmp>=0)calc_speed+=tmp;
else calc_speed+=(tmp*-1);
if(up){
char *pd = (char*) dGeomGetData (geom);
if(last==UP || last==LEFT || last==UL1)
if(pd == (char *)'1' && keystates['a'] && pointrelvel[0] > -1 && pointrelvel[0] < 1){
last=UL1;
}
else last=UL;
if(last==DOWN || last==RIGHT || last== UR1)
if(pd == (char *)'1' && keystates['d'] && pointrelvel[0] > -1 && pointrelvel[0] < 1){
last=UR1;
}
else last=UR;
}
if(!keystates['a'] && !keystates['d'] && !up) //idle
// dBodyAddForce(body,-pointrelvel[0]/1.5,0,0); //for active_control _only_!
;
if(pointrelvel[0]<=1 && pointrelvel[0]>=-1 && !up) //stand animation
if(last!=0 && last!=1) last-=2;
if(calc_speed>=1){
translated_val+=(double)1/texture[last].n;
calc_speed=0;
}
if(last_prev!=last) translated_val=0;
last_prev=last;
dGeomSetData (geom, (void*)'0');
}
}
void AvatarGameObj::step_impl() {
dBodyID body = get_entity().get_id();
const Channel* chn;
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::TranslateX);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_STRAFE/MAX_FPS);
dBodyAddRelForce(body, -v, 0.0, 0.0);
}
bool pushing_up = false;
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::TranslateY);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_STRAFE/MAX_FPS);
if (Saving::get().config().invertTranslateY()) {
v = -v;
}
dBodyAddRelForce(body, 0.0, -v, 0.0);
if (v < 0) {
pushing_up = true;
}
}
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::TranslateZ);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_ACCEL/MAX_FPS);
dBodyAddRelForce(body, 0.0, 0.0, -v);
}
const dReal* avel = dBodyGetAngularVel(body);
dVector3 rel_avel;
dBodyVectorFromWorld(body, avel[0], avel[1], avel[2], rel_avel);
// X-turn and x-counterturn
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::RotateY);
if (chn->is_on()) {
float v = -(chn->get_value())*(MAX_TURN/MAX_FPS);
dBodyAddRelTorque(body, 0.0, v, 0.0);
} else {
float cv = rel_avel[1]*-CTURN_COEF/MAX_FPS;
dBodyAddRelTorque(body, 0.0, cv, 0.0);
}
// Y-turn and y-counterturn
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::RotateX);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_TURN/MAX_FPS);
if (Saving::get().config().invertRotateY()) {
v = -v;
}
dBodyAddRelTorque(body, v, 0.0, 0.0);
} else {
float cv = rel_avel[0]*-CTURN_COEF/MAX_FPS;
dBodyAddRelTorque(body, cv, 0.0, 0.0);
}
// Roll and counter-roll
chn = &Input::get_axis_ch(ORSave::AxisBoundAction::RotateZ);
if (chn->is_on()) {
float v = (chn->get_value())*(MAX_ROLL/MAX_FPS);
dBodyAddRelTorque(body, 0.0, 0.0, v);
} else {
float cv = rel_avel[2]*(-CROLL_COEF/MAX_FPS);
dBodyAddRelTorque(body, 0.0, 0.0, cv);
}
// Changing stance between superman-style and upright
if (_attached) {
_uprightness += UPRIGHTNESS_STEP_DIFF;
} else {
_uprightness -= UPRIGHTNESS_STEP_DIFF;
}
if (_uprightness > 1.0) { _uprightness = 1.0; } else if (_uprightness < 0.0) { _uprightness = 0.0; }
update_geom_offsets();
_attached = _attached_this_frame;
// If we are attached, work to keep ourselves ideally oriented to the attachment surface
if (_attached) {
Vector sn_rel = vector_from_world(_sn);
Vector lvel = Vector(dBodyGetLinearVel(body));
Vector lvel_rel = vector_from_world(lvel);
Vector avel = Vector(dBodyGetAngularVel(body));
Vector avel_rel = vector_from_world(avel);
// Apply as much of each delta as we can
// X and Z orientation delta
// TODO Maybe should translate body so that the contact point stays in the same spot through rotation
float a = limit_abs(_zrot_delta, RUNNING_ADJ_RATE_Z_ROT/MAX_FPS);
Vector body_x(vector_to_world(Vector(cos(a), sin(a), 0)));
a = limit_abs(-_xrot_delta, RUNNING_ADJ_RATE_X_ROT/MAX_FPS);
Vector body_y(vector_to_world(Vector(0, cos(a), sin(a))));
dMatrix3 matr;
dRFrom2Axes(matr, body_x.x, body_x.y, body_x.z, body_y.x, body_y.y, body_y.z);
dBodySetRotation(body, matr);
// Y position delta
// If the user is pushing up, set the target point high above the ground so we escape sticky attachment
set_pos(get_pos() + _sn*limit_abs(_ypos_delta + (pushing_up ? RUNNING_MAX_DELTA_Y_POS*2 : 0), RUNNING_ADJ_RATE_Y_POS/MAX_FPS));
// Y linear velocity delta
lvel_rel.y += limit_abs(_ylvel_delta, RUNNING_ADJ_RATE_Y_LVEL/MAX_FPS);
lvel = vector_to_world(lvel_rel);
dBodySetLinearVel(body, lvel.x, lvel.y, lvel.z);
// X and Z angular velocity delta
avel_rel.x += limit_abs(_xavel_delta, RUNNING_ADJ_RATE_X_AVEL/MAX_FPS);
avel_rel.z += limit_abs(_zavel_delta, RUNNING_ADJ_RATE_Z_AVEL/MAX_FPS);
avel = vector_to_world(avel_rel);
dBodySetAngularVel(body, avel.x, avel.y, avel.z);
}
if (_attached_this_frame) {
_attached_this_frame = false;
dGeomEnable(get_entity().get_geom("sticky_attach"));
} else {
dGeomDisable(get_entity().get_geom("sticky_attach"));
}
}