//-------------------------------------------------------------------------
bool PUScriptTranslator::passValidatePropertyValidQuaternion(PUScriptCompiler* compiler,
PUPropertyAbstractNode* prop)
{
Quaternion val;
if(getQuaternion(prop->values.begin(), prop->values.end(), &val))
{
return true;
}
// compiler->addError(ScriptCompiler::CE_INVALIDPARAMETERS, prop->file, prop->line,
// "PU Compiler: " + prop->values.front()->getValue() + " is not a valid Quaternion");
return false;
}
std::array<Radian, 3> ImuFilter::getEulerAngles() const {
arma::colvec4 q = getQuaternion();
double sq1 = q(1)*q(1);
double sq2 = q(2)*q(2);
double sq3 = q(3)*q(3);
std::array<Radian, 3> angles;
angles[0] = std::atan2( 2.0f * (q(0)*q(1) + q(2)*q(3)), 1 - 2*sq1 - 2*sq2) * radians;
angles[1] = std::asin( 2.0f * (q(0)*q(2) - q(3)*q(1))) * radians;
angles[2] = std::atan2( 2.0f * (q(0)*q(3) + q(1)*q(2)), 1 - 2*sq2 - 2*sq3) * radians;
return angles;
}
/**
* @inheritDoc
*/
void PlayerNetworkObject::Update(f32 DeltaTime) {
ASSERT(m_bInitialized);
// Send the packet everytime it's dirty or for a heartbeat
bool heartbeat_triggered = !m_heartbeat.is_stopped() && m_heartbeat.elapsed().wall >= m_heartbeat_delay;
if (m_velocityDirty || m_rotationDirty || heartbeat_triggered) {
bool velocityNotNull = m_velocity != Math::Vector3::Zero;
bool rotationNotNull = m_rotation != Math::Vector3::Zero;
m_heartbeat.stop();
if (velocityNotNull || rotationNotNull) {
m_heartbeat.start();
}
Proto::ObjectUpdated objectUpdated;
Proto::Object* object = objectUpdated.add_objects();
object->set_id(m_entity->getId());
object->set_name(m_entity->getName());
Proto::SystemObject* systemObject = object->add_systemobjects();
systemObject->set_type(Proto::SystemType_Name(Proto::SystemType::Network));
systemObject->set_systemtype(Proto::SystemType::Network);
if (m_velocityDirty || velocityNotNull) {
m_velocityDirty = false;
Proto::Property* positionProperty = systemObject->add_properties();
positionProperty->set_name("Position");
getVector3(&m_position, positionProperty->mutable_value());
Proto::Property* velocityProperty = systemObject->add_properties();
velocityProperty->set_name("Velocity");
getVector3(&m_velocity, velocityProperty->mutable_value());
}
if (m_rotationDirty || rotationNotNull) {
m_rotationDirty = false;
Proto::Property* orientationProperty = systemObject->add_properties();
orientationProperty->set_name("Orientation");
getQuaternion(&m_orientation, orientationProperty->mutable_value());
Proto::Property* rotationProperty = systemObject->add_properties();
rotationProperty->set_name("Rotation");
getVector3(&m_rotation, rotationProperty->mutable_value());
}
std::string data;
objectUpdated.AppendToString(&data);
DownstreamMessageProto downstreamMessage;
downstreamMessage.set_type(DownstreamMessageProto::PLAYER_MOVE);
downstreamMessage.set_data(data);
GetSystemScene<NetworkScene>()->GetSystem<NetworkSystem>()->getNetworkService()->send(downstreamMessage);
}
}
// ----------------------------------------------------------------------------
KartUpdateMessage::KartUpdateMessage(ENetPacket* pkt)
: Message(pkt, MT_KART_INFO)
{
World *world = World::getWorld();
unsigned int num_karts = getInt();
for(unsigned int i=0; i<num_karts; i++)
{
// Currently not used
KartControl kc(this);
Vec3 xyz = getVec3();
btQuaternion q = getQuaternion();
Kart *kart = world->getKart(i);
kart->setXYZ(xyz);
kart->setRotation(q);
} // for i
}; // KartUpdateMessage
uint8_t Accelerometer::getYawPitchRoll(float *data)
{
uint8_t fifoBuffer[64];
Quaternion q;
VectorFloat gravity;
uint16_t packetSize = getFIFOPacketSize();
getFIFOBytes(fifoBuffer, packetSize);
getQuaternion(&q, fifoBuffer);
getGravity(&gravity, &q);
getYawPitchRoll(data, &q, &gravity);
data[0] = (data[0] * 180/M_PI) - zeroValues[0];
data[1] = (data[1] * 180/M_PI) - zeroValues[1];
data[2] = (data[2] * 180/M_PI) - zeroValues[2];
return 0;
}
arma::mat33 ImuFilter::getMatrix() const {
arma::colvec4 q = getQuaternion();
arma::mat33 rotMat;
rotMat(0, 0) = 1-2*(q(2)*q(2) + q(3)*q(3));
rotMat(1, 1) = 1-2*(q(1)*q(1) + q(3)*q(3));
rotMat(2, 2) = 1-2*(q(1)*q(1) + q(2)*q(2));
rotMat(0, 1) = -2*q(0)*q(3) + 2*q(1)*q(2);
rotMat(0, 2) = 2*q(0)*q(2) + 2*q(1)*q(3);
rotMat(1, 0) = 2*q(0)*q(3) + 2*q(1)*q(2);
rotMat(1, 2) = -2*q(0)*q(1) + 2*q(2)*q(3);
rotMat(2, 0) = -2*q(0)*q(2) + 2*q(1)*q(3);
rotMat(2, 1) = 2*q(0)*q(1) + 2*q(2)*q(3);
return rotMat;
}
void GLWidget::mouseMoveEvent(QMouseEvent *e){
if( m_mouseClick){
float x = 2.0 * (float(e->x()) / w) - 1.0;
float y = 1.0 - 2.0 * (float(e->y()) / h);
QPointF p(x, y);
Vector2f now( x, y);
if( getQuaternion())
m_trackBalls.move( p, QQuaternion());
else{
float dX = x - m_lastPoint.x;
float dY = y - m_lastPoint.y;
camP -= dX*.5;
camT += dY*.5;
}
m_lastPoint = now;
char s[100];
sprintf( s, "Position: <%f,%f,%f>,%f,%f,<%f,%f>", freeCam.pos.x, freeCam.pos.y, freeCam.pos.z, camT, camP, m_lastPoint.x, m_lastPoint.y);
emit updateStatus(s);
}
}
void Filter3D<RealT>::operator()(std::string const& id, cv::Mat& measuredTrans, cv::Mat& measuredRot)
{
//Create&insert or return the related filter
//Second set of 4 params are for Kalman filter: # of state dimensions, # of measurement dimensions,
//# of control input dimensions and float precision of internal matrices
auto pair = mFilters.emplace(std::piecewise_construct,
std::make_tuple(id),
std::make_tuple(7, 7, 3, CV_TYPE));
cv::KalmanFilter& filter = pair.first->second.filter;
cv::Vec<RealT,4>& prevQuat = pair.first->second.prevQuat;
bool& deleted = pair.first->second.deleted;
//Newly inserted or lazy-deleted
if(pair.second || deleted){
deleted = false;
initFilter(filter, prevQuat, measuredTrans, measuredRot);
}
//Already existing filter
else{
RealT* state = (RealT*)mTempState.ptr();
double* trans = (double*)measuredTrans.ptr();
//Fill state
state[0] = (RealT)trans[0]; //x
state[1] = (RealT)trans[1]; //y
state[2] = (RealT)trans[2]; //z
getQuaternion((double*)measuredRot.ptr(), state + 3);
//Do the correction step
shortestPathQuat(prevQuat);
filter.correct(mTempState).copyTo(mTempState);
normalizeQuat();
//Write state back
trans[0] = state[0]; //x
trans[1] = state[1]; //y
trans[2] = state[2]; //z
getAngleAxis(state + 3, (double*)measuredRot.ptr());
}
}
void SParts::calcPosture()
{
assert(isBody());
if (m_children.size() <= 0) { return; }
const dReal *pos = getPosition();
const dReal *q = getQuaternion();
for (ChildC::iterator i=m_children.begin(); i!=m_children.end(); i++) {
Child *child = *i;
Vector3d v(pos[0], pos[1], pos[2]);
Rotation r;
r.setQuaternion(q);
if (child->currj) {
v += child->currj->getAnchor();
}
child->nextp->calcPosition(child->currj, child->nextj, v, r);
}
}
/**
* Calculates all relevant joystick/gamepad stuff and rotates, moves the camera accordingly.
*/
void mtCalcJoyMovement (double interval)
{
// This pulls and handles all usb-stream joystick events.
handleJoystickEvents();
MTVec3D sideDirection = mtNormVector3D(mtCrossProduct3D(G_JoyViewVector, G_JoyUpVector));
double maxAngle = MT_MAX_ANGLE - mtAngleVectorVector(G_JoyViewVector, G_JoyUpVector); // around 90° -- +70°
double minAngle = MT_MIN_ANGLE - mtAngleVectorVector(G_JoyViewVector, G_JoyUpVector); // around 90° -- -70°
maxAngle = maxAngle < 0 ? -1.0 : maxAngle;
minAngle = minAngle > 0 ? 1.0 : minAngle;
MTQuaternion q = getQuaternion(sideDirection, mtToVector3D(0, 1, 0), minAngle, maxAngle, interval);
G_JoyViewVector = mtRotatePointWithQuaternion(q, G_JoyViewVector);
double forwardTranslation = -(getTranslationAxisValue(4) + MT_AXIS_4_OFFSET) / MT_XBOX_TRANS_NORMALISATION;
MTVec3D forwardVec = mtNormVector3D(mtToVector3D(G_JoyViewVector.x, 0, G_JoyViewVector.z));
G_JoyTranslation = mtAddVectorVector(G_JoyTranslation, mtMultiplyVectorScalar(forwardVec, forwardTranslation));
double sideTranslation = (getTranslationAxisValue(3) + MT_AXIS_3_OFFSET) / MT_XBOX_TRANS_NORMALISATION;
MTVec3D sideVec = mtNormVector3D(mtToVector3D(sideDirection.x, 0, sideDirection.z));
G_JoyTranslation = mtAddVectorVector(G_JoyTranslation, mtMultiplyVectorScalar(sideVec, sideTranslation));
MTVec3D upDirection = mtToVector3D(0, 1, 0);
double upTranslation = (getTranslationAxisValue(2) + MT_AXIS_2_OFFSET) / MT_XBOX_TRANS_NORMALISATION;
G_JoyTranslation = mtAddVectorVector(G_JoyTranslation, mtMultiplyVectorScalar(upDirection, upTranslation));
MTVec3D downDirection = mtToVector3D(0, -1, 0);
double downTranslation = (getTranslationAxisValue(5) + MT_AXIS_5_OFFSET) / MT_XBOX_TRANS_NORMALISATION;
G_JoyTranslation = mtAddVectorVector(G_JoyTranslation, mtMultiplyVectorScalar(downDirection, downTranslation));
G_JoyPosition = G_JoyTranslation;
G_JoyViewVector = mtNormVector3D(G_JoyViewVector);
}
/**
* @inheritDoc
*/
void PlayerInputObject::createShot(void) {
Proto::Object shotProto;
shotProto.set_id(ObjectId::gen().str());
shotProto.set_name("Shot");
shotProto.set_template_("ShotTemplate");
auto physicSystemObject = shotProto.add_systemobjects();
physicSystemObject->set_systemtype(Proto::SystemType::Physic);
physicSystemObject->set_type("Movable");
auto positionProperty = physicSystemObject->add_properties();
positionProperty->set_name("Position");
getVector3(&m_position, positionProperty->mutable_value());
auto orientationProperty = physicSystemObject->add_properties();
orientationProperty->set_name("Orientation");
getQuaternion(&m_orientation, orientationProperty->mutable_value());
auto networkSystemObject = shotProto.add_systemobjects();
networkSystemObject->set_systemtype(Proto::SystemType::Network);
networkSystemObject->set_type("Replicable");
m_createObjectQueue->push_back(shotProto);
PostChanges(System::Changes::Generic::CreateObject);
}
/**
* Berechnet aktuellen Rotationswinkel des Wuerfels.
* @param interval Dauer der Bewegung in Sekunden.
*/
void
calcRotation (double interval)
{
float ypr[3] = {0.0,0.0,0.0};
float q[4] = {0.0,0.0,0.0,0.0};
getYPR(ypr);
getQuaternion (q);
/*printf("A: %f, B: %f, C: %f D: %f\n", q[0], q[1], q[2], q[3]);*/
G_Quaternion->s = q[0];
G_Quaternion->v[0] = q[1];
G_Quaternion->v[1] = q[2];
G_Quaternion->v[2] = q[3];
/*
setQuaternionMovement ();
*/
/*printf("Yaw: %f, Pitch: %f, Roll: %f\n", ypr[0], ypr[1], ypr[2]);*/
/* printf("A: %f, B: %f, C: %f D: %f\n", G_Quaternion->s, G_Quaternion->v[0], G_Quaternion->v[1], G_Quaternion->v[2]);*/
g_rotationAngleY = ypr[0];
g_rotationAngleX = ypr[1];
printf ("ypr x : %f\n", ypr[1]);
g_rotationAngleZ = ypr[2];
}
void GLWidget::wheelEvent(QWheelEvent *e){
if( getQuaternion()){
freeCam.pos.x += e->delta()/8.0;
setActiveCamera( freeCam);
}
}
Ogre::Quaternion TranslationRotation3D::ogreRotation() const {
double x, y, z, w;
getQuaternion(x, y, z, w);
return Ogre::Quaternion(w, x, y, z);
}
void NIFStream::getQuaternions(std::vector<osg::Quat> &quat, size_t size)
{
quat.resize(size);
for(size_t i = 0;i < quat.size();i++)
quat[i] = getQuaternion();
}
int main(int argc, char** argv)
{
init_robot(&robot);
struct coord *destination;
destination = new_coord(0, 0);
destination->x = 0;
destination->y = 0;
pthread_mutex_init(&state.frame_lock, NULL);
pthread_cond_init(&state.has_frame, NULL);
robot.dest = destination;
gpioSetMode(4, PI_OUTPUT);
gpioSetMode(24, PI_OUTPUT);
gpioSetMode(5, PI_OUTPUT);
gpioSetMode(14, PI_OUTPUT);
//870 for at the ground,
// gpioServo(24,1380);
// gpioServo(4, 2100);
pthread_t range_thread, render_thread, cv_thread;
pthread_create(&range_thread, NULL, &range_read, &robot);
pthread_create(&render_thread, NULL, &render_task, NULL);
// pthread_create(&cv_thread, NULL, &find_circles, &state);
gpioWrite(5, 0);
gpioWrite(14, 0);
start_control_panel(&robot, &state);
pthread_t this_thread = pthread_self();
//set_thread_priority(this_thread, 0);
//set_thread_priority(robot.server->serve_thread, 1);
//set_thread_priority(range_thread, 2);
read(robot.encoder_handle, robot.sensor_data, sizeof(robot.sensor_data));
getQuaternion(&robot.position.orientation, robot.sensor_data);
robot.position.last_left = *((int *) (robot.sensor_data + 42));
robot.position.last_right = *((int *) (robot.sensor_data + 46));
struct robot_task *position_task, *point_task, *remote_task,
*spin_task, *move_task, *adjust_task, *stand_up_task, *fly_task,
*avoid_task, *stay_task;
position_task = (struct robot_task *)malloc(sizeof(*position_task));
position_task->task_func = &update_position2;
fly_task = (struct robot_task *)malloc(sizeof(*fly_task));
fly_task->task_func = &buzz;
point_task = (struct robot_task *)malloc(sizeof(*point_task));
robot.turret.target.x = 500;
robot.turret.target.y = 0;
robot.turret.target.z = 0;
point_task->task_func = &pointer;
remote_task = (struct robot_task *)malloc(sizeof(*remote_task));
remote_task->task_func = &remote;
avoid_task = (struct robot_task *)malloc(sizeof(*avoid_task));
avoid_task->task_func = &stay_away;
stay_task = (struct robot_task *)malloc(sizeof(*stay_task));
stay_task->task_func = &stay_within;
move_task = (struct robot_task *)malloc(sizeof(*move_task));
move_task->task_func = &move;
struct waypoint the_point;
the_point.point.x = 1000;
the_point.point.y = 0;
the_point.point.z = -40;
INIT_LIST_HEAD(&the_point.list_entry);
// list_add(&the_point.list_entry, &robot.waypoints);
adjust_task = (struct robot_task *)malloc(sizeof(*adjust_task));
adjust_task->task_func = &adjust_speeds;
stand_up_task = (struct robot_task *)malloc(sizeof(*stand_up_task));
stand_up_task->task_func = &stand_up;
INIT_LIST_HEAD(&position_task->list_item);
INIT_LIST_HEAD(&fly_task->list_item);
INIT_LIST_HEAD(&point_task->list_item);
INIT_LIST_HEAD(&remote_task->list_item);
INIT_LIST_HEAD(&avoid_task->list_item);
INIT_LIST_HEAD(&stay_task->list_item);
INIT_LIST_HEAD(&move_task->list_item);
INIT_LIST_HEAD(&adjust_task->list_item);
INIT_LIST_HEAD(&stand_up_task->list_item);
list_add_tail(&position_task->list_item, &robot.task_list);
// list_add_tail(&fly_task->list_item, &robot.task_list);
list_add_tail(&point_task->list_item, &robot.task_list);
list_add_tail(&remote_task->list_item, &robot.task_list);
// list_add_tail(&avoid_task->list_item, &robot.task_list);
// list_add_tail(&stay_task->list_item, &robot.task_list);
list_add_tail(&move_task->list_item, &robot.task_list);
list_add_tail(&adjust_task->list_item, &robot.task_list);
list_add_tail(&stand_up_task->list_item, &robot.task_list);
get_sensor_data(&robot.position2, robot.sensor_data);
init_position(&robot.position2);
//Variables for Kalman filter
struct matrix_2x2 A, B, H, Q, R, current_prob_estimate;
float current_state_estimate[2];
current_state_estimate[0] = 0;
current_state_estimate[1] = 0;
A.elements[0][0] = 1;
A.elements[0][1] = 0;
A.elements[1][0] = 0;
A.elements[1][1] = 1;
Q.elements[0][0] = 0.01;
Q.elements[0][1] = 0;
Q.elements[1][0] = 0;
Q.elements[1][1] = 0.01;
R.elements[0][0] = 0.4;
R.elements[0][1] = 0;
R.elements[1][0] = 0;
R.elements[1][1] = 0.4;
current_prob_estimate.elements[0][0] = 1;
current_prob_estimate.elements[0][1] = 0;
current_prob_estimate.elements[1][0] = 0;
current_prob_estimate.elements[1][1] = 1;
static int look_wait = 0;
while(1)
{
read(robot.encoder_handle, robot.sensor_data, sizeof(robot.sensor_data));
get_sensor_data(&robot.position2, robot.sensor_data);
// fprintf(stderr, "%i, %i\n", robot.position2.left, robot.position2.right);
if (!((robot.position2.orientation.x == robot.position2.orientation.y) &&
(robot.position2.orientation.x == robot.position2.orientation.z) &&
(robot.position2.orientation.x == robot.position2.orientation.w)))
{
do_robot_tasks(&robot);
}
state.roll = robot.turret.roll * 180 / M_PI;
static struct vect sightings[3];
int has_dest = list_empty(&robot.waypoints);
int is_spinning = list_empty(&fly_task->list_item);
// fprintf(stderr, "%i, %i\n", has_dest, is_spinning);
//Prediction Step
float predicted_state_estimate[2];
mat_mult_2xv(predicted_state_estimate, &A, current_state_estimate);
struct matrix_2x2 predicted_prob_estimate;
mat_add_2x2(&predicted_prob_estimate, & current_prob_estimate, &Q);
if (!isnan(state.x_pos) && state.set_target >= 1)
{
float x_in_vid = (state.x_pos - .5) * -2;
float y_in_vid = (state.y_pos - .5) * -2;
// Get obervation.
screen_to_world_vect(&robot, &sightings[0], x_in_vid, y_in_vid);
float observation[2];
observation[0] = sightings[0].x;
observation[1] = sightings[0].y;
float innovation[2];
innovation[0] = observation[0] - predicted_state_estimate[0];
innovation[1] = observation[1] - predicted_state_estimate[1];
float innovation_magnitude = sqrtf((innovation[0] * innovation[0]) + (innovation [1] * innovation[1]));
//fprintf(stderr, "%f, %f, %f, %f, %f, %f, %f, %f\n", innovation_magnitude, robot.turret.yaw, robot.turret.pitch, x_in_vid, y_in_vid, sightings[0].x, sightings[0].y, sightings[0].z);
struct matrix_2x2 innovation_covariance;
mat_add_2x2(&innovation_covariance, &predicted_prob_estimate, &R);
// Update
struct matrix_2x2 kalman_gain, inv_innovation_covariance;
mat_inverse_2x2(&inv_innovation_covariance, &innovation_covariance);
mat_mult_2x2(&kalman_gain, &predicted_prob_estimate, &inv_innovation_covariance);
float step[2];
mat_mult_2xv(step, &kalman_gain, innovation);
current_state_estimate[0] = step[0] + predicted_state_estimate[0];
current_state_estimate[1] = step[1] + predicted_state_estimate[1];
struct matrix_2x2 intermediate;
intermediate.elements[0][0] = 1 - kalman_gain.elements[0][0];
intermediate.elements[0][1] = 0 - kalman_gain.elements[0][1];
intermediate.elements[1][0] = 0 - kalman_gain.elements[1][0];
intermediate.elements[1][1] = 1 - kalman_gain.elements[1][1];
mat_mult_2x2(¤t_prob_estimate, &intermediate, &predicted_prob_estimate);
//fprintf(stderr, "%f, %f\n", current_state_estimate[0], current_state_estimate[1]);
if (innovation_magnitude < 800)
{
look_wait = 300;
list_del_init(&fly_task->list_item);
if (innovation_magnitude < 100)
{
if (list_empty(&robot.waypoints))
{
list_add(&the_point.list_entry, &robot.waypoints);
}
robot.turret.target.x = current_state_estimate[0];
robot.turret.target.y = current_state_estimate[1];
robot.turret.target.z = -40;
the_point.point.x = current_state_estimate[0];
the_point.point.y = current_state_estimate[1];
the_point.point.z = -40;
//fprintf(stderr, "time to stop spinning\n");
}
//fprintf(stderr, "%f, %f, %f\n", current_state_estimate[0], current_state_estimate[1], innovation_magnitude);
}
/* if (fabs(state.x_pos) > .25 || fabs(state.y_pos) > .25)
{
robot.turret.target = temp;
the_point.point = temp;
}
else
{
the_point.point = temp;
}
if (list_empty(&robot.waypoints))
{
list_del_init(&fly_task->list_item);
list_add_tail(&the_point.list_entry, &robot.waypoints);
}*/
// fprintf(stderr, "%f, %f\n", state.x_pos, state.y_pos);
// fprintf(stderr, "%f, %f, %f\n", temp.x, temp.y, temp.z);
state.set_target = 0;
}
if (look_wait > 0)
{
look_wait--;
}
if (list_empty(&robot.waypoints) && list_empty(&fly_task->list_item) && look_wait == 0)
{
list_add_tail(&fly_task->list_item, &robot.task_list);
// fprintf(stderr, "I should spin\n");
}
// fprintf(stderr, "%i, %i\n", robot.position2.left, robot.position2.right);
float distance = get_expected_distance(&robot);
// fprintf(stderr, "%f, %f\n", distance, robot.range);
// fprintf(stderr, "%f, %f, %f\n", robot.turret.target.x, robot.turret.target.y, robot.turret.target.z);
// screen_to_world_vect(&robot, &robot.turret.target, 0, 0);
// fprintf(stderr, "%f, %f, %f\n", robot.turret.target.x, robot.turret.target.y, robot.turret.target.z);
struct timeval now, then;
// printf("%f, %f, %f, %f, %f, %f\n", robot.position2.position.x, robot.position2.position.y, robot.turret.target.x, robot.turret.target.y, robot.dest->x, robot.dest->y);
gettimeofday(&now, NULL);
/* fprintf(stderr, "%ld.%06ld, %f, %f, %f, %f, %i, %i, %f\n", now.tv_sec, now.tv_usec,
robot.position2.orientation.x, robot.position2.orientation.y,
robot.position2.orientation.z, robot.position2.orientation.w,
robot.position2.left, robot.position2.right, robot.position2.heading);
*/
//printf("%f\n", robot.range);
//printf("%f, %f, %f\n", robot.position2.position.x, robot.position2.position.y, robot.position2.tilt);
// fprintf(stderr, "%f, %f, %f, %i, %i\n", robot.position2.position.x, robot.position2.position.y, robot.position2.heading, robot.position2.left, robot.position2.right);
}
}
void PhysicsBody::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & WorldFieldMask)
{
if(_BodyID != 0)
{
dBodyDestroy(_BodyID);
}
if(getWorld() != NULL)
{
_BodyID = dBodyCreate(getWorld()->getWorldID());
}
}
if( ((whichField & PositionFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetPosition(_BodyID, getPosition().x(),getPosition().y(),getPosition().z());
}
if( ((whichField & RotationFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMatrix3 rotation;
Vec4f v1 = getRotation()[0];
Vec4f v2 = getRotation()[1];
Vec4f v3 = getRotation()[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dBodySetRotation(_BodyID, rotation);
}
if( ((whichField & QuaternionFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dQuaternion q;
q[0]=getQuaternion().w();
q[1]=getQuaternion().x();
q[2]=getQuaternion().y();
q[3]=getQuaternion().z();
dBodySetQuaternion(_BodyID,q);
}
if( ((whichField & LinearVelFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearVel(_BodyID, getLinearVel().x(),getLinearVel().y(),getLinearVel().z());
}
if( ((whichField & AngularVelFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularVel(_BodyID, getAngularVel().x(),getAngularVel().y(),getAngularVel().z());
}
if( ((whichField & ForceFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetForce(_BodyID, getForce().x(),getForce().y(),getForce().z());
}
if( ((whichField & TorqueFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetTorque(_BodyID, getTorque().x(),getTorque().y(),getTorque().z());
}
if( ((whichField & AutoDisableFlagFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableFlag(_BodyID, getAutoDisableFlag());
}
if( ((whichField & AutoDisableLinearThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableLinearThreshold(_BodyID, getAutoDisableLinearThreshold());
}
if( ((whichField & AutoDisableAngularThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableAngularThreshold(_BodyID, getAutoDisableAngularThreshold());
}
if( ((whichField & AutoDisableStepsFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableSteps(_BodyID, getAutoDisableSteps());
}
if( ((whichField & AutoDisableTimeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAutoDisableTime(_BodyID, getAutoDisableTime());
}
if( ((whichField & FiniteRotationModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode());
}
if( ((whichField & FiniteRotationModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationMode(_BodyID, getFiniteRotationMode());
}
if( ((whichField & FiniteRotationAxisFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetFiniteRotationAxis(_BodyID, getFiniteRotationAxis().x(),getFiniteRotationAxis().y(),getFiniteRotationAxis().z());
}
if( ((whichField & GravityModeFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetGravityMode(_BodyID, getGravityMode());
}
if( ((whichField & LinearDampingFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearDamping(_BodyID, getLinearDamping());
}
if( ((whichField & AngularDampingFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularDamping(_BodyID, getAngularDamping());
}
if( ((whichField & LinearDampingThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetLinearDampingThreshold(_BodyID, getLinearDampingThreshold());
}
if( ((whichField & AngularDampingThresholdFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dBodySetAngularDampingThreshold(_BodyID, getAngularDampingThreshold());
}
if( ((whichField & MaxAngularSpeedFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
if(getMaxAngularSpeed() >= 0.0)
{
dBodySetMaxAngularSpeed(_BodyID, getMaxAngularSpeed());
}
else
{
dBodySetMaxAngularSpeed(_BodyID, dInfinity);
}
}
if( ((whichField & MassFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMass TheMass;
dBodyGetMass(_BodyID, &TheMass);
TheMass.mass = getMass();
dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & MassCenterOfGravityFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
//dMass TheMass;
//dBodyGetMass(_BodyID, &TheMass);
////TheMass.c[0] = getMassCenterOfGravity().x();
////TheMass.c[1] = getMassCenterOfGravity().y();
////TheMass.c[2] = getMassCenterOfGravity().z();
//Vec4f v1 = getMassInertiaTensor()[0];
//Vec4f v2 = getMassInertiaTensor()[1];
//Vec4f v3 = getMassInertiaTensor()[2];
//TheMass.I[0] = v1.x();
//TheMass.I[1] = v1.y();
//TheMass.I[2] = v1.z();
//TheMass.I[3] = getMassCenterOfGravity().x();
//TheMass.I[4] = v2.x();
//TheMass.I[5] = v2.y();
//TheMass.I[6] = v2.z();
//TheMass.I[7] = getMassCenterOfGravity().y();
//TheMass.I[8] = v3.x();
//TheMass.I[9] = v3.y();
//TheMass.I[10] = v3.z();
//TheMass.I[11] = getMassCenterOfGravity().z();
//dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & MassInertiaTensorFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
dMass TheMass;
dBodyGetMass(_BodyID, &TheMass);
Vec4f v1 = getMassInertiaTensor()[0];
Vec4f v2 = getMassInertiaTensor()[1];
Vec4f v3 = getMassInertiaTensor()[2];
TheMass.I[0] = v1.x();
TheMass.I[1] = v1.y();
TheMass.I[2] = v1.z();
TheMass.I[3] = 0;
TheMass.I[4] = v2.x();
TheMass.I[5] = v2.y();
TheMass.I[6] = v2.z();
TheMass.I[7] = 0;
TheMass.I[8] = v3.x();
TheMass.I[9] = v3.y();
TheMass.I[10] = v3.z();
TheMass.I[11] = 0;
dBodySetMass(_BodyID, &TheMass);
}
if( ((whichField & KinematicFieldMask)
|| (whichField & WorldFieldMask))
&& getWorld() != NULL)
{
if(getKinematic())
{
dBodySetKinematic(_BodyID);
}
else
{
dBodySetDynamic(_BodyID);
}
}
}
static Ogre::Matrix4 getMatrix4(Environment::orientation_t orientation, Ogre::Vector3 translation)
{
Ogre::Matrix4 matrix(getQuaternion(orientation));
matrix.setTrans(translation);
return matrix;
}
virtual bool compare(Item* item1, Item* item2)
{
if(Property::compare(item1, item2) == false) return false;
return getQuaternion(item1) == getQuaternion(item2);
}
virtual void copy(Item* source, Item* destination, ItemDictionary )
{
setQuaternion(destination, getQuaternion(source));
}
Environment::Environment(Ogre::SceneManager* sceneManager, btDynamicsWorld& world, std::istream& level) :
_sceneManager(sceneManager),
_world(world),
_DebugDrawers(),
DebugAI(-1)
{
for(int i = 0; i < 5; ++i)
{
_DebugDrawers.push_back(std::unique_ptr<DebugDrawer>(new DebugDrawer(sceneManager, 0.5)));
}
boost::posix_time::ptime t1 = boost::posix_time::microsec_clock::universal_time();
while (!level.eof())
{
std::string MeshName;
std::string Orientation;
float x, y, z;
level >> MeshName >> x >> y >> z >> Orientation;
if (MeshName.compare("") && (MeshName[0] != '#'))
{
std::cout << "mesh " << MeshName << " at (" << x << "," << y << "," << z << "), " << Orientation << std::endl;
orientation_t o;
if (!Orientation.compare("N"))
o = North;
else if (!Orientation.compare("S"))
o = South;
else if (!Orientation.compare("E"))
o = East;
else if (!Orientation.compare("W"))
o = West;
else
{
std::stringstream str;
str << "Invalid orientation (" << Orientation << ")";
throw std::invalid_argument(str.str());
}
Ogre::Entity * Entity = _sceneManager->createEntity(MeshName);
Entity->setCastShadows(false);
_blocks.push_back(Block(Entity, o, Ogre::Vector3(x,y,z)));
}
}
boost::posix_time::ptime t2 = boost::posix_time::microsec_clock::universal_time();
Ogre::StaticGeometry *sg = _sceneManager->createStaticGeometry("environment");
boost::posix_time::ptime t3 = boost::posix_time::microsec_clock::universal_time();
_NavMesh.AgentHeight = 1.8;
_NavMesh.AgentRadius = 0.8;
_NavMesh.AgentMaxSlope = M_PI / 4;
_NavMesh.AgentMaxClimb = 0.5;
_NavMesh.CellHeight = 0.2;
_NavMesh.CellSize = 0.2;
_NavMesh.QueryExtent = Ogre::Vector3(10, 10, 10);
//for(auto const & block : _blocks)
BOOST_FOREACH(auto const & block, _blocks)
{
sg->addEntity(block._entity, block._position, getQuaternion(block._orientation));
OgreConverter converter(*block._entity);
Ogre::Matrix4 transform = getMatrix4(block._orientation, block._position);
converter.AddToTriMesh(transform, _TriMesh);
converter.AddToHeightField(transform, _NavMesh);
}
void PhysicsGeom::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if(whichField & BodyFieldMask)
{
if(getBody() != NULL)
{
dGeomSetBody(_GeomID, getBody()->getBodyID());
}
else
{
dGeomSetBody(_GeomID, 0);
}
}
if(whichField & PositionFieldMask)
{
dGeomSetPosition(_GeomID, getPosition().x(),getPosition().y(),getPosition().z());
}
if(whichField & RotationFieldMask)
{
dMatrix3 rotation;
Vec4f v1 = getRotation()[0];
Vec4f v2 = getRotation()[1];
Vec4f v3 = getRotation()[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dGeomSetRotation(_GeomID, rotation);
}
if(whichField & QuaternionFieldMask)
{
dQuaternion q;
q[0]=getQuaternion().w();
q[1]=getQuaternion().x();
q[2]=getQuaternion().y();
q[3]=getQuaternion().z();
dGeomSetQuaternion(_GeomID,q);
}
if(whichField & OffsetPositionFieldMask &&
getBody() != NULL)
{
dGeomSetOffsetPosition(_GeomID, getOffsetPosition().x(),getOffsetPosition().y(),getOffsetPosition().z());
}
if(whichField & OffsetRotationFieldMask &&
getBody() != NULL)
{
dMatrix3 rotation;
Vec4f v1 = getOffsetRotation()[0];
Vec4f v2 = getOffsetRotation()[1];
Vec4f v3 = getOffsetRotation()[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dGeomSetOffsetRotation(_GeomID, rotation);
}
if(whichField & OffsetQuaternionFieldMask && getBody() != NULL)
{
dQuaternion q;
q[0]=getOffsetQuaternion().w();
q[1]=getOffsetQuaternion().x();
q[2]=getOffsetQuaternion().y();
q[3]=getOffsetQuaternion().z();
dGeomSetOffsetQuaternion(_GeomID,q);
}
if(whichField & CategoryBitsFieldMask)
{
dGeomSetCategoryBits(_GeomID, getCategoryBits());
}
if(whichField & CollideBitsFieldMask)
{
dGeomSetCollideBits(_GeomID, getCollideBits());
}
if(whichField & SpaceFieldMask)
{
dSpaceID CurSpace(dGeomGetSpace(_GeomID));
if(CurSpace != 0 &&
(getSpace() == NULL ||
CurSpace != getSpace()->getSpaceID()))
{
dSpaceRemove(CurSpace,_GeomID);
}
if(getSpace() != NULL)
{
dSpaceAdd(getSpace()->getSpaceID(), _GeomID);
}
}
if(whichField & EnableFieldMask)
{
if(getEnable())
{
dGeomEnable(_GeomID);
}
else
{
dGeomDisable(_GeomID);
}
}
}
