Quaternion Deserializer::ReadQuaternion()
{
float data[4];
Read(data, sizeof data);
return Quaternion(data);
}
int main(int argc, char* argv[]) {
if (argc == 2) {
omp_set_num_threads(atoi(argv[1]));
} else {
omp_set_num_threads(1);
}
//=========================================================================================================
ChSystemGPU * system_gpu = new ChSystemGPU;
ChCollisionSystemGPU *mcollisionengine = new ChCollisionSystemGPU();
system_gpu->SetIntegrationType(ChSystem::INT_ANITESCU);
//=========================================================================================================
system_gpu->SetMaxiter(max_iter);
system_gpu->SetIterLCPmaxItersSpeed(max_iter);
((ChLcpSolverGPU *) (system_gpu->GetLcpSolverSpeed()))->SetMaxIteration(max_iter);
system_gpu->SetTol(1e-3);
system_gpu->SetTolSpeeds(1e-3);
((ChLcpSolverGPU *) (system_gpu->GetLcpSolverSpeed()))->SetTolerance(1e-3);
((ChLcpSolverGPU *) (system_gpu->GetLcpSolverSpeed()))->SetCompliance(0, 0, 0);
((ChLcpSolverGPU *) (system_gpu->GetLcpSolverSpeed()))->SetContactRecoverySpeed(.6);
((ChLcpSolverGPU *) (system_gpu->GetLcpSolverSpeed()))->SetSolverType(ACCELERATED_PROJECTED_GRADIENT_DESCENT);
((ChCollisionSystemGPU *) (system_gpu->GetCollisionSystem()))->SetCollisionEnvelope(particle_radius.x * .05);
mcollisionengine->setBinsPerAxis(R3(num_per_dir.x * 2, num_per_dir.y * 2, num_per_dir.z * 2));
mcollisionengine->setBodyPerBin(100, 50);
system_gpu->Set_G_acc(ChVector<>(0, gravity, 0));
system_gpu->SetStep(timestep);
//=========================================================================================================
cout << num_per_dir.x << " " << num_per_dir.y << " " << num_per_dir.z << " " << num_per_dir.x * num_per_dir.y * num_per_dir.z << endl;
if (particle_radius.x == particle_radius.y == particle_radius.z) {
addPerturbedLayer(R3(0, -5 + particle_radius.y + container_thickness, 0), SPHERE, particle_radius, num_per_dir, R3(.01, .01, .01), 4, .1, system_gpu);
} else {
addPerturbedLayer(R3(0, -5 + particle_radius.y + container_thickness, 0), ELLIPSOID, particle_radius, num_per_dir, R3(.01, .01, .01), 4, .1, system_gpu);
}
//=========================================================================================================
ChSharedBodyPtr L = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr R = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr F = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr B = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
ChSharedBodyPtr Bottom = ChSharedBodyPtr(new ChBody(new ChCollisionModelParallel));
InitObject(L, 100000, Vector(-container_size.x + container_thickness, container_height - container_thickness, 0), Quaternion(1, 0, 0, 0), container_friction, container_friction, 0, true, true,
-20, -20);
InitObject(R, 100000, Vector(container_size.x - container_thickness, container_height - container_thickness, 0), Quaternion(1, 0, 0, 0), container_friction, container_friction, 0, true, true, -20,
-20);
InitObject(F, 100000, Vector(0, container_height - container_thickness, -container_size.z + container_thickness), Quaternion(1, 0, 0, 0), container_friction, container_friction, 0, true, true,
-20, -20);
InitObject(B, 100000, Vector(0, container_height - container_thickness, container_size.z - container_thickness), Quaternion(1, 0, 0, 0), container_friction, container_friction, 0, true, true, -20,
-20);
InitObject(Bottom, 100000, Vector(0, container_height - container_size.y, 0), Quaternion(1, 0, 0, 0), container_friction, container_friction, 0, true, true, -20, -20);
AddCollisionGeometry(L, BOX, Vector(container_thickness, container_size.y, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(R, BOX, Vector(container_thickness, container_size.y, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(F, BOX, Vector(container_size.x, container_size.y, container_thickness), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(B, BOX, Vector(container_size.x, container_size.y, container_thickness), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
AddCollisionGeometry(Bottom, BOX, Vector(container_size.x, container_thickness, container_size.z), Vector(0, 0, 0), Quaternion(1, 0, 0, 0));
FinalizeObject(L, (ChSystemGPU *) system_gpu);
FinalizeObject(R, (ChSystemGPU *) system_gpu);
FinalizeObject(F, (ChSystemGPU *) system_gpu);
FinalizeObject(B, (ChSystemGPU *) system_gpu);
FinalizeObject(Bottom, (ChSystemGPU *) system_gpu);
//=========================================================================================================
//////Rendering specific stuff:
ChOpenGLManager * window_manager = new ChOpenGLManager();
ChOpenGL openGLView(window_manager, system_gpu, 800, 600, 0, 0, "Test_Solvers");
openGLView.render_camera->camera_position = glm::vec3(0, -5, -10);
openGLView.render_camera->camera_look_at = glm::vec3(0, -5, 0);
openGLView.render_camera->camera_scale = .1;
openGLView.SetCustomCallback(RunTimeStep);
openGLView.StartSpinning(window_manager);
window_manager->CallGlutMainLoop();
//=========================================================================================================
for (int i = 0; i < num_steps; i++) {
system_gpu->DoStepDynamics(timestep);
double TIME = system_gpu->GetChTime();
double STEP = system_gpu->GetTimerStep();
double BROD = system_gpu->GetTimerCollisionBroad();
double NARR = system_gpu->GetTimerCollisionNarrow();
double LCP = system_gpu->GetTimerLcp();
double UPDT = system_gpu->GetTimerUpdate();
int BODS = system_gpu->GetNbodies();
int CNTC = system_gpu->GetNcontacts();
int REQ_ITS = ((ChLcpSolverGPU*) (system_gpu->GetLcpSolverSpeed()))->GetTotalIterations();
printf("%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7.4f|%7d|%7d|%7d\n", TIME, STEP, BROD, NARR, LCP, UPDT, BODS, CNTC, REQ_ITS);
if (i % 1000 == 0) {
cout << "SAVED STATE" << endl;
DumpObjects(system_gpu, "diagonal_impact_settled.txt", "\t");
}
RunTimeStep(system_gpu, i);
}
DumpObjects(system_gpu, "diagonal_impact_settled.txt", "\t");
}
// althold_run - runs the althold controller
// should be called at 100hz or more
void Sub::althold_run()
{
uint32_t tnow = AP_HAL::millis();
// initialize vertical speeds and acceleration
pos_control.set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
pos_control.set_max_accel_z(g.pilot_accel_z);
if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
// Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle)
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
last_pilot_heading = ahrs.yaw_sensor;
return;
}
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// get pilot desired lean angles
float target_roll, target_pitch;
// Check if set_attitude_target_no_gps is valid
if (tnow - sub.set_attitude_target_no_gps.last_message_ms < 5000) {
float target_yaw;
Quaternion(
set_attitude_target_no_gps.packet.q
).to_euler(
target_roll,
target_pitch,
target_yaw
);
target_roll = degrees(target_roll);
target_pitch = degrees(target_pitch);
target_yaw = degrees(target_yaw);
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll * 1e2f, target_pitch * 1e2f, target_yaw * 1e2f, true);
return;
}
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
// get pilot's desired yaw rate
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
// call attitude controller
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor;
last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
} else { // hold current heading
// this check is required to prevent bounce back after very fast yaw maneuvers
// the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
if (tnow < last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
target_yaw_rate = 0; // Stop rotation on yaw axis
// call attitude controller with target yaw rate = 0 to decelerate on yaw axis
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
last_pilot_heading = ahrs.yaw_sensor; // update heading to hold
} else { // call attitude controller holding absolute absolute bearing
attitude_control.input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, last_pilot_heading, true);
}
}
if (fabsf(channel_throttle->norm_input()-0.5f) > 0.05f) { // Throttle input above 5%
// output pilot's throttle
attitude_control.set_throttle_out(channel_throttle->norm_input(), false, g.throttle_filt);
// reset z targets to current values
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
} else { // hold z
if (ap.at_bottom) {
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); // clear velocity and position targets, and integrator
pos_control.set_alt_target(inertial_nav.get_altitude() + 10.0f); // set target to 10 cm above bottom
} else if (rangefinder_alt_ok()) {
// if rangefinder is ok, use surface tracking
float target_climb_rate = get_surface_tracking_climb_rate(0, pos_control.get_alt_target(), G_Dt);
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
}
pos_control.update_z_controller();
}
motors.set_forward(channel_forward->norm_input());
motors.set_lateral(channel_lateral->norm_input());
}
//-----------------------------------------------------------------------
void Extruder::_extrudeCapImpl(TriangleBuffer& buffer) const
{
std::vector<int> indexBuffer;
PointList pointList;
buffer.rebaseOffset();
Triangulator t;
if (mShapeToExtrude)
t.setShapeToTriangulate(mShapeToExtrude);
else
t.setMultiShapeToTriangulate(mMultiShapeToExtrude);
t.triangulate(indexBuffer, pointList);
buffer.estimateIndexCount(2*indexBuffer.size());
buffer.estimateVertexCount(2*pointList.size());
//begin cap
buffer.rebaseOffset();
Quaternion qBegin = Utils::_computeQuaternion(mExtrusionPath->getDirectionAfter(0));
if (mRotationTrack)
{
Real angle = mRotationTrack->getFirstValue();
qBegin = qBegin*Quaternion((Radian)angle, Vector3::UNIT_Z);
}
Real scaleBegin=1.;
if (mScaleTrack)
scaleBegin = mScaleTrack->getFirstValue();
for (size_t j =0;j<pointList.size();j++)
{
Vector2 vp2 = pointList[j];
Vector3 vp(vp2.x, vp2.y, 0);
Vector3 normal = -Vector3::UNIT_Z;
Vector3 newPoint = mExtrusionPath->getPoint(0)+qBegin*(scaleBegin*vp);
addPoint(buffer, newPoint,
qBegin*normal,
vp2);
}
for (size_t i=0;i<indexBuffer.size()/3;i++)
{
buffer.index(indexBuffer[i*3]);
buffer.index(indexBuffer[i*3+2]);
buffer.index(indexBuffer[i*3+1]);
}
// end cap
buffer.rebaseOffset();
Quaternion qEnd = Utils::_computeQuaternion(mExtrusionPath->getDirectionBefore(mExtrusionPath->getSegCount()));
if (mRotationTrack)
{
Real angle = mRotationTrack->getLastValue();
qEnd = qEnd*Quaternion((Radian)angle, Vector3::UNIT_Z);
}
Real scaleEnd=1.;
if (mScaleTrack)
scaleEnd = mScaleTrack->getLastValue();
for (size_t j =0;j<pointList.size();j++)
{
Vector2 vp2 = pointList[j];
Vector3 vp(vp2.x, vp2.y, 0);
Vector3 normal = Vector3::UNIT_Z;
Vector3 newPoint = mExtrusionPath->getPoint(mExtrusionPath->getSegCount())+qEnd*(scaleEnd*vp);
addPoint(buffer, newPoint,
qEnd*normal,
vp2);
}
for (size_t i=0;i<indexBuffer.size()/3;i++)
{
buffer.index(indexBuffer[i*3]);
buffer.index(indexBuffer[i*3+1]);
buffer.index(indexBuffer[i*3+2]);
}
}
Quaternion operator*( real_t s ) const {
return Quaternion( w * s, x * s, y * s, z * s );
}
Quaternion Quaternion::RotationBetween(Vector3f a, Vector3f b)
{
return Quaternion();
}
void setupAnimation(void)
{
//Color Keyframe Sequence
ColorKeyframes = KeyframeColorsSequence3f::create();
ColorKeyframes->addKeyframe(Color4f(1.0f,0.0f,0.0f,1.0f),0.0f);
ColorKeyframes->addKeyframe(Color4f(0.0f,1.0f,0.0f,1.0f),2.0f);
ColorKeyframes->addKeyframe(Color4f(0.0f,0.0f,1.0f,1.0f),4.0f);
ColorKeyframes->addKeyframe(Color4f(1.0f,0.0f,0.0f,1.0f),6.0f);
//Vector Keyframe Sequence
VectorKeyframes = KeyframeVectorsSequence3f::create();
VectorKeyframes->addKeyframe(Vec3f(0.0f,0.0f,0.0f),0.0f);
VectorKeyframes->addKeyframe(Vec3f(0.0f,1.0f,0.0f),1.0f);
VectorKeyframes->addKeyframe(Vec3f(1.0f,1.0f,0.0f),2.0f);
VectorKeyframes->addKeyframe(Vec3f(1.0f,0.0f,0.0f),3.0f);
VectorKeyframes->addKeyframe(Vec3f(0.0f,0.0f,0.0f),4.0f);
//Rotation Keyframe Sequence
RotationKeyframes = KeyframeRotationsSequenceQuat::create();
RotationKeyframes->addKeyframe(Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*0.0),0.0f);
RotationKeyframes->addKeyframe(Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*0.5),1.0f);
RotationKeyframes->addKeyframe(Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*1.0),2.0f);
RotationKeyframes->addKeyframe(Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*1.5),3.0f);
RotationKeyframes->addKeyframe(Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*0.0),4.0f);
//Transformation Keyframe Sequence
TransformationKeyframes = KeyframeTransformationsSequence44f::create();
Matrix TempMat;
TempMat.setTransform(Vec3f(0.0f,0.0f,0.0f), Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*0.0));
TransformationKeyframes->addKeyframe(TempMat,1.0f);
TempMat.setTransform(Vec3f(0.0f,1.0f,0.0f), Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*0.5));
TransformationKeyframes->addKeyframe(TempMat,2.0f);
TempMat.setTransform(Vec3f(1.0f,1.0f,0.0f), Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*1.0));
TransformationKeyframes->addKeyframe(TempMat,3.0f);
TempMat.setTransform(Vec3f(1.0f,0.0f,0.0f), Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*1.5));
TransformationKeyframes->addKeyframe(TempMat,4.0f);
TempMat.setTransform(Vec3f(0.0f,0.0f,0.0f), Quaternion(Vec3f(0.0f,1.0f,0.0f), 3.14159f*0.0));
TransformationKeyframes->addKeyframe(TempMat,5.0f);
//Animator
TheAnimator = KeyframeAnimator::create();
beginEditCP(TheAnimator, KeyframeAnimator::KeyframeSequenceFieldMask);
TheAnimator->setKeyframeSequence(TransformationKeyframes);
endEditCP(TheAnimator, KeyframeAnimator::KeyframeSequenceFieldMask);
//Animation
TheAnimation = FieldAnimation::create();
beginEditCP(TheAnimation);
TheAnimation->setAnimator(TheAnimator);
TheAnimation->setInterpolationType(LINEAR_INTERPOLATION);
TheAnimation->setCycling(-1);
endEditCP(TheAnimation);
TheAnimation->setAnimatedField(getFieldContainer("Transform",std::string("TorusNodeTransformationCore")), std::string("matrix"));
//Animation Listener
TheAnimation->addAnimationListener(&TheAnimationListener);
//Animation Advancer
TheAnimationAdvancer = ElapsedTimeAnimationAdvancer::create();
beginEditCP(TheAnimationAdvancer);
ElapsedTimeAnimationAdvancer::Ptr::dcast(TheAnimationAdvancer)->setStartTime( 0.0 );
beginEditCP(TheAnimationAdvancer);
}
TransformObject::TransformObject(void)
: pos(Vector3f()), scale(Vector3f(1.0f, 1.0f, 1.0f))
{
SetRotation(Quaternion());
}
Quaternion Quaternion::operator+ (const Quaternion& rkQ) const
{
return Quaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
}
void Physics::step( real_t dt )
{
// TODO step the world forward by dt. Need to detect collisions, apply
// forces, and integrate positions and orientations.
//
// Note: put RK4 here, not in any of the physics bodies
//
// Must use the functions that you implemented
//
// Note, when you change the position/orientation of a physics object,
// change the position/orientation of the graphical object that represents
// it
Derivative initial;
Derivative k1, k2, k3, k4;
Vector3 sumAngles;
for(unsigned int i = 0; i < spheres.size(); i++){
initial.velocity = spheres[i]->velocity;
initial.angular_velocity = spheres[i]->angular_velocity;
initial.position = spheres[i]->position;
initial.orientation = spheres[i]->orientation;
initial.angle = Vector3(0,0,0);
resetForce(*spheres[i]);
/****************************RK4**************************************/
/* k1 */
setPosition(initial, k1, initial, 0);
updateState(*spheres[i], k1);
spheres[i]->apply_force(gravity, Vector3(0,0,0));
for(unsigned int j = 0; j < springs.size(); j++){
if(springs[j]->body1->id == spheres[i]->id || springs[j]->body2->id == spheres[i]->id)
springs[j]->step(dt);
}
Evaluate(k1, *spheres[i], dt);
resetForce(*spheres[i]);
/* k2 */
setPosition(k1, k2, initial, dt/2);
updateState(*spheres[i], k2);
spheres[i]->apply_force(gravity, Vector3(0,0,0));
for(unsigned int j = 0; j < springs.size(); j++){
if(springs[j]->body1->id == spheres[i]->id || springs[j]->body2->id == spheres[i]->id)
springs[j]->step(dt/2);
}
Evaluate(k2, *spheres[i], dt);
resetForce(*spheres[i]);
/* k3 */
setPosition(k2, k3, initial, dt/2);
updateState(*spheres[i], k3);
spheres[i]->apply_force(gravity, Vector3(0,0,0));
for(unsigned int j = 0; j < springs.size(); j++){
if(springs[j]->body1->id == spheres[i]->id || springs[j]->body2->id == spheres[i]->id)
springs[j]->step(dt/2);
}
Evaluate(k3, *spheres[i], dt);
resetForce(*spheres[i]);
/* k4 */
setPosition(k3, k4, initial, dt);
updateState(*spheres[i], k4);
spheres[i]->apply_force(gravity, Vector3(0,0,0));
for(unsigned int j = 0; j < springs.size(); j++){
if(springs[j]->body1->id == spheres[i]->id || springs[j]->body2->id == spheres[i]->id)
springs[j]->step(dt);
}
Evaluate(k4, *spheres[i], dt);
resetForce(*spheres[i]);
/***************udpate position and orientation after RK4***************/
spheres[i]->position = initial.position + 1.0 / 6.0 * (k1.position + 2.0 * (k2.position + k3.position) + k4.position);
spheres[i]->velocity = initial.velocity + 1.0 / 6.0 * (k1.velocity + 2.0 * (k2.velocity + k3.velocity) + k4.velocity);
spheres[i]->angular_velocity = initial.angular_velocity + 1.0 / 6.0 * (k1.angular_velocity + 2.0 * (k2.angular_velocity + k3.angular_velocity) + k4.angular_velocity);
sumAngles = 1.0 / 6.0 * (k1.angle + 2.0 * (k2.angle + k3.angle) + k4.angle);
if(sumAngles != Vector3(0,0,0)){
spheres[i]->orientation = initial.orientation * Quaternion(normalize(sumAngles), length(sumAngles));
}
else{
spheres[i]->orientation = initial.orientation;
}
spheres[i]->sphere->position = spheres[i]->position;
spheres[i]->sphere->orientation = spheres[i]->orientation;
/*******collision detection************/
for(unsigned int j = 0; j < spheres.size(); j++){
if(spheres[i]->id != spheres[j]->id){
collides(*spheres[i], *spheres[j], collision_damping);
}
}
for(unsigned int j = 0; j < triangles.size(); j++){
collides(*spheres[i], *triangles[j], collision_damping);
}
for(unsigned int j = 0; j < planes.size(); j++){
collides(*spheres[i], *planes[j], collision_damping);
}
}
}
void TransformObject::SetRotation(Vector3f eulerAngleAmounts)
{
rot = Quaternion(eulerAngleAmounts);
CalculateNewDirVectors();
}
//-----------------------------------------------------------------------
void Node::setOrientation( Real w, Real x, Real y, Real z)
{
setOrientation(Quaternion(w, x, y, z));
}
void dtRotate(DtScalar x, DtScalar y, DtScalar z, DtScalar w) {
if (currentObject) currentObject->rotate(Quaternion(x, y, z, w));
}
Quaternion Quaternion::Conjugate() {
return Quaternion(s, -vx, -vy, -vz);
}
Quaternion Quaternion::Slerp(const Quaternion& from, const Quaternion& to, const Float32& time)
{
return Quaternion();
}
Quaternion Quaternion::operator- (const Quaternion& rkQ) const
{
return Quaternion(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);
}
Quaternion Quaternion::Identity()
{
return Quaternion();
}
Quaternion Quaternion::operator* (Real fScalar) const
{
return Quaternion(fScalar*w,fScalar*x,fScalar*y,fScalar*z);
}
bool Ekf::initialiseFilter(void)
{
// Keep accumulating measurements until we have a minimum of 10 samples for the baro and magnetoemter
// Sum the IMU delta angle measurements
imuSample imu_init = _imu_buffer.get_newest();
_delVel_sum += imu_init.delta_vel;
// Sum the magnetometer measurements
if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
if (_mag_counter == 0) {
_mag_filt_state = _mag_sample_delayed.mag;
}
_mag_counter ++;
_mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f;
}
// set the default height source from the adjustable parameter
if (_hgt_counter == 0) {
_primary_hgt_source = _params.vdist_sensor_type;
}
if (_primary_hgt_source == VDIST_SENSOR_RANGE) {
if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) {
if (_hgt_counter == 0) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_hgt_filt_state = _range_sample_delayed.rng;
}
_hgt_counter ++;
_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _range_sample_delayed.rng;
}
} else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) {
if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
if (_hgt_counter == 0) {
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_hgt_filt_state = _baro_sample_delayed.hgt;
}
_hgt_counter ++;
_hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _baro_sample_delayed.hgt;
}
} else {
return false;
}
// check to see if we have enough measurements and return false if not
if (_hgt_counter <= 10 || _mag_counter <= 10) {
return false;
} else {
// reset variables that are shared with post alignment GPS checks
_gps_drift_velD = 0.0f;
_gps_alt_ref = 0.0f;
// Zero all of the states
_state.ang_error.setZero();
_state.vel.setZero();
_state.pos.setZero();
_state.gyro_bias.setZero();
_state.gyro_scale(0) = _state.gyro_scale(1) = _state.gyro_scale(2) = 1.0f;
_state.accel_z_bias = 0.0f;
_state.mag_I.setZero();
_state.mag_B.setZero();
_state.wind_vel.setZero();
// get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static
float pitch = 0.0f;
float roll = 0.0f;
if (_delVel_sum.norm() > 0.001f) {
_delVel_sum.normalize();
pitch = asinf(_delVel_sum(0));
roll = atan2f(-_delVel_sum(1), -_delVel_sum(2));
} else {
return false;
}
// calculate initial tilt alignment
matrix::Euler<float> euler_init(roll, pitch, 0.0f);
_state.quat_nominal = Quaternion(euler_init);
_output_new.quat_nominal = _state.quat_nominal;
// initialise the filtered alignment error estimate to a larger starting value
_tilt_err_length_filt = 1.0f;
// calculate the averaged magnetometer reading
Vector3f mag_init = _mag_filt_state;
// calculate the initial magnetic field and yaw alignment
resetMagHeading(mag_init);
// calculate the averaged height reading to calulate the height of the origin
_hgt_sensor_offset = _hgt_filt_state;
// if we are not using the baro height as the primary source, then calculate an offset relative to the origin
// so it can be used as a backup
if (!_control_status.flags.baro_hgt) {
baroSample baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt - _hgt_sensor_offset;
} else {
_baro_hgt_offset = 0.0f;
}
// initialise the state covariance matrix
initialiseCovariance();
// initialise the terrain estimator
initHagl();
return true;
}
}
Quaternion Quaternion::operator- () const
{
return Quaternion(-w,-x,-y,-z);
}
namespace Math
{
const Quaternion Quaternion::identity = Quaternion(1, 0, 0, 0);
}
Quaternion operator* (Real fScalar, const Quaternion& rkQ)
{
return Quaternion(fScalar*rkQ.w,fScalar*rkQ.x,fScalar*rkQ.y,
fScalar*rkQ.z);
}
//-----------------------------------------------------------------------
void Extruder::_extrudeBodyImpl(TriangleBuffer& buffer, const Shape* shapeToExtrude) const
{
assert(mExtrusionPath && shapeToExtrude && "Shape and Path must not be null!");
unsigned int numSegPath = mExtrusionPath->getSegCount();
unsigned int numSegShape = shapeToExtrude->getSegCount();
assert(numSegPath>0 && numSegShape>0 && "Shape and path must contain at least two points");
Real totalPathLength = mExtrusionPath->getTotalLength();
Real totalShapeLength = shapeToExtrude->getTotalLength();
// Merge shape and path with tracks
Ogre::Real lineicPos=0.;
Path path = *mExtrusionPath;
if (mRotationTrack)
path = path.mergeKeysWithTrack(*mRotationTrack);
if (mScaleTrack)
path = path.mergeKeysWithTrack(*mScaleTrack);
if (mPathTextureTrack)
path = path.mergeKeysWithTrack(*mPathTextureTrack);
numSegPath = path.getSegCount();
Shape shape = *shapeToExtrude;
if (mShapeTextureTrack)
shape = shape.mergeKeysWithTrack(*mShapeTextureTrack);
numSegShape = shape.getSegCount();
// Estimate vertex and index count
buffer.rebaseOffset();
buffer.estimateIndexCount(numSegShape*numSegPath*6);
buffer.estimateVertexCount((numSegShape+1)*(numSegPath+1));
Vector3 oldup;
for (unsigned int i = 0; i <= numSegPath; ++i)
{
Vector3 v0 = path.getPoint(i);
Vector3 direction = path.getAvgDirection(i);
Quaternion q = Utils::_computeQuaternion(direction);
Radian angle = (q*Vector3::UNIT_Y).angleBetween(oldup);
if (i>0 && angle>(Radian)Math::HALF_PI/2.)
{
q = Utils::_computeQuaternion(direction, oldup);
}
oldup = q * Vector3::UNIT_Y;
Real scale=1.;
if (i>0) lineicPos += (v0-path.getPoint(i-1)).length();
// Get the values of angle and scale
if (mRotationTrack)
{
Real angle;
angle = mRotationTrack->getValue(lineicPos, lineicPos / totalPathLength, i);
q = q*Quaternion((Radian)angle, Vector3::UNIT_Z);
}
if (mScaleTrack)
{
scale = mScaleTrack->getValue(lineicPos, lineicPos / totalPathLength, i);
}
Real uTexCoord;
if (mPathTextureTrack)
uTexCoord = mPathTextureTrack->getValue(lineicPos, lineicPos / totalPathLength, i);
else
uTexCoord = lineicPos / totalPathLength;
Real lineicShapePos = 0.;
// Insert new points
for (unsigned int j =0; j <= numSegShape; ++j)
{
Vector2 vp2 = shapeToExtrude->getPoint(j);
//Vector2 vp2direction = shapeToExtrude->getAvgDirection(j);
Vector2 vp2normal = shapeToExtrude->getAvgNormal(j);
Vector3 vp(vp2.x, vp2.y, 0);
Vector3 normal(vp2normal.x, vp2normal.y, 0);
buffer.rebaseOffset();
Vector3 newPoint = v0+q*(scale*vp);
if (j>0)
lineicShapePos += (vp2 - shape.getPoint(j-1)).length();
Real vTexCoord;
if (mShapeTextureTrack)
vTexCoord = mShapeTextureTrack->getValue(lineicShapePos, lineicShapePos / totalShapeLength, j);
else
vTexCoord = lineicShapePos / totalShapeLength;
addPoint(buffer, newPoint,
q*normal,
Vector2(uTexCoord, vTexCoord));
if (j <numSegShape && i <numSegPath)
{
if (shapeToExtrude->getOutSide() == SIDE_LEFT)
{
buffer.triangle(numSegShape + 1, numSegShape + 2, 0);
buffer.triangle(0, numSegShape + 2, 1);
}
else
{
buffer.triangle(numSegShape + 2, numSegShape + 1, 0);
buffer.triangle(numSegShape + 2, 0, 1);
}
}
}
}
}
Quaternion Quaternion::Inverse(const Quaternion& rotation)
{
return Quaternion();
}
bool OpenglApplication::initialize()
{
bool rv = true;
// set isUpdateAll
isUpdateAll = true;
isMeshOnly = false;
// set camera paremeters
// aspect ratio is set by the application, so we can ignore it
Camera* camera = &camera_control.camera;
camera->fov = PI / 4.0;
camera->near_clip = 0.1;
camera->far_clip = 100.0;
camera->position = Vector3( 0.0, 2.5, 5.5 );
camera->orientation = Quaternion::Identity;
camera->pitch( -PI / 5.0 );
// hacked-in scene (DO NOT CHANGE)
mesh.filename = "models/pool.obj";
rv = rv && mesh.load();
// copy vertices from loaded mesh
scene.mesh.num_vertices = mesh.vertices.size();
scene.mesh.vertices = new Vector3[scene.mesh.num_vertices];
scene.mesh.num_triangles = mesh.triangles.size();
scene.mesh.triangles = new Triangle[scene.mesh.num_triangles];
for ( size_t i = 0; i < scene.mesh.num_vertices; ++i ) {
scene.mesh.vertices[i] = mesh.vertices[i].position;
}
for ( size_t i = 0; i < scene.mesh.num_triangles; ++i ) {
for ( size_t j = 0; j < 3; ++j ) {
scene.mesh.triangles[i].vertices[j] = mesh.triangles[i].vertices[j];
}
}
scene.mesh_position.position = Vector3( 0.0, -1.0, 0.0 );
scene.mesh_position.orientation = Quaternion( Vector3::UnitY, PI / 3.6 );
scene.mesh_position.scale = Vector3( 0.5, 0.5, 0.5 );
// create a watersurface
WaterSurface::WavePoint* p;
heightmap.wave_points.push_back(WaterSurface::WavePoint());
p = &heightmap.wave_points.back();
p->position = Vector2(.71,.78);
p->falloff = 2;
p->amplitude = .15;
p->timerate = -2.0*PI;
p->period = 8*PI;
heightmap.wave_points.push_back(WaterSurface::WavePoint());
p = &heightmap.wave_points.back();
p->position = Vector2(-.21,-.35);
p->falloff = 2;
p->amplitude = .2;
p->timerate = -8.0/3.0*PI;
p->period = 10*PI;
scene.heightmap = &heightmap;
scene.heightmap_position.position = Vector3( 0.0, -0.4, 0.0 );
scene.heightmap_position.orientation = Quaternion( Vector3::UnitY, PI / 3.6 );
scene.heightmap_position.scale = Vector3( 1.5, 0.5, 1.5 );
rv = rv && project.initialize( &camera_control.camera, &scene );
return rv;
}
Quaternion Quaternion::Euler(const Vector3f& eulerAngles)
{
return Quaternion();
}
Quaternion Quaternion::operator+(Quaternion x)
{
return Quaternion(v + x.v, w + x.w);
}
Quaternion Quaternion::LookRotation(const Vector3f & lookDirection, const Vector3f& up)
{
return Quaternion();
}
Quaternion Quaternion::Zero()
{
return Quaternion();
}
Quaternion Quaternion::conjugate() const
{
return Quaternion(-x, -y, -z, w);
}
