void cPhysicsObject::Update(durationms_t currentTime)
{
if (bDynamic) {
const dReal* p0 = nullptr;
const dReal* r0 = nullptr;
dQuaternion q;
if (bBody) {
p0 = dBodyGetPosition(body);
r0 = dBodyGetQuaternion(body);
const dReal* v0 = dBodyGetLinearVel(body);
//const dReal *a0=dBodyGetAngularVel(body);
v[0] = v0[0];
v[1] = v0[1];
v[2] = v0[2];
} else {
p0 = dGeomGetPosition(geom);
dGeomGetQuaternion(geom, q);
r0 = q;
// These are static for the moment
v[0] = 0.0f;
v[1] = 0.0f;
v[2] = 0.0f;
}
ASSERT(p0 != nullptr);
ASSERT(r0 != nullptr);
position.Set(p0[0], p0[1], p0[2]);
rotation.SetFromODEQuaternion(r0);
}
}
void GameObject::Render()
{
const dReal *pos = dBodyGetPosition(m_body);
const dReal *rot = dBodyGetQuaternion(m_body);
m_sceneNode->setPosition(Ogre::Vector3(pos[0], pos[1], pos[2]));
m_sceneNode->setOrientation(Ogre::Quaternion(rot[0], rot[1], rot[2], rot[3]));
}
void ompl::control::OpenDEStateSpace::readState(base::State *state) const
{
auto *s = state->as<StateType>();
for (int i = (int)env_->stateBodies_.size() - 1; i >= 0; --i)
{
unsigned int _i4 = i * 4;
const dReal *pos = dBodyGetPosition(env_->stateBodies_[i]);
const dReal *vel = dBodyGetLinearVel(env_->stateBodies_[i]);
const dReal *ang = dBodyGetAngularVel(env_->stateBodies_[i]);
double *s_pos = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
++_i4;
double *s_vel = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
++_i4;
double *s_ang = s->as<base::RealVectorStateSpace::StateType>(_i4)->values;
++_i4;
for (int j = 0; j < 3; ++j)
{
s_pos[j] = pos[j];
s_vel[j] = vel[j];
s_ang[j] = ang[j];
}
const dReal *rot = dBodyGetQuaternion(env_->stateBodies_[i]);
base::SO3StateSpace::StateType &s_rot = *s->as<base::SO3StateSpace::StateType>(_i4);
s_rot.w = rot[0];
s_rot.x = rot[1];
s_rot.y = rot[2];
s_rot.z = rot[3];
}
s->collision = 0;
}
void Simulator::GetCurrentState(double s[])
{
/*
* State for each body consists of position, orientation (stored as a quaternion),
* linear velocity, and angular velocity. In the current robot model,
* the bodies are vehicle chassis and the vehicle wheels.
*/
const int n = (int) m_robotBodies.size();
for (int i = 0; i < n; ++i)
{
const dReal *pos = dBodyGetPosition (m_robotBodies[i]);
const dReal *rot = dBodyGetQuaternion(m_robotBodies[i]);
const dReal *vel = dBodyGetLinearVel (m_robotBodies[i]);
const dReal *ang = dBodyGetAngularVel(m_robotBodies[i]);
const int offset = 13 * i;
s[offset ] = pos[0];
s[offset + 1] = pos[1];
s[offset + 2] = pos[2];
s[offset + 3] = rot[0];
s[offset + 4] = rot[1];
s[offset + 5] = rot[2];
s[offset + 6] = rot[3];
s[offset + 7] = vel[0];
s[offset + 8] = vel[1];
s[offset + 9] = vel[2];
s[offset + 10] = ang[0];
s[offset + 11] = ang[1];
s[offset + 12] = ang[2];
}
s[13 * n] = dRandGetSeed();
}
Kinematic PMoveable::odeToKinematic()
{
Kinematic k;
dQuaternion q_result, q_result1, q_base;
float norm;
const dReal *b_info;
const dReal *q_current = dBodyGetQuaternion(body);
q_base[0] = 0; q_base[1] = 0; q_base[2] = 0; q_base[3] = 1;
dQMultiply0(q_result1, q_current, q_base);
dQMultiply2(q_result, q_result1, q_current);
k.orientation_v = Vec3f(q_result[1], q_result[2], q_result[3]);
norm = sqrt(q_result[1] * q_result[1] + q_result[3] * q_result[3]);
if (norm == 0)
k.orientation = 0;
else
k.orientation = atan2(q_result[1] / norm, q_result[3] / norm);
b_info = dBodyGetPosition(body);
k.pos = Vec3f(b_info[0], b_info[1], b_info[2]);
b_info = dBodyGetLinearVel(body);
k.vel = Vec3f(b_info[0], b_info[1], b_info[2]);
return k;
}
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;
}
ngl::Quaternion RigidBody::getQuaternion()const
{
// When getting, the returned values are pointers to internal data structures,
// so the vectors are valid until any changes are made to the rigid body
// system structure.
const dReal *pos=dBodyGetQuaternion(m_id);
return ngl::Quaternion(*pos,*(pos+1),*(pos+2),*(pos+3));
}
double calcularAngulo(int i)
{
Quaternion qt;
const dReal *rotacaoReal = dBodyGetQuaternion (robot[i].body[0]);
qt(0) = rotacaoReal[0];
qt(1) = rotacaoReal[1];
qt(2) = rotacaoReal[2];
qt(3) = rotacaoReal[3];
Vetor3 v;
v = qt.toEuler ();
return v(2);
}
void SParts::graspObj(std::string objName, dJointID &jointID)
{
m_graspObj = objName;
m_onGrasp = true;
m_grasp_jointID = jointID;
// keep the orientation when the grasp is started
const dReal *qua = dBodyGetQuaternion(m_odeobj->body());
m_gini[0] = qua[0];
m_gini[1] = qua[1];
m_gini[2] = qua[2];
m_gini[3] = qua[3];
}
bool ODERigidObject::WriteState(File& f) const
{
//TODO: use body quaternion
Vector3 w,v;
const dReal* pos=dBodyGetPosition(bodyID);
const dReal* q=dBodyGetQuaternion(bodyID);
GetVelocity(w,v);
if(!WriteArrayFile(f,pos,3)) return false;
if(!WriteArrayFile(f,q,4)) return false;
if(!WriteFile(f,w)) return false;
if(!WriteFile(f,v)) return false;
return true;
}
IoObject *IoODEBody_quaternion(IoODEBody *self, IoObject *locals, IoMessage *m)
{
IoODEBody_assertValidBody(self, locals, m);
{
const dReal *q = dBodyGetQuaternion(BODYID);
IoSeq *v = IoSeq_makeFloatArrayOfSize_(IOSTATE, 4);
UArray *u = IoSeq_rawUArray(v);
UArray_at_put_(u, 0, q[0]);
UArray_at_put_(u, 1, q[1]);
UArray_at_put_(u, 2, q[2]);
UArray_at_put_(u, 3, q[3]);
return v;
}
}
void CDynamics3DEntity::UpdateEntityStatus() {
/* Update entity position and orientation */
const dReal* ptPosition = dBodyGetPosition(m_tBody);
GetEmbodiedEntity().SetPosition(
CVector3(ptPosition[0],
ptPosition[1],
ptPosition[2]));
const dReal* ptOrientation = dBodyGetQuaternion(m_tBody);
GetEmbodiedEntity().SetOrientation(
CQuaternion(ptOrientation[0],
ptOrientation[1],
ptOrientation[2],
ptOrientation[3]));
}
void Rigid::_update(){
const dReal* dPos = dBodyGetPosition(mBodyID);
mNode->setPosition((Ogre::Real)dPos[0],
(Ogre::Real)dPos[1],
(Ogre::Real)dPos[2]);
const dReal* dQ = dBodyGetQuaternion(mBodyID);
mNode->setOrientation(Ogre::Quaternion((Ogre::Real)dQ[0],
(Ogre::Real)dQ[1],
(Ogre::Real)dQ[2],
(Ogre::Real)dQ[3]));
}
Matrix PhysicsBody::getTransformation(void) const
{
Vec3f Translation;
const dReal* t = dBodyGetPosition(_BodyID);
Translation.setValues( t[0],t[1],t[2] );
const dReal* q = dBodyGetQuaternion(_BodyID);
Quaternion Rotation;
Rotation.setValueAsQuat(q[1], q[2], q[3], q[0]);
Matrix Transformation;
Transformation.setTransform(Translation,Rotation);
return Transformation;
}
static void simLoop (int pause)
{
if (!pause) {
dBodyAddTorque (anchor_body,torque[0],torque[1],torque[2]);
dBodyAddTorque (test_body,torque[0],torque[1],torque[2]);
dWorldStep (world,0.03);
iteration++;
if (iteration >= 100) {
// measure the difference between the anchor and test bodies
const dReal *w1 = dBodyGetAngularVel (anchor_body);
const dReal *w2 = dBodyGetAngularVel (test_body);
const dReal *q1 = dBodyGetQuaternion (anchor_body);
const dReal *q2 = dBodyGetQuaternion (test_body);
dReal maxdiff = dMaxDifference (w1,w2,1,3);
printf ("w-error = %.4e (%.2f,%.2f,%.2f) and (%.2f,%.2f,%.2f)\n",
maxdiff,w1[0],w1[1],w1[2],w2[0],w2[1],w2[2]);
maxdiff = dMaxDifference (q1,q2,1,4);
printf ("q-error = %.4e\n",maxdiff);
reset_test();
}
}
dReal sides[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {6*SIDE,6*SIDE,6*SIDE};
dReal sides3[3] = {3*SIDE,3*SIDE,3*SIDE};
dsSetColor (1,1,1);
dsDrawBox (dBodyGetPosition(anchor_body), dBodyGetRotation(anchor_body),
sides3);
dsSetColor (1,0,0);
dsDrawBox (dBodyGetPosition(test_body), dBodyGetRotation(test_body), sides2);
dsSetColor (1,1,0);
for (int i=0; i<NUM; i++)
dsDrawBox (dBodyGetPosition (particle[i]),
dBodyGetRotation (particle[i]), sides);
}
void dGeomGetQuaternion (dxGeom *g, dQuaternion quat)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
if (g->body) {
const dReal * body_quat = dBodyGetQuaternion (g->body);
quat[0] = body_quat[0];
quat[1] = body_quat[1];
quat[2] = body_quat[2];
quat[3] = body_quat[3];
}
else {
dRtoQ (g->R, quat);
}
}
void dGeomGetQuaternion (dxGeom *g, dQuaternion quat)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
if (g->body && !g->offset_posr) {
const dReal * body_quat = dBodyGetQuaternion (g->body);
quat[0] = body_quat[0];
quat[1] = body_quat[1];
quat[2] = body_quat[2];
quat[3] = body_quat[3];
}
else {
g->recomputePosr();
dRtoQ (g->final_posr->R, quat);
}
}
void CPHIsland::Repair()
{
if(!m_flags.is_active()) return;
dBodyID body;
for (body = firstbody; body; body = (dxBody *) body->next)
{
if(!dV_valid(dBodyGetAngularVel(body)))
dBodySetAngularVel(body,0.f,0.f,0.f);
if(!dV_valid(dBodyGetLinearVel(body)))
dBodySetLinearVel(body,0.f,0.f,0.f);
if(!dV_valid(dBodyGetPosition(body)))
dBodySetPosition(body,0.f,0.f,0.f);
if(!dQ_valid(dBodyGetQuaternion(body)))
{
dQuaternion q={1.f,0.f,0.f,0.f};//dQSetIdentity(q);
dBodySetQuaternion(body,q);
}
}
}
bool ODERigidObject::WriteState(File& f) const
{
//TODO: use body quaternion
Vector3 w,v;
const dReal* pos=dBodyGetPosition(bodyID);
const dReal* q=dBodyGetQuaternion(bodyID);
GetVelocity(w,v);
//do we need this?
const dReal* frc=dBodyGetForce(bodyID);
const dReal* trq=dBodyGetTorque(bodyID);
if(!WriteArrayFile(f,pos,3)) return false;
if(!WriteArrayFile(f,q,4)) return false;
if(!WriteFile(f,w)) return false;
if(!WriteFile(f,v)) return false;
if(!WriteArrayFile(f,frc,3)) return false;
if(!WriteArrayFile(f,trq,3)) return false;
return true;
}
int main(int argc, char** argv)
{
init_gfx();
init_2d();
world = dWorldCreate();
dBodyID ball = dBodyCreate(world);
dBodySetPosition(ball, 0, 0, 0);
for (;;) {
Uint8* chars = SDL_GetKeyState(NULL);
const dReal* pos = dBodyGetPosition(ball);
const dReal* q = dBodyGetQuaternion(ball);
double angle = get_q_angle(q);
const dReal* axis = get_q_axis(q);
if (chars[SDLK_LEFT]) {
dBodyAddForceAtPos(ball, -force, 0, 0,
pos[0], pos[1]+1, pos[2]);
}
if (chars[SDLK_RIGHT]) {
dBodyAddForceAtPos(ball, force, 0, 0,
pos[0], pos[1]+1, pos[2]);
}
if (chars[SDLK_UP]) {
dBodyAddForce(ball, 0, +force, 0);
}
if (chars[SDLK_DOWN]) {
dBodyAddForce(ball, 0, -force, 0);
}
glTranslatef(pos[0], pos[1], pos[2]);
glRotatef(angle * 180 / M_PI, axis[0], axis[1], axis[2]);
draw_circle();
dWorldQuickStep(world, 0.01);
step();
}
}
bool SSimEntity::getQuaternion(dQuaternion q, bool pre)
{
int geomNum = getGeomNum();
// All of the quaternion connected by fixed joint should be the same
dReal qu[] = {0.0, 0.0, 0.0, 0.0};
const dReal *tmp_qu = dBodyGetQuaternion(m_parts.body);
q[0] = tmp_qu[0];
q[1] = tmp_qu[1];
q[2] = tmp_qu[2];
q[3] = tmp_qu[3];
// return false if it is not rotated
if (m_qw == q[0] && m_qx == q[1] && m_qy == q[2] && m_qz == q[3]){
return false;
}
// if it is rotated
else {
return true;
}
}
void YGEBodyAsset::update(long delta){
if(!hasBody){
createBody();
} else {
double second = delta / 1000000.0f;
double m = 1; //second * 1000;
dBodySetAngularDamping(bodyId, angularDamping * second);
dBodySetLinearDamping(bodyId, linearDamping * second);
dBodyAddTorque(bodyId, torqueAccum.x * m, torqueAccum.y * m, torqueAccum.z * m);
dBodyAddForce(bodyId, forceAccum.x * m, forceAccum.y * m, forceAccum.z * m);
const dReal* pos = dBodyGetPosition (bodyId);
const dReal* rot = dBodyGetQuaternion(bodyId);
parent->setPosition(YGEMath::Vector3(pos[0], pos[1], pos[2]));
parent->setOrientation(YGEMath::Quaternion(rot[0], rot[1], rot[2], rot[3]));
}
}
const dReal * TrackedVehicle::getQuaternion() {
return dBodyGetQuaternion(this->vehicleBody);
}
void SParts::calcPosition(Joint *currj, Joint *nextj, const Vector3d &anchorv, const Rotation &R)
{
if (currj) {
DUMP(("currj = %s\n", currj->name()));
}
DUMP(("name = %s\n", name()));
if (nextj) {
DUMP(("nextj = %s\n", nextj->name()));
}
DUMP(("anchorv = (%f, %f, %f)\n", anchorv.x(), anchorv.y(), anchorv.z()));
{
Vector3d v(m_pos.x(), m_pos.y(), m_pos.z());
// Calculate shift vector from positoin/anchor/orientation
if (currj) {
v -= currj->getAnchor();
DUMP(("parent anchor = %s (%f, %f, %f)\n",
currj->name(),
currj->getAnchor().x(),
currj->getAnchor().y(), currj->getAnchor().z()));
}
DUMP(("v1 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
Rotation rr(R);
if (currj) {
rr *= currj->getRotation();
}
v.rotate(rr);
DUMP(("v2 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
v += anchorv;
DUMP(("v3 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
//DUMP(("v4 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
setPosition(v);
if(m_onGrasp) {
dBodyID body = odeobj().body();
//int num = dBodyGetNumJoints(body);
// get grasp joint
dJointID joint = dBodyGetJoint(body, 0);
// get target body from joint
dBodyID targetBody = dJointGetBody(joint, 1);
// get position of grasp target and parts
const dReal *pos = dBodyGetPosition(body);
const dReal *tpos = dBodyGetPosition(targetBody);
// Set if it is moved
if(pos[0] != tpos[0] || pos[1] != tpos[1] || pos[2] != tpos[2]) {
dBodySetPosition(targetBody, pos[0], pos[1], pos[2]);
}
// get position of grasp target and parts
const dReal *qua = dBodyGetQuaternion(body);
const dReal *tqua = dBodyGetQuaternion(targetBody);
// rotation from initial orientation
dQuaternion rot;
dQMultiply2(rot, qua, m_gini);
// Set if it is rotated
if(rot[0] != tqua[0] || rot[1] != tqua[1] || rot[2] != tqua[2] || rot[3] != tqua[3]) {
dBodySetQuaternion(targetBody, rot);
}
//LOG_MSG(("target2 %d", targetBody));
//LOG_MSG(("(%f, %f, %f)", tpos[0], tpos[1], tpos[2]));
//LOG_MSG(("onGrasp!! num = %d, joint = %d", num));
}
}
Rotation rr(R);
rr *= m_rot;
if (currj) {
rr *= currj->getRotation();
}
setRotation(rr);
if (!nextj) { return; }
for (ChildC::iterator i=m_children.begin(); i!=m_children.end(); i++) {
Child *child = *i;
Rotation R_(R);
if (currj) {
R_ *= currj->getRotation();
}
Vector3d nextv;
nextv = nextj->getAnchor();
if (currj) {
nextv -= currj->getAnchor();
}
nextv.rotate(R_);
nextv += anchorv;
DUMP(("nextv1 : (%f, %f, %f)\n", nextv.x(), nextv.y(), nextv.z()));
// act in a case that multiple JOINTs are connected to one link
if(strcmp(nextj->name(),child->currj->name()) == 0) {
child->nextp->calcPosition(child->currj, child->nextj, nextv, R_);
}
}
}
const dReal * SParts::getQuaternion()
{
return dBodyGetQuaternion(m_odeobj->body());
}
void object::get_orientation(double& x, double& y, double& z, double& w) const
{
if(!body_id) return;
const dReal* quat = dBodyGetQuaternion (body_id);
x = quat[1]; y = quat[2]; z = quat[3]; w = quat[0];
}
static void _soy_scenes_td_side_align_to_z_axis (soyscenesTDSide* self) {
#line 64 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
g_return_if_fail (self != NULL);
#line 405 "TDSide.c"
{
GeeLinkedList* _body_list = NULL;
GeeLinkedList* _tmp0_ = NULL;
GeeLinkedList* _tmp1_ = NULL;
gint _body_size = 0;
GeeLinkedList* _tmp2_ = NULL;
gint _tmp3_ = 0;
gint _tmp4_ = 0;
gint _body_index = 0;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp0_ = self->tdbodies;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp1_ = _g_object_ref0 (_tmp0_);
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_body_list = _tmp1_;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp2_ = _body_list;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp3_ = gee_abstract_collection_get_size ((GeeCollection*) _tmp2_);
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp4_ = _tmp3_;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_body_size = _tmp4_;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_body_index = -1;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
while (TRUE) {
#line 433 "TDSide.c"
gint _tmp5_ = 0;
gint _tmp6_ = 0;
gint _tmp7_ = 0;
soybodiesBody* body = NULL;
GeeLinkedList* _tmp8_ = NULL;
gint _tmp9_ = 0;
gpointer _tmp10_ = NULL;
dxVector3* rot = NULL;
soybodiesBody* _tmp11_ = NULL;
struct dxBody* _tmp12_ = NULL;
dxVector3* _tmp13_ = NULL;
dxQuaternion* quat = NULL;
soybodiesBody* _tmp14_ = NULL;
struct dxBody* _tmp15_ = NULL;
dxQuaternion* _tmp16_ = NULL;
dReal quat_len = 0.0;
dxQuaternion* _tmp17_ = NULL;
dxQuaternion* _tmp18_ = NULL;
dxQuaternion* _tmp19_ = NULL;
dReal _tmp20_ = 0.0;
dxQuaternion* _tmp21_ = NULL;
dReal _tmp22_ = 0.0;
dxQuaternion* _tmp23_ = NULL;
dReal _tmp24_ = 0.0;
dxQuaternion* _tmp25_ = NULL;
dReal _tmp26_ = 0.0;
gfloat _tmp27_ = 0.0F;
dxQuaternion* _tmp28_ = NULL;
dxQuaternion* _tmp29_ = NULL;
dReal _tmp30_ = 0.0;
dReal _tmp31_ = 0.0;
dxQuaternion* _tmp32_ = NULL;
dxQuaternion* _tmp33_ = NULL;
dReal _tmp34_ = 0.0;
dReal _tmp35_ = 0.0;
soybodiesBody* _tmp36_ = NULL;
struct dxBody* _tmp37_ = NULL;
dxQuaternion* _tmp38_ = NULL;
soybodiesBody* _tmp39_ = NULL;
struct dxBody* _tmp40_ = NULL;
dxVector3* _tmp41_ = NULL;
dReal _tmp42_ = 0.0;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp5_ = _body_index;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_body_index = _tmp5_ + 1;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp6_ = _body_index;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp7_ = _body_size;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
if (!(_tmp6_ < _tmp7_)) {
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
break;
#line 488 "TDSide.c"
}
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp8_ = _body_list;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp9_ = _body_index;
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp10_ = gee_abstract_list_get ((GeeAbstractList*) _tmp8_, _tmp9_);
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
body = (soybodiesBody*) _tmp10_;
#line 66 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp11_ = body;
#line 66 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp12_ = _tmp11_->body;
#line 66 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp13_ = dBodyGetAngularVel (_tmp12_);
#line 66 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
rot = _tmp13_;
#line 67 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp14_ = body;
#line 67 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp15_ = _tmp14_->body;
#line 67 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp16_ = dBodyGetQuaternion (_tmp15_);
#line 67 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
quat = _tmp16_;
#line 69 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp17_ = quat;
#line 69 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp17_->x = (dReal) 0;
#line 70 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp18_ = quat;
#line 70 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp18_->y = (dReal) 0;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp19_ = quat;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp20_ = _tmp19_->w;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp21_ = quat;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp22_ = _tmp21_->w;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp23_ = quat;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp24_ = _tmp23_->z;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp25_ = quat;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp26_ = _tmp25_->z;
#line 71 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp27_ = sqrtf ((((gfloat) _tmp20_) * ((gfloat) _tmp22_)) + (((gfloat) _tmp24_) * ((gfloat) _tmp26_)));
#line 71 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
quat_len = (dReal) _tmp27_;
#line 72 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp28_ = quat;
#line 72 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp29_ = quat;
#line 72 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp30_ = _tmp29_->w;
#line 72 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp31_ = quat_len;
#line 72 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp29_->w = _tmp30_ / _tmp31_;
#line 73 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp32_ = quat;
#line 73 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp33_ = quat;
#line 73 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp34_ = _tmp33_->z;
#line 73 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp35_ = quat_len;
#line 73 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp33_->z = _tmp34_ / _tmp35_;
#line 75 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp36_ = body;
#line 75 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp37_ = _tmp36_->body;
#line 75 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp38_ = quat;
#line 75 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
dBodySetQuaternion (_tmp37_, _tmp38_);
#line 76 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp39_ = body;
#line 76 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp40_ = _tmp39_->body;
#line 76 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp41_ = rot;
#line 76 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_tmp42_ = _tmp41_->z;
#line 76 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
dBodySetAngularVel (_tmp40_, (dReal) 0, (dReal) 0, _tmp42_);
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_g_object_unref0 (body);
#line 582 "TDSide.c"
}
#line 65 "/home/jeff/Documents/libraries/libsoy/src/scenes/TDSide.gs"
_g_object_unref0 (_body_list);
#line 586 "TDSide.c"
}
}
shell_root CPHShellSplitterHolder::ElementSingleSplit(const element_fracture &split_elem,const CPHElement* source_element)
{
//const CPHShellSplitter& splitter=m_splitters[aspl];
//CPHElement* element=m_pShell->elements[splitter.m_element];
CPhysicsShell *new_shell_last=P_create_Shell();
CPHShell *new_shell_last_desc=smart_cast<CPHShell*>(new_shell_last);
new_shell_last->mXFORM.set(m_pShell->mXFORM); const u16 start_joint=split_elem.second.m_start_jt_num;
R_ASSERT(_valid(new_shell_last->mXFORM));
const u16 end_joint=split_elem.second.m_end_jt_num;
//it is not right for multiple joints attached to the unsplited part becource all these need to be reattached
if(start_joint!=end_joint)
{
JOINT_STORAGE& joints=m_pShell->joints;
JOINT_I i=joints.begin()+ start_joint,e=joints.begin()+ end_joint;
for(;i!=e;++i)
{
CPHJoint* joint=(*i);
if(joint->PFirst_element()==source_element)
{
IKinematics* K = m_pShell->PKinematics();
dVector3 safe_pos1, safe_pos2;
dQuaternion safe_q1, safe_q2;
CPhysicsElement* el1=cast_PhysicsElement(split_elem.first),*el2=joint->PSecond_element();
dBodyID body1=el1->get_body(), body2=el2->get_body();
dVectorSet(safe_pos1,dBodyGetPosition(body1));
dVectorSet(safe_pos2,dBodyGetPosition(body2));
dQuaternionSet(safe_q1,dBodyGetQuaternion(body1));
dQuaternionSet(safe_q2,dBodyGetQuaternion(body2));
//m_pShell->PlaceBindToElForms();
K->LL_GetBindTransform(bones_bind_forms);
el1->SetTransform(bones_bind_forms[el1->m_SelfID]);
el2->SetTransform(bones_bind_forms[el2->m_SelfID]);
joint->ReattachFirstElement(split_elem.first);
dVectorSet(const_cast<dReal*>(dBodyGetPosition(body1)),safe_pos1);
dVectorSet(const_cast<dReal*>(dBodyGetPosition(body2)),safe_pos2);
dQuaternionSet(const_cast<dReal*>(dBodyGetQuaternion(body1)),safe_q1);
dQuaternionSet(const_cast<dReal*>(dBodyGetQuaternion(body2)),safe_q2);
dBodySetPosition(body1,safe_pos1[0],safe_pos1[1],safe_pos1[2]);
dBodySetPosition(body2,safe_pos2[0],safe_pos2[1],safe_pos2[2]);
dBodySetQuaternion(body1,safe_q1);
dBodySetQuaternion(body2,safe_q2);
}
}
// m_pShell->joints[split_elem.second.m_start_jt_num]->ReattachFirstElement(split_elem.first);
}
//the last new shell will have all splitted old elements end joints and one new element reattached to old joint
//m_splitters.erase(m_splitters.begin()+aspl);
//now aspl points to the next splitter
if((split_elem.first)->FracturesHolder())//if this element can be splitted add a splitter for it
new_shell_last_desc->AddSplitter(CPHShellSplitter::splElement,0,u16(-1));//
new_shell_last_desc->add_Element(split_elem.first);
//pass splitters taking into account that one element was olready added
PassEndSplitters(split_elem.second,new_shell_last_desc,0,0);
InitNewShell(new_shell_last_desc);
m_pShell->PassEndElements(split_elem.second.m_start_el_num,split_elem.second.m_end_el_num,new_shell_last_desc);
m_pShell->PassEndJoints(split_elem.second.m_start_jt_num,split_elem.second.m_end_jt_num,new_shell_last_desc);
new_shell_last_desc->set_PhysicsRefObject(0);
///////////////////temporary for initialization set old Kinematics in new shell/////////////////
new_shell_last->set_Kinematics(m_pShell->PKinematics());
new_shell_last_desc->AfterSetActive();
new_shell_last->set_Kinematics(NULL);
VERIFY2(split_elem.second.m_bone_id<64,"strange root");
VERIFY(_valid(new_shell_last->mXFORM));
VERIFY(dBodyStateValide(source_element->get_bodyConst()));
VERIFY(dBodyStateValide(split_elem.first->get_body()));
new_shell_last->set_ObjectContactCallback(NULL);
new_shell_last->set_PhysicsRefObject(NULL);
return mk_pair(new_shell_last,split_elem.second.m_bone_id);
}
const dReal * SkidSteeringVehicle::getQuaternion() {
return dBodyGetQuaternion(this->vehicleBody);
}
/**
* This method updates the position and orientation of the grabbed
* object and all connected objects. If the grabbed object has
* no connections to other objects (via joints) the method simply
* applies the passed position and orientation to the grabbed object.
* Otherwise the calculation of the new transformation takes place in
* two steps:
* First the orientational difference between the grabbed object and
* and the passed orientation is calculated and applied to the object
* as torque. Then a simulation-step takes place, where the timestep
* is set to 0.5, what should leed to the half orientation correction.
* After the simulation step the objects velocities are set to zero.
* In the second step the difference of the objects position and the
* passed one are calculated and applied to the object as force. Like
* in step one a simulation-step with dt=0.5 takes place and the
* velocities are set to zero again.
* After the computation of the new transformations, the method iterates
* over all objects, which are grabbed or are linked with the grabbed
* object and copies their transformation from the ODE-representation
* to the Entity-members.
* At last the method calls the checkConstraints-method of all currently
* used joints, so that the joints can be activated or deactivated.
* @param position: position of the manipulating object
* @param orientation: orientation of the manipulating object
*/
TransformationData JointInteraction::update(TransformationData trans)
{
if (!isInitialized)
init();
gmtl::AxisAnglef AxAng;
gmtl::Vec3f diff;
dQuaternion quat;
userTrans.position = trans.position;
userTrans.orientation = trans.orientation;
if (!isGrabbing)
return identityTransformation();
int numJoints = dBodyGetNumJoints(grabbedObject->body);
if (!numJoints)
{ // Simple code for objects without Joints
convert(quat, userTrans.orientation);
dBodySetPosition(grabbedObject->body, userTrans.position[0], userTrans.position[1], userTrans.position[2]);
dBodySetQuaternion(grabbedObject->body, quat);
}
else
{ // Code for object with Joints
TransformationData objectTrans;
gmtl::Quatf rotation;
gmtl::Vec3f force, torque;
const dReal* pos = dBodyGetPosition(grabbedObject->body);
const dReal* rot = dBodyGetQuaternion(grabbedObject->body);
convert(objectTrans.position, pos);
convert(objectTrans.orientation, rot);
// TODO: move this code to manipulationTerminationModel
// ungrab object if distance is too high
// diff = userTrans.position - objectTrans.position;
// if (gmtl::length(diff) > maxDist)
// {
// printf("JointInteraction::update(): distance %f too high!\n", gmtl::length(diff));
// ungrab();
// return identityTransformation();
// } // if
// STEP 1: apply Torque
// calculate Torque = angular offset from object orientation to current orientation
rotation = userTrans.orientation;
rotation *= invert(objectTrans.orientation);
gmtl::set(AxAng, rotation);
torque[0] = AxAng[0]*AxAng[1]; // rotAngle*rotX
torque[1] = AxAng[0]*AxAng[2]; // rotAngle*rotY
torque[2] = AxAng[0]*AxAng[3]; // rotAngle*rotZ
// TODO: move this code to manipulationTerminationModel
// ungrab object if angular distance is too high
// if (gmtl::length(torque) > maxRotDist)
// {
// printf("JointInteraction::update(): rotation %f too high!\n", gmtl::length(torque));
// ungrab();
// return identityTransformation();
// } // if
// create a Ball Joint to adjust orientation without changing the position
dJointID universalJoint = dJointCreateBall(world, 0);
dJointAttach(universalJoint, grabbedObject->body, 0);
dJointSetBallAnchor(universalJoint, pos[0], pos[1], pos[2]);
// printf("Torque = %f %f %f\n", torque[0], torque[1], torque[2]);
/***
* old way --> is very instable due to high timestep!!!
***
* dBodySetTorque(grabbedObject->body, torque[0], torque[1], torque[2]);
* // Half way to destination
* dWorldStep(world, 0.5);
**/
// take some simulation-steps for adjusting object's orientation
for (int i=0; i < stepsPerFrame; i++)
{
dBodySetTorque(grabbedObject->body, torque[0], torque[1], torque[2]);
dWorldStep(world, 1.0f/stepsPerFrame);
} // for
dJointDestroy(universalJoint);
// Reset speed and angular velocity to 0
dBodySetLinearVel(grabbedObject->body, 0, 0, 0);
dBodySetAngularVel(grabbedObject->body, 0, 0, 0);
// STEP 2: apply force into wanted direction
// calculate Force = vector from object position to current position
force = (userTrans.position - objectTrans.position);
// printf("Force = %f %f %f\n", force[0], force[1], force[2]);
/***
* old way --> is very instable due to high timestep!!!
***
* dBodySetForce(grabbedObject->body, force[0], force[1], force[2]);
* // Half way to destination
* dWorldStep(world, 0.5);
**/
// take some simulation-steps for adjusting object's position
for (int i=0; i < stepsPerFrame; i++)
{
dBodySetForce(grabbedObject->body, force[0], force[1], force[2]);
dWorldStep(world, 1.0f/stepsPerFrame);
} // for
// Reset speed and angular velocity to 0
dBodySetLinearVel(grabbedObject->body, 0, 0, 0);
dBodySetAngularVel(grabbedObject->body, 0, 0, 0);
}
// Get position and Orientation from simulated object
TransformationData newTransform = identityTransformation();
const dReal* pos = dBodyGetPosition(grabbedObject->body);
const dReal* rot = dBodyGetQuaternion(grabbedObject->body);
convert(newTransform.orientation, rot);
convert(newTransform.position, pos);
// Get position and Orientation from simulated objects
std::map<int, ODEObject*>::iterator it = linkedObjMap.begin();
ODEObject* object;
TransformationData linkedObjectTrans = identityTransformation();
while (it != linkedObjMap.end())
{
object = linkedObjMap[(*it).first];
if (object != grabbedObject)
{
linkedObjectTrans = object->entity->getWorldTransformation();
const dReal* pos1 = dBodyGetPosition(object->body);
const dReal* rot1 = dBodyGetQuaternion(object->body);
convert (linkedObjectTrans.orientation, rot1);
convert (linkedObjectTrans.position, pos1);
linkedObjPipes[it->first]->push_back(linkedObjectTrans);
// gmtl::set(AxAng, newRot);
/* object->entity->setTranslation(pos1[0], pos1[1], pos1[2]);
object->entity->setRotation(AxAng[1], AxAng[2], AxAng[3], AxAng[0]);*/
// assert(false);
// object->entity->setEnvironmentTransformation(linkedObjectTrans);
// object->entity->update();
} // if
++it;
} // while
// Update angles in HingeJoints and check joint-constraints
HingeJoint* hinge;
for (int i=0; i < (int)attachedJoints.size(); i++)
{
hinge = dynamic_cast<HingeJoint*>(attachedJoints[i]);
if (hinge)
hinge->checkAngles();
assert(attachedJoints[i]);
attachedJoints[i]->checkConstraints();
} // for
TransformationData result;
multiply(result, newTransform, deltaTrans);
TransformationPipe* pipe = linkedObjPipes[grabbedObject->entity->getEnvironmentBasedId()];
if (pipe)
pipe->push_back(result);
else
printd(WARNING, "JointInteraction::update(): Could not find Pipe for manipulated Object!\n");
return newTransform;
} // update
