Quaternion Quaternion::mul(Vector3f r)
{
float w_ = -x * r.getX() - y * r.getY() - z * r.getZ();
float x_ = w * r.getX() + y * r.getZ() - z * r.getY();
float y_ = w * r.getY() + z * r.getX() - x * r.getZ();
float z_ = w * r.getZ() + x * r.getY() - y * r.getX();
Quaternion *product = new Quaternion(x_, y_, z_, w_);
return *product;
}
//----------------------------------------------------------------------------
void GelatinCube::CreateSprings ()
{
// The inner 4-by-4-by-4 particles are used as the control points of a
// B-spline volume. The outer layer of particles are immovable to
// prevent the cuboid from collapsing into itself.
int iSlices = 6;
int iRows = 6;
int iCols = 6;
// Viscous forces applied. If you set viscosity to zero, the cuboid
// wiggles indefinitely since there is no dissipation of energy. If
// the viscosity is set to a positive value, the oscillations eventually
// stop. The length of time to steady state is inversely proportional
// to the viscosity.
#ifdef _DEBUG
float fStep = 0.1f;
#else
float fStep = 0.01f; // simulation needs to run slower in release mode
#endif
float fViscosity = 0.01f;
m_pkModule = new PhysicsModule(iSlices,iRows,iCols,fStep,fViscosity);
// The initial cuboid is axis-aligned. The outer shell is immovable.
// All other masses are constant.
float fSFactor = 1.0f/(float)(iSlices-1);
float fRFactor = 1.0f/(float)(iRows-1);
float fCFactor = 1.0f/(float)(iCols-1);
int iSlice, iRow, iCol;
for (iSlice = 0; iSlice < iSlices; iSlice++)
{
for (iRow = 0; iRow < iRows; iRow++)
{
for (iCol = 0; iCol < iCols; iCol++)
{
m_pkModule->Position(iSlice,iRow,iCol) =
Vector3f(iCol*fCFactor,iRow*fRFactor,iSlice*fSFactor);
if ( 1 <= iSlice && iSlice < iSlices-1
&& 1 <= iRow && iRow < iRows-1
&& 1 <= iCol && iCol < iCols-1 )
{
m_pkModule->SetMass(iSlice,iRow,iCol,1.0f);
m_pkModule->Velocity(iSlice,iRow,iCol) =
0.1f*Vector3f(Mathf::SymmetricRandom(),
Mathf::SymmetricRandom(),Mathf::SymmetricRandom());
}
else
{
m_pkModule->SetMass(iSlice,iRow,iCol,Mathf::MAX_REAL);
m_pkModule->Velocity(iSlice,iRow,iCol) = Vector3f::ZERO;
}
}
}
}
// springs are at rest in the initial configuration
const float fConstant = 10.0f;
Vector3f kDiff;
for (iSlice = 0; iSlice < iSlices-1; iSlice++)
{
for (iRow = 0; iRow < iRows; iRow++)
{
for (iCol = 0; iCol < iCols; iCol++)
{
m_pkModule->ConstantS(iSlice,iRow,iCol) = fConstant;
kDiff = m_pkModule->Position(iSlice+1,iRow,iCol) -
m_pkModule->Position(iSlice,iRow,iCol);
m_pkModule->LengthS(iSlice,iRow,iCol) = kDiff.Length();
}
}
}
for (iSlice = 0; iSlice < iSlices; iSlice++)
{
for (iRow = 0; iRow < iRows-1; iRow++)
{
for (iCol = 0; iCol < iCols; iCol++)
{
m_pkModule->ConstantR(iSlice,iRow,iCol) = fConstant;
kDiff = m_pkModule->Position(iSlice,iRow+1,iCol) -
m_pkModule->Position(iSlice,iRow,iCol);
m_pkModule->LengthR(iSlice,iRow,iCol) = kDiff.Length();
}
}
}
for (iSlice = 0; iSlice < iSlices; iSlice++)
{
for (iRow = 0; iRow < iRows; iRow++)
{
for (iCol = 0; iCol < iCols-1; iCol++)
{
m_pkModule->ConstantC(iSlice,iRow,iCol) = fConstant;
kDiff = m_pkModule->Position(iSlice,iRow,iCol+1) -
m_pkModule->Position(iSlice,iRow,iCol);
m_pkModule->LengthC(iSlice,iRow,iCol) = kDiff.Length();
}
}
}
}
void
AP_AHRS_DCM::drift_correction(float deltat)
{
Matrix3f temp_dcm = _dcm_matrix;
Vector3f velocity;
uint32_t last_correction_time;
// perform yaw drift correction if we have a new yaw reference
// vector
drift_correction_yaw();
// apply trim
temp_dcm.rotateXY(_trim);
// rotate accelerometer values into the earth frame
_accel_ef = temp_dcm * _ins.get_accel();
// integrate the accel vector in the earth frame between GPS readings
_ra_sum += _accel_ef * deltat;
// keep a sum of the deltat values, so we know how much time
// we have integrated over
_ra_deltat += deltat;
num_sat=_gps->num_sats;
if (!have_gps() ||
_gps->status() < GPS::GPS_OK_FIX_3D ||
_gps->num_sats < _gps_minsats) {
// no GPS, or not a good lock. From experience we need at
// least 6 satellites to get a really reliable velocity number
// from the GPS.
//
// As a fallback we use the fixed wing acceleration correction
// if we have an airspeed estimate (which we only have if
// _fly_forward is set), otherwise no correction
if (_ra_deltat < 0.2f) {
// not enough time has accumulated
return;
}
float airspeed;
if (_airspeed && _airspeed->use()) {
airspeed = _airspeed->get_airspeed();
} else {
airspeed = _last_airspeed;
}
// use airspeed to estimate our ground velocity in
// earth frame by subtracting the wind
velocity = _dcm_matrix.colx() * airspeed;
// add in wind estimate
velocity += _wind;
last_correction_time = hal.scheduler->millis();
_have_gps_lock = false;
} else {
if (_gps->last_fix_time == _ra_sum_start) {
// we don't have a new GPS fix - nothing more to do
return;
}
velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), _gps->velocity_down());
last_correction_time = _gps->last_fix_time;
if (_have_gps_lock == false) {
// if we didn't have GPS lock in the last drift
// correction interval then set the velocities equal
_last_velocity = velocity;
}
_have_gps_lock = true;
// keep last airspeed estimate for dead-reckoning purposes
Vector3f airspeed = velocity - _wind;
airspeed.z = 0;
_last_airspeed = airspeed.length();
}
if (have_gps()) {
// use GPS for positioning with any fix, even a 2D fix
_last_lat = _gps->latitude;
_last_lng = _gps->longitude;
_position_offset_north = 0;
_position_offset_east = 0;
// once we have a single GPS lock, we can update using
// dead-reckoning from then on
_have_position = true;
} else {
// update dead-reckoning position estimate
_position_offset_north += velocity.x * _ra_deltat;
_position_offset_east += velocity.y * _ra_deltat;
}
// see if this is our first time through - in which case we
// just setup the start times and return
if (_ra_sum_start == 0) {
_ra_sum_start = last_correction_time;
_last_velocity = velocity;
return;
}
// equation 9: get the corrected acceleration vector in earth frame. Units
// are m/s/s
Vector3f GA_e;
GA_e = Vector3f(0, 0, -1.0f);
bool using_gps_corrections = false;
if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS);
Vector3f vdelta = (velocity - _last_velocity) * v_scale;
GA_e += vdelta;
GA_e.normalize();
if (GA_e.is_inf()) {
// wait for some non-zero acceleration information
return;
}
using_gps_corrections = true;
}
// calculate the error term in earth frame.
_ra_sum /= (_ra_deltat * GRAVITY_MSS);
// get the delayed ra_sum to match the GPS lag
Vector3f GA_b;
if (using_gps_corrections) {
GA_b = ra_delayed(_ra_sum);
} else {
GA_b = _ra_sum;
}
GA_b.normalize();
if (GA_b.is_inf()) {
// wait for some non-zero acceleration information
return;
}
Vector3f error = GA_b % GA_e;
#define YAW_INDEPENDENT_DRIFT_CORRECTION 0
#if YAW_INDEPENDENT_DRIFT_CORRECTION
// step 2 calculate earth_error_Z
float earth_error_Z = error.z;
// equation 10
float tilt = pythagorous2(GA_e.x, GA_e.y);
// equation 11
float theta = atan2f(GA_b.y, GA_b.x);
// equation 12
Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z);
// step 6
error = GA_b % GA_e2;
error.z = earth_error_Z;
#endif // YAW_INDEPENDENT_DRIFT_CORRECTION
// to reduce the impact of two competing yaw controllers, we
// reduce the impact of the gps/accelerometers on yaw when we are
// flat, but still allow for yaw correction using the
// accelerometers at high roll angles as long as we have a GPS
if (use_compass()) {
if (have_gps() && gps_gain == 1.0f) {
error.z *= sinf(fabsf(roll));
} else {
error.z = 0;
}
}
// if ins is unhealthy then stop attitude drift correction and
// hope the gyros are OK for a while. Just slowly reduce _omega_P
// to prevent previous bad accels from throwing us off
if (!_ins.healthy()) {
error.zero();
} else {
// convert the error term to body frame
error = _dcm_matrix.mul_transpose(error);
}
if (error.is_nan() || error.is_inf()) {
// don't allow bad values
check_matrix();
return;
}
_error_rp_sum += error.length();
_error_rp_count++;
// base the P gain on the spin rate
float spin_rate = _omega.length();
// we now want to calculate _omega_P and _omega_I. The
// _omega_P value is what drags us quickly to the
// accelerometer reading.
_omega_P = error * _P_gain(spin_rate) * _kp;
if (_flags.fast_ground_gains) {
_omega_P *= 8;
}
if (_flags.fly_forward && _gps && _gps->status() >= GPS::GPS_OK_FIX_2D &&
_gps->ground_speed_cm < GPS_SPEED_MIN &&
_ins.get_accel().x >= 7 &&
pitch_sensor > -3000 && pitch_sensor < 3000) {
// assume we are in a launch acceleration, and reduce the
// rp gain by 50% to reduce the impact of GPS lag on
// takeoff attitude when using a catapult
_omega_P *= 0.5f;
}
// accumulate some integrator error
if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
_omega_I_sum += error * _ki * _ra_deltat;
_omega_I_sum_time += _ra_deltat;
}
if (_omega_I_sum_time >= 5) {
// limit the rate of change of omega_I to the hardware
// reported maximum gyro drift rate. This ensures that
// short term errors don't cause a buildup of omega_I
// beyond the physical limits of the device
float change_limit = _gyro_drift_limit * _omega_I_sum_time;
_omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit);
_omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit);
_omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit);
_omega_I += _omega_I_sum;
_omega_I_sum.zero();
_omega_I_sum_time = 0;
}
// zero our accumulator ready for the next GPS step
_ra_sum.zero();
_ra_deltat = 0;
_ra_sum_start = last_correction_time;
// remember the velocity for next time
_last_velocity = velocity;
if (_have_gps_lock && _flags.fly_forward) {
// update wind estimate
estimate_wind(velocity);
}
}
//----------------------------------------------------------------------------------------------------
Vector3f Vector3f::Cross( const Vector3f& A, const Vector3f& B )
{
return A.Cross( B );
}
//----------------------------------------------------------------------------------------------------
Vector3f Vector3f::Normalize( const Vector3f& A )
{
Vector3f vec = A;
vec.Normalize();
return vec;
}
bool AP_Arming::compass_checks(bool report)
{
if ((checks_to_perform) & ARMING_CHECK_ALL ||
(checks_to_perform) & ARMING_CHECK_COMPASS) {
if (!_compass.use_for_yaw()) {
// compass use is disabled
return true;
}
if (!_compass.healthy()) {
if (report) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: Compass not healthy"));
}
return false;
}
// check compass learning is on or offsets have been set
if (!_compass.learn_offsets_enabled() && !_compass.configured()) {
if (report) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: Compass not calibrated"));
}
return false;
}
//check if compass is calibrating
if (_compass.is_calibrating()) {
if (report) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("Arm: Compass calibration running"));
}
return false;
}
//check if compass has calibrated and requires reboot
if (_compass.compass_cal_requires_reboot()) {
if (report) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: Compass calibrated requires reboot"));
}
return false;
}
// check for unreasonable compass offsets
Vector3f offsets = _compass.get_offsets();
if (offsets.length() > AP_ARMING_COMPASS_OFFSETS_MAX) {
if (report) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: Compass offsets too high"));
}
return false;
}
// check for unreasonable mag field length
float mag_field = _compass.get_field().length();
if (mag_field > AP_ARMING_COMPASS_MAGFIELD_MAX || mag_field < AP_ARMING_COMPASS_MAGFIELD_MIN) {
if (report) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: Check mag field"));
}
return false;
}
// check all compasses point in roughly same direction
if (!_compass.consistent()) {
if (report) {
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent compasses"));
}
return false;
}
}
return true;
}
void MotionCombinator::update(JointRequest& jointRequest)
{
specialActionOdometry += theSpecialActionsOutput.odometryOffset;
const JointRequest* jointRequests[MotionRequest::numOfMotions];
jointRequests[MotionRequest::walk] = &theWalkingEngineOutput;
jointRequests[MotionRequest::kick] = &theKickEngineOutput;
jointRequests[MotionRequest::specialAction] = &theSpecialActionsOutput;
jointRequests[MotionRequest::stand] = &theStandOutput;
jointRequests[MotionRequest::getUp] = &theGetUpEngineOutput;
jointRequests[MotionRequest::dmpKick] = &theDmpKickEngineOutput;
const JointRequest* armJointRequests[ArmMotionRequest::numOfArmMotions];
armJointRequests[ArmMotionRequest::none] = &theNonArmeMotionEngineOutput;
armJointRequests[ArmMotionRequest::keyFrame] = &theArmKeyFrameEngineOutput;
jointRequest.angles[Joints::headYaw] = theHeadJointRequest.pan;
jointRequest.angles[Joints::headPitch] = theHeadJointRequest.tilt;
copy(*jointRequests[theMotionSelection.targetMotion], jointRequest);
ASSERT(jointRequest.isValid());
// Find fully active motion and set MotionInfo
if(theMotionSelection.ratios[theMotionSelection.targetMotion] == 1.f)
{
Pose2f odometryOffset;
// default values
motionInfo.motion = theMotionSelection.targetMotion;
motionInfo.isMotionStable = true;
motionInfo.upcomingOdometryOffset = Pose2f();
lastJointAngles = theJointAngles;
switch(theMotionSelection.targetMotion)
{
case MotionRequest::walk:
odometryOffset = theWalkingEngineOutput.odometryOffset;
motionInfo.walkRequest = theWalkingEngineOutput.executedWalk;
motionInfo.upcomingOdometryOffset = theWalkingEngineOutput.upcomingOdometryOffset;
break;
case MotionRequest::kick:
odometryOffset = theKickEngineOutput.odometryOffset;
motionInfo.kickRequest = theKickEngineOutput.executedKickRequest;
motionInfo.isMotionStable = theKickEngineOutput.isStable;
break;
case MotionRequest::specialAction:
odometryOffset = specialActionOdometry;
specialActionOdometry = Pose2f();
motionInfo.specialActionRequest = theSpecialActionsOutput.executedSpecialAction;
motionInfo.isMotionStable = theSpecialActionsOutput.isMotionStable;
break;
case MotionRequest::getUp:
motionInfo.isMotionStable = false;
odometryOffset = theGetUpEngineOutput.odometryOffset;
break;
case MotionRequest::stand:
case MotionRequest::dmpKick:
default:
break;
}
if(theRobotInfo.hasFeature(RobotInfo::zGyro) && (theFallDownState.state == FallDownState::upright || theMotionSelection.targetMotion == MotionRequest::getUp))
{
Vector3f rotatedGyros = theInertialData.orientation * theInertialData.gyro.cast<float>();
odometryOffset.rotation = rotatedGyros.z() * theFrameInfo.cycleTime;
}
odometryData += odometryOffset;
ASSERT(jointRequest.isValid());
}
else // interpolate motions
{
const bool interpolateStiffness = !(theMotionSelection.targetMotion != MotionRequest::specialAction && theMotionSelection.specialActionRequest.specialAction == SpecialActionRequest::playDead &&
theMotionSelection.ratios[MotionRequest::specialAction] > 0.f); // do not interpolate from play_dead
for(int i = 0; i < MotionRequest::numOfMotions; ++i)
if(i != theMotionSelection.targetMotion && theMotionSelection.ratios[i] > 0.)
{
interpolate(*jointRequests[i], *jointRequests[theMotionSelection.targetMotion], theMotionSelection.ratios[i], jointRequest, interpolateStiffness, Joints::headYaw, Joints::headPitch);
if(theArmMotionSelection.armRatios[ArmMotionRequest::none] == 1)
interpolate(*jointRequests[i], *jointRequests[theMotionSelection.targetMotion], theMotionSelection.ratios[i], jointRequest, interpolateStiffness, Joints::lShoulderPitch, Joints::lHand);
if(theArmMotionSelection.armRatios[theArmMotionSelection.rightArmRatiosOffset + ArmMotionRequest::none] == 1)
interpolate(*jointRequests[i], *jointRequests[theMotionSelection.targetMotion], theMotionSelection.ratios[i], jointRequest, interpolateStiffness, Joints::rShoulderPitch, Joints::rHand);
interpolate(*jointRequests[i], *jointRequests[theMotionSelection.targetMotion], theMotionSelection.ratios[i], jointRequest, interpolateStiffness, Joints::lHipYawPitch, Joints::rAnkleRoll);
}
}
ASSERT(jointRequest.isValid());
auto combinateArmMotions = [&](Arms::Arm const arm)
{
const int ratioIndexOffset = arm * theArmMotionSelection.rightArmRatiosOffset;
const Joints::Joint startJoint = arm == Arms::left ? Joints::lShoulderPitch : Joints::rShoulderPitch;
const Joints::Joint endJoint = arm == Arms::left ? Joints::lHand : Joints::rHand;
if(theArmMotionSelection.armRatios[ratioIndexOffset + ArmMotionRequest::none] != 1.f)
{
if(theArmMotionSelection.armRatios[ratioIndexOffset + ArmMotionRequest::none] > 0 &&
ArmMotionRequest::none != theArmMotionSelection.targetArmMotion[arm])
copy(jointRequest, theNonArmeMotionEngineOutput, startJoint, endJoint);
if(ArmMotionRequest::none != theArmMotionSelection.targetArmMotion[arm])
copy(*armJointRequests[theArmMotionSelection.targetArmMotion[arm]], jointRequest, startJoint, endJoint);
}
if(theArmMotionSelection.armRatios[ratioIndexOffset + theArmMotionSelection.targetArmMotion[arm]] == 1.f)
{
armMotionInfo.armMotion[arm] = theArmMotionSelection.targetArmMotion[arm];
switch(theArmMotionSelection.targetArmMotion[arm])
{
case ArmMotionRequest::keyFrame:
armMotionInfo.armKeyFrameRequest = theArmMotionSelection.armKeyFrameRequest;
break;
case ArmMotionRequest::none:
default:
break;
}
}
else
{
const bool interpolateStiffness = !(theMotionSelection.targetMotion != MotionRequest::specialAction &&
theMotionSelection.specialActionRequest.specialAction == SpecialActionRequest::playDead &&
theMotionSelection.ratios[MotionRequest::specialAction] > 0.f &&
theArmMotionSelection.armRatios[ratioIndexOffset + ArmMotionRequest::none] > 0);
const JointRequest toJointRequest = theArmMotionSelection.targetArmMotion[arm] == ArmMotionRequest::none ?
*jointRequests[theMotionSelection.targetMotion] : *armJointRequests[theArmMotionSelection.targetArmMotion[arm]];
for(int i = 0; i < ArmMotionRequest::numOfArmMotions; ++i)
{
if(i != theArmMotionSelection.targetArmMotion[arm] && theArmMotionSelection.armRatios[ratioIndexOffset + i] > 0)
{
interpolate(*armJointRequests[i], toJointRequest, theArmMotionSelection.armRatios[ratioIndexOffset + i], jointRequest, interpolateStiffness, startJoint, endJoint);
}
}
}
ASSERT(jointRequest.isValid());
};
combinateArmMotions(Arms::left);
combinateArmMotions(Arms::right);
if(emergencyOffEnabled)
{
if(theFallDownState.state == FallDownState::falling && motionInfo.motion != MotionRequest::specialAction)
{
saveFall(jointRequest);
centerHead(jointRequest);
centerArms(jointRequest);
currentRecoveryTime = 0;
ASSERT(jointRequest.isValid());
}
else if((theFallDownState.state == FallDownState::staggering || theFallDownState.state == FallDownState::onGround) && (motionInfo.motion != MotionRequest::specialAction))
{
centerHead(jointRequest);
ASSERT(jointRequest.isValid());
}
else
{
if(theFallDownState.state == FallDownState::upright)
{
headJawInSavePosition = false;
headPitchInSavePosition = false;
isFallingStarted = false;
}
if(currentRecoveryTime < recoveryTime)
{
currentRecoveryTime += 1;
float ratio = (1.f / float(recoveryTime)) * currentRecoveryTime;
for(int i = 0; i < Joints::numOfJoints; i++)
{
jointRequest.stiffnessData.stiffnesses[i] = 30 + int(ratio * float(jointRequest.stiffnessData.stiffnesses[i] - 30));
}
}
}
}
float sum(0);
int count(0);
for(int i = Joints::lHipYawPitch; i < Joints::numOfJoints; i++)
{
if(jointRequest.angles[i] != JointAngles::off && jointRequest.angles[i] != JointAngles::ignore && lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += abs(jointRequest.angles[i] - lastJointRequest.angles[i]);
count++;
}
}
PLOT("module:MotionCombinator:deviations:JointRequest:legsOnly", sum / count);
for(int i = 0; i < Joints::lHipYawPitch; i++)
{
if(jointRequest.angles[i] != JointAngles::off && jointRequest.angles[i] != JointAngles::ignore && lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += abs(jointRequest.angles[i] - lastJointRequest.angles[i]);
count++;
}
}
PLOT("module:MotionCombinator:deviations:JointRequest:all", sum / count);
sum = 0;
count = 0;
for(int i = Joints::lHipYawPitch; i < Joints::numOfJoints; i++)
{
if(lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += abs(lastJointRequest.angles[i] - theJointAngles.angles[i]);
count++;
}
}
PLOT("module:MotionCombinator:differenceToJointData:legsOnly", sum / count);
for(int i = 0; i < Joints::lHipYawPitch; i++)
{
if(lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
{
sum += abs(lastJointRequest.angles[i] - theJointAngles.angles[i]);
count++;
}
}
lastJointRequest = jointRequest;
PLOT("module:MotionCombinator:differenceToJointData:all", sum / count);
#ifndef NDEBUG
if(!jointRequest.isValid())
{
{
std::string logDir = "";
#ifdef TARGET_ROBOT
logDir = "../logs/";
#endif
OutMapFile stream(logDir + "jointRequest.log");
stream << jointRequest;
OutMapFile stream2(logDir + "motionSelection.log");
stream2 << theMotionSelection;
}
ASSERT(false);
}
#endif
}
void SCollision::performCollision(){
float playerHitRadius = 0.2;
if(!sound){ sound = new Sound("sounds/clang.wav"); }
GameRoom *gr = SCollision::gameState->GetRoom();
//gr->monitor.Enter('r'); //What the hell's this for?
vector<GameObject*> obs = gr->GetGameObjects();
Camera *cam = SCollision::gameState->GetCamera();
list<Projectile *> *bullets = SCollision::gameState->GetParticleSystems()->GetBullets();
list<Projectile *>::iterator it1 = bullets->begin();
while(it1 != bullets->end()){
Projectile *p1 = *it1;
if(p1->isDead()){it1++; continue;}
if(p1->typeString == "Enemy"){
//calculate collision between enemy and bullets
list<Projectile *>::iterator it2 = bullets->begin();
while(it2 != bullets->end()){
Projectile *p2 = *it2;
if(p2->isDead()){it2++; continue;}
if(p2->owner == "player" && p2->typeString != "NavShot"){ //enemy should take damage at collision
if( (p1->getPosition() - p2->getPosition()).norm() < (p1->hitRadius+p2->hitRadius)){
//enemy p1 takes damage -> use hit() to code them
sound->Play();
p1->health = p1->health - p2->damage;
p2->hit(p2->getPosition());
p1->hit(p1->getPosition());
if(p1->isDead()){
//create and attach explosion where enemy was
Projectile *pNew = new SmokyBullet(p1->getPosition(),
Vector3f(0.0,0.0,0.0), 0.8, 0.1, 0.1, 0.7);
pNew->hit(p1->getPosition());
gameState->AddProjectile(pNew);
}
}
}
it2++;
}
}
//calculate collision between player and all other projectiles
if(p1->owner != "player" && p1->typeString != "NavShot"){
if( (cam->getPosition() - p1->getPosition()).norm() < (playerHitRadius+p1->hitRadius)){
//player takes damage
sound->Play();
Render::health = Render::health - p1->damage;
Render::hitEffect();
p1->hit(p1->getPosition());
}
}
//process firing bullets
if(p1->firesOwnBullets && p1->fireBulletCountDown <= 0.0){ //fire a bullet towards cam
p1->fireBulletCountDown = 20000.0 * (rand()%100000 / 100000.0) ;;
Vector3f shootDir = (cam->getPosition() - p1->getPosition());
Vector3f delta = Vector3f( ( (rand()%100000+1) / 100000.0)-0.5 ,
( (rand()%100000+1) / 100000.0)-0.5 ,
( (rand()%100000+1) / 100000.0)-0.5 );
shootDir = shootDir + delta*0.3;
shootDir = shootDir.normalized()*2;
Projectile *pNew = new Slug(p1->getPosition(),
shootDir, 0.8, 0.1, 0.1, 0.7);
pNew->owner = "enemy";
pNew->damage = 5.0;
//pNew->hit(p1->getPosition());
gameState->AddProjectile(pNew);
}
it1++;
}
//deleteDeadEnemies from repel list
ProjectileEnemies::deleteDead();
//gr->monitor.Exit('r'); //seriously, what's this for?
}
Vector3f Sphere::getNormal(Vector3f point){
Vector3f N = point - center;
return N / N.getLength();
}
Forest::Forest(Actor* parent, ForestDesc* desc) : Actor( parent )
{
assert( desc );
assert( desc->surface );
assert( desc->assetName.length() );
assert( desc->cache.length() );
// copy descriptor
_desc = *desc;
// load asset
_asset = Gameplay::iEngine->createAsset( engine::atBinary, _desc.assetName.c_str() );
// enumerate clumps
callback::ClumpL clumps;
_asset->forAllClumps( callback::enumerateClumps, &clumps );
assert( clumps.size() );
// fill batch schemes
Matrix4f clumpM;
MatrixConversion trunkConversion;
MatrixConversion canopyConversion;
_canopyScheme.numLods = _trunkScheme.numLods = clumps.size();
for( callback::ClumpI clumpI = clumps.begin(); clumpI != clumps.end(); clumpI++ )
{
// determine lod Id and check for consistency
unsigned int lodId = getClumpLodId( *clumpI );
if( _trunkScheme.lodGeometry[lodId] )
{
throw Exception( "Clump \"%s\" is a duplication of existing LOD!", (*clumpI)->getName() );
}
// fill schemes
engine::IAtomic* trunkAtomic = Gameplay::iEngine->getAtomic( *clumpI, Gameplay::iEngine->findFrame( (*clumpI)->getFrame(), "Trunk" ) ); assert( trunkAtomic );
engine::IAtomic* canopyAtomic = Gameplay::iEngine->getAtomic( *clumpI, Gameplay::iEngine->findFrame( (*clumpI)->getFrame(), "Canopy" ) ); assert( canopyAtomic );
_trunkScheme.lodGeometry[lodId] = trunkAtomic->getGeometry();
_canopyScheme.lodGeometry[lodId] = canopyAtomic->getGeometry();
_trunkScheme.lodDistance[lodId] = _canopyScheme.lodDistance[lodId] = _desc.lodDistance[lodId];
// calculate conversions for nearest LOD
if( lodId == 0 )
{
clumpM = (*clumpI)->getFrame()->getLTM();
trunkConversion.setup( clumpM, trunkAtomic->getFrame()->getLTM() );
canopyConversion.setup( clumpM, canopyAtomic->getFrame()->getLTM() );
}
}
_trunkScheme.flags = _canopyScheme.flags = 0;
// check schemes
assert( _trunkScheme.isValid() );
assert( _canopyScheme.isValid() );
// create full cache names
std::string instanceCache = _desc.cache; instanceCache += ".matrices";
std::string trunkBspCache = _desc.cache; trunkBspCache += ".trunk";
std::string canopyBspCache = _desc.cache; canopyBspCache += ".canopy";
// try to load forest from cache
IResource* resource = getCore()->getResource( instanceCache.c_str(), "rb" );
if( resource )
{
unsigned int numTrees;
fread( &numTrees, sizeof(unsigned int), 1, resource->getFile() );
_treeMatrix.resize( numTrees );
fread( &_treeMatrix[0], sizeof(Matrix4f), numTrees, resource->getFile() );
resource->release();
}
else
{
// obtain surface properties
Matrix4f ltm = _desc.surface->getFrame()->getLTM();
engine::IGeometry* geometry = _desc.surface->getGeometry();
engine::Mesh* mesh = geometry->createMesh();
float preservedDistance = _desc.collScale * ( geometry->getAABBSup() - geometry->getAABBInf() ).length();
// iterate surface triangles
Vector3f vertex[3];
Vector3f edge[3];
Vector3f edgeN[2];
Vector3f normal;
Vector3f pos;
float cosA, sinA, angle, square, probability, scale;
unsigned int i,j,numTreesInTriangle;
Matrix4f instanceM;
for( i=0; i<mesh->numTriangles; i++ )
{
// transform triangle vertices to world space
vertex[0] = Gameplay::iEngine->transformCoord( mesh->vertices[mesh->triangles[i].vertexId[0]], ltm );
vertex[1] = Gameplay::iEngine->transformCoord( mesh->vertices[mesh->triangles[i].vertexId[1]], ltm );
vertex[2] = Gameplay::iEngine->transformCoord( mesh->vertices[mesh->triangles[i].vertexId[2]], ltm );
// calculate triangle square value...
edge[0] = vertex[1] - vertex[0];
edge[1] = vertex[2] - vertex[0];
edge[2] = vertex[2] - vertex[1];
edgeN[0] = edge[0]; edgeN[0].normalize();
edgeN[1] = edge[1]; edgeN[1].normalize();
normal.cross( edgeN[0], edgeN[1] );
cosA = Vector3f::dot( edgeN[0], edgeN[1] );
if( cosA > 1.0f ) angle = 0.0f;
else if( cosA < -1.0f ) angle = 180;
else angle = acosf( cosA );
sinA = sin( angle );
square = 0.5f * edge[0].length() * edge[1].length() * sinA / 10000.0f;
// obtain number of particles in this triangle
numTreesInTriangle = unsigned int( square * _desc.density );
if( !numTreesInTriangle )
{
// include probability method to decide to place grass on to this triangle
probability = square / ( 1 / _desc.density );
assert( probability <= 1.0f );
if( probability > 0 && getCore()->getRandToolkit()->getUniform( 0, 1 ) <= probability ) numTreesInTriangle++;
}
// generate trees
for( j=0; j<numTreesInTriangle; j++ )
{
// generate scale
scale = getCore()->getRandToolkit()->getUniform( _desc.minScale, _desc.maxScale );
// generate coordinate
pos = generateRandomPosition( vertex, edge );
// generate matrix
instanceM.set(
clumpM[0][0] * scale, clumpM[0][1] * scale, clumpM[0][2] * scale, 0.0f,
clumpM[1][0] * scale, clumpM[1][1] * scale, clumpM[1][2] * scale, 0.0f,
clumpM[2][0] * scale, clumpM[2][1] * scale, clumpM[2][2] * scale, 0.0f,
0.0f, 0.0f, 0.0f, 1.0f
);
instanceM = Gameplay::iEngine->rotateMatrix( instanceM, Vector3f(0,1,0), getCore()->getRandToolkit()->getUniform(0,360) );
instanceM[3][0] = pos[0];
instanceM[3][1] = pos[1];
instanceM[3][2] = pos[2];
_treeMatrix.push_back( instanceM );
}
Scene::progressCallback(
Gameplay::iLanguage->getUnicodeString(844),
float( i ) / float( mesh->numTriangles ),
getScene()
);
}
// release temporary resources
Gameplay::iEngine->releaseMesh( mesh );
// write solution
ccor::IResource* resource = getCore()->getResource( instanceCache.c_str(), "wb" );
unsigned int numTrees = _treeMatrix.size();
fwrite( &numTrees, sizeof(unsigned int), 1, resource->getFile() );
fwrite( &_treeMatrix[0], sizeof(Matrix4f), numTrees, resource->getFile() );
resource->release();
}
// build batches
_trunkBatch = Gameplay::iEngine->createBatch( _treeMatrix.size(), &_trunkScheme ); assert( _trunkBatch );
_canopyBatch = Gameplay::iEngine->createBatch( _treeMatrix.size(), &_canopyScheme ); assert( _canopyBatch );
// fill batches
for( unsigned int i=0; i<_treeMatrix.size(); i++ )
{
_trunkBatch->setMatrix( i, trunkConversion.convert( _treeMatrix[i] ) );
_canopyBatch->setMatrix( i, canopyConversion.convert( _treeMatrix[i] ) );
}
// create spatial acceleration structures BSP trees
_trunkBatch->createBatchTree( _desc.bspLeafSize, trunkBspCache.c_str() );
_canopyBatch->createBatchTree( _desc.bspLeafSize, canopyBspCache.c_str() );
// add batches to scene
getScene()->getStage()->add( _trunkBatch );
getScene()->getStage()->add( _canopyBatch );
// add layers to scene
if( _desc.layers )
{
getScene()->getStage()->add( _desc.layers );
}
// set no sound
_rustleSound = _squeakSound = NULL;
}
static float getSquaredLength(const Vector3f& v)
{
return v.dot( v );
}
int main() {
// read in the file
Coords posvel("dm_1.0000.bin");
cout << "Read in " << posvel.npart << " particles\n";
// Exercise the interface and code
{
Vector3f av;
Map< MatrixXf > epos = posvel.pos.matrix();
av = epos.rowwise().sum()/ posvel.npart;
cout << "Average position : " << av.transpose() << endl;
Map< MatrixXf > evel = posvel.vel.matrix();
av = evel.rowwise().sum()/ posvel.npart;
cout << "Average velocity : " << av.transpose() << endl;
}
// Define the grid
MA<array4d> grid(extents[4][Ngrid][Ngrid][Ngrid+2]); // Pad for FFT, 1 component for overdensity, 3 for velocity
// Define views on this grid
MA<view4_4d> rgrid = grid.view<view4_4d>(indices[r_()][r_()][r_()][r_(0, Ngrid)]);
MA<ref4c> cgrid ( new ref4c(reinterpret_cast< complex<double>* >(grid.ref()), extents[4][Ngrid][Ngrid][Ngrid/2 + 1]));
MA<array4d::reference> dense = rgrid.sub(0);
// CIC grid
cout << "Integrated density, density-weighted velocity -->\n";
for (int ii = 0; ii < 4; ++ii) {
cic(rgrid.sub(ii), posvel, ii-1);
cout << boost::format("%1$6i %2$20.10f\n") % ii % (rgrid.sub(ii).sum());
}
// Normalize by density
int nzeros;
cout << "Integrated velocity field -->\n";
for (int ii=1; ii < 4; ++ii) {
nzeros = 0;
boost::function < void(double&, double&) > ff = if_(_2 > 0)[_1 /= _2].else_[var(nzeros)++];
multi_for(rgrid.sub(ii), dense,ff);
cout << boost::format("%1$6i %2$20.10f %3$10i\n") % ii % (rgrid.sub(ii).sum()*Lbox) % nzeros;
}
// Normalize the density by rho_mean
cout << dense.sum() << endl;
dense /= static_cast<double>(posvel.npart)/pow( static_cast<double>(Ngrid), 3);
cout << dense.sum() << endl;
// FFT
fftw_forward(grid);
// Compute k_i v_i for each grid separately
kdot_impl kdot;
for (int ii=1; ii < 4; ++ii) {
kdot.idim = ii-1;
multi_for_native_indices(cgrid.sub(ii), kdot);
}
// Now add the vectors together --- things are contiguous, so go ahead and use the
// easy route
cgrid.sub(1)() += cgrid.sub(2)() + cgrid.sub(3)();
// This should be what we need modulo normalization factors
// Compute P(k)
PkStruct pk;
double lkmin = log(0.008); double lkmax = log(0.3);
pk.setbins(lkmin, lkmax, 20);
// We want to compute delta* delta, delta* theta, and theta* theta
complex_mult_impl cmult;
// First do delta* theta -- and store this in cgrid.sub(2)
multi_for(cgrid.sub(0), cgrid.sub(1), cgrid.sub(2), cmult);
// delta* delta and theta* theta -- store in place
multi_for(cgrid.sub(0), cgrid.sub(0), cgrid.sub(0), cmult); // We do it this way to make the connection clear with delta* theta
multi_for(cgrid.sub(1), cgrid.sub(1), cgrid.sub(1), cmult); // We do it this way to make the connection clear with delta* theta
// Do the matter power spectrum
multi_for_native_indices(cgrid.sub(0), pk);
cout << pk.pk() << endl;
writeMatrix("pk_matter.dat", "%1$10.4e ", pk.pk());
// Do the velocity power spectrum
pk.reset();
multi_for_native_indices(cgrid.sub(1), pk);
double fac; fac = pow(Lbox, 2);
cout << pk.pk(fac) << endl; // The true adds in the correct velocity normalization
writeMatrix("pk_theta.dat", "%1$10.4e ", pk.pk(fac));
// Do the cross power spectrum
pk.reset();
multi_for_native_indices(cgrid.sub(2), pk);
fac = Lbox;
cout << pk.pk(fac) << endl; // The true adds in the correct velocity normalization
writeMatrix("pk_delta_theta.dat", "%1$10.4e ", pk.pk(fac));
}
// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros.
//
// This drift correction implementation is based on a paper
// by Bill Premerlani from here:
// http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf
void
AP_AHRS_DCM::drift_correction(float deltat)
{
Vector3f velocity;
uint32_t last_correction_time;
// perform yaw drift correction if we have a new yaw reference
// vector
drift_correction_yaw();
// rotate accelerometer values into the earth frame
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
if (_ins.get_accel_health(i)) {
/*
by using get_delta_velocity() instead of get_accel() the
accel value is sampled over the right time delta for
each sensor, which prevents an aliasing effect
*/
Vector3f delta_velocity;
float delta_velocity_dt;
_ins.get_delta_velocity(i, delta_velocity);
delta_velocity_dt = _ins.get_delta_velocity_dt(i);
if (delta_velocity_dt > 0) {
_accel_ef[i] = _dcm_matrix * (delta_velocity / delta_velocity_dt);
// integrate the accel vector in the earth frame between GPS readings
_ra_sum[i] += _accel_ef[i] * deltat;
}
}
}
//update _accel_ef_blended
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
if (_ins.get_accel_count() == 2 && _ins.use_accel(0) && _ins.use_accel(1)) {
float imu1_weight_target = _active_accel_instance == 0 ? 1.0f : 0.0f;
// slew _imu1_weight over one second
_imu1_weight += constrain_float(imu1_weight_target-_imu1_weight, -deltat, deltat);
_accel_ef_blended = _accel_ef[0] * _imu1_weight + _accel_ef[1] * (1.0f - _imu1_weight);
} else {
_accel_ef_blended = _accel_ef[_ins.get_primary_accel()];
}
#else
_accel_ef_blended = _accel_ef[_ins.get_primary_accel()];
#endif // HAL_CPU_CLASS >= HAL_CPU_CLASS_75
// keep a sum of the deltat values, so we know how much time
// we have integrated over
_ra_deltat += deltat;
if (!have_gps() ||
_gps.status() < AP_GPS::GPS_OK_FIX_3D ||
_gps.num_sats() < _gps_minsats) {
// no GPS, or not a good lock. From experience we need at
// least 6 satellites to get a really reliable velocity number
// from the GPS.
//
// As a fallback we use the fixed wing acceleration correction
// if we have an airspeed estimate (which we only have if
// _fly_forward is set), otherwise no correction
if (_ra_deltat < 0.2f) {
// not enough time has accumulated
return;
}
float airspeed;
if (airspeed_sensor_enabled()) {
airspeed = _airspeed->get_airspeed();
} else {
airspeed = _last_airspeed;
}
// use airspeed to estimate our ground velocity in
// earth frame by subtracting the wind
velocity = _dcm_matrix.colx() * airspeed;
// add in wind estimate
velocity += _wind;
last_correction_time = hal.scheduler->millis();
_have_gps_lock = false;
} else {
if (_gps.last_fix_time_ms() == _ra_sum_start) {
// we don't have a new GPS fix - nothing more to do
return;
}
velocity = _gps.velocity();
last_correction_time = _gps.last_fix_time_ms();
if (_have_gps_lock == false) {
// if we didn't have GPS lock in the last drift
// correction interval then set the velocities equal
_last_velocity = velocity;
}
_have_gps_lock = true;
// keep last airspeed estimate for dead-reckoning purposes
Vector3f airspeed = velocity - _wind;
airspeed.z = 0;
_last_airspeed = airspeed.length();
}
if (have_gps()) {
// use GPS for positioning with any fix, even a 2D fix
_last_lat = _gps.location().lat;
_last_lng = _gps.location().lng;
_position_offset_north = 0;
_position_offset_east = 0;
// once we have a single GPS lock, we can update using
// dead-reckoning from then on
_have_position = true;
} else {
// update dead-reckoning position estimate
_position_offset_north += velocity.x * _ra_deltat;
_position_offset_east += velocity.y * _ra_deltat;
}
// see if this is our first time through - in which case we
// just setup the start times and return
if (_ra_sum_start == 0) {
_ra_sum_start = last_correction_time;
_last_velocity = velocity;
return;
}
// equation 9: get the corrected acceleration vector in earth frame. Units
// are m/s/s
Vector3f GA_e;
GA_e = Vector3f(0, 0, -1.0f);
if (_ra_deltat <= 0) {
// waiting for more data
return;
}
bool using_gps_corrections = false;
float ra_scale = 1.0f/(_ra_deltat*GRAVITY_MSS);
if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
float v_scale = gps_gain.get() * ra_scale;
Vector3f vdelta = (velocity - _last_velocity) * v_scale;
GA_e += vdelta;
GA_e.normalize();
if (GA_e.is_inf()) {
// wait for some non-zero acceleration information
_last_failure_ms = hal.scheduler->millis();
return;
}
using_gps_corrections = true;
}
// calculate the error term in earth frame.
// we do this for each available accelerometer then pick the
// accelerometer that leads to the smallest error term. This takes
// advantage of the different sample rates on different
// accelerometers to dramatically reduce the impact of aliasing
// due to harmonics of vibrations that match closely the sampling
// rate of our accelerometers. On the Pixhawk we have the LSM303D
// running at 800Hz and the MPU6000 running at 1kHz, by combining
// the two the effects of aliasing are greatly reduced.
Vector3f error[INS_MAX_INSTANCES];
float error_dirn[INS_MAX_INSTANCES];
Vector3f GA_b[INS_MAX_INSTANCES];
int8_t besti = -1;
float best_error = 0;
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
if (!_ins.get_accel_health(i)) {
// only use healthy sensors
continue;
}
_ra_sum[i] *= ra_scale;
// get the delayed ra_sum to match the GPS lag
if (using_gps_corrections) {
GA_b[i] = ra_delayed(i, _ra_sum[i]);
} else {
GA_b[i] = _ra_sum[i];
}
if (GA_b[i].is_zero()) {
// wait for some non-zero acceleration information
continue;
}
GA_b[i].normalize();
if (GA_b[i].is_inf()) {
// wait for some non-zero acceleration information
continue;
}
error[i] = GA_b[i] % GA_e;
// Take dot product to catch case vectors are opposite sign and parallel
error_dirn[i] = GA_b[i] * GA_e;
float error_length = error[i].length();
if (besti == -1 || error_length < best_error) {
besti = i;
best_error = error_length;
}
// Catch case where orientation is 180 degrees out
if (error_dirn[besti] < 0.0f) {
best_error = 1.0f;
}
}
if (besti == -1) {
// no healthy accelerometers!
_last_failure_ms = hal.scheduler->millis();
return;
}
_active_accel_instance = besti;
#define YAW_INDEPENDENT_DRIFT_CORRECTION 0
#if YAW_INDEPENDENT_DRIFT_CORRECTION
// step 2 calculate earth_error_Z
float earth_error_Z = error.z;
// equation 10
float tilt = pythagorous2(GA_e.x, GA_e.y);
// equation 11
float theta = atan2f(GA_b[besti].y, GA_b[besti].x);
// equation 12
Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z);
// step 6
error = GA_b[besti] % GA_e2;
error.z = earth_error_Z;
#endif // YAW_INDEPENDENT_DRIFT_CORRECTION
// to reduce the impact of two competing yaw controllers, we
// reduce the impact of the gps/accelerometers on yaw when we are
// flat, but still allow for yaw correction using the
// accelerometers at high roll angles as long as we have a GPS
if (AP_AHRS_DCM::use_compass()) {
if (have_gps() && is_equal(gps_gain,1.0f)) {
error[besti].z *= sinf(fabsf(roll));
} else {
error[besti].z = 0;
}
}
// if ins is unhealthy then stop attitude drift correction and
// hope the gyros are OK for a while. Just slowly reduce _omega_P
// to prevent previous bad accels from throwing us off
if (!_ins.healthy()) {
error[besti].zero();
} else {
// convert the error term to body frame
error[besti] = _dcm_matrix.mul_transpose(error[besti]);
}
if (error[besti].is_nan() || error[besti].is_inf()) {
// don't allow bad values
check_matrix();
_last_failure_ms = hal.scheduler->millis();
return;
}
_error_rp = 0.8f * _error_rp + 0.2f * best_error;
// base the P gain on the spin rate
float spin_rate = _omega.length();
// sanity check _kp value
if (_kp < AP_AHRS_RP_P_MIN) {
_kp = AP_AHRS_RP_P_MIN;
}
// we now want to calculate _omega_P and _omega_I. The
// _omega_P value is what drags us quickly to the
// accelerometer reading.
_omega_P = error[besti] * _P_gain(spin_rate) * _kp;
if (use_fast_gains()) {
_omega_P *= 8;
}
if (_flags.fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
_gps.ground_speed() < GPS_SPEED_MIN &&
_ins.get_accel().x >= 7 &&
pitch_sensor > -3000 && pitch_sensor < 3000) {
// assume we are in a launch acceleration, and reduce the
// rp gain by 50% to reduce the impact of GPS lag on
// takeoff attitude when using a catapult
_omega_P *= 0.5f;
}
// accumulate some integrator error
if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
_omega_I_sum += error[besti] * _ki * _ra_deltat;
_omega_I_sum_time += _ra_deltat;
}
if (_omega_I_sum_time >= 5) {
// limit the rate of change of omega_I to the hardware
// reported maximum gyro drift rate. This ensures that
// short term errors don't cause a buildup of omega_I
// beyond the physical limits of the device
float change_limit = _gyro_drift_limit * _omega_I_sum_time;
_omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit);
_omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit);
_omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit);
_omega_I += _omega_I_sum;
_omega_I_sum.zero();
_omega_I_sum_time = 0;
}
// zero our accumulator ready for the next GPS step
memset(&_ra_sum[0], 0, sizeof(_ra_sum));
_ra_deltat = 0;
_ra_sum_start = last_correction_time;
// remember the velocity for next time
_last_velocity = velocity;
}
void Mesh::_CalculateNormalsAndTangents()
{
if (mT.IsValid() || mV.IsEmpty()) return;
// Don't bother with points or lines
if (mPrimitive == IGraphics::Primitive::Point ||
mPrimitive == IGraphics::Primitive::Line ||
mPrimitive == IGraphics::Primitive::LineStrip) return;
// Allocate a temporary buffer to store binormals
Array<Vector3f> binormals (mV.GetSize());
// Remember whether we already have normals to work with
bool calculateNormals = (mV.GetSize() != mN.GetSize());
// We can only calculate tangents if the texture coordinates are available
bool calculateTangents = (mV.GetSize() == mTc0.GetSize());
// If we should calculate normals, clear the existing normal array
if (calculateNormals)
{
mN.Clear();
mN.ExpandTo(mV.GetSize());
}
// Expand the tangent array
if (calculateTangents) mT.ExpandTo(mV.GetSize());
// The number of indices
uint size = mIndices.IsValid() ? mIndices.GetSize() : GetNumberOfVertices();
// Triangles
if (size > 2)
{
bool even = true;
uint i0, i1, i2;
for (uint i = 0; i + 2 < size; )
{
i0 = i;
i1 = i+1;
i2 = i+2;
if (mIndices.IsValid())
{
i0 = mIndices[i0];
i1 = mIndices[i1];
i2 = mIndices[i2];
}
ASSERT(i0 < mV.GetSize(), "Index out of bounds!");
ASSERT(i1 < mV.GetSize(), "Index out of bounds!");
ASSERT(i2 < mV.GetSize(), "Index out of bounds!");
const Vector3f& v0 ( mV[i0] );
const Vector3f& v1 ( mV[i1] );
const Vector3f& v2 ( mV[i2] );
Vector3f v10 (v1 - v0);
Vector3f v20 (v2 - v0);
if (calculateNormals)
{
Vector3f normal (Cross(v10, v20));
mN[i0] += normal;
mN[i1] += normal;
mN[i2] += normal;
}
if (calculateTangents)
{
const Vector2f& t0 ( mTc0[i0] );
const Vector2f& t1 ( mTc0[i1] );
const Vector2f& t2 ( mTc0[i2] );
Vector2f t10 (t1 - t0);
Vector2f t20 (t2 - t0);
float denominator = t10.x * t20.y - t20.x * t10.y;
if ( Float::IsNotZero(denominator) )
{
float scale = 1.0f / denominator;
Vector3f tangent ((v10.x * t20.y - v20.x * t10.y) * scale,
(v10.y * t20.y - v20.y * t10.y) * scale,
(v10.z * t20.y - v20.z * t10.y) * scale);
Vector3f binormal((v20.x * t10.x - v10.x * t20.x) * scale,
(v20.y * t10.x - v10.y * t20.x) * scale,
(v20.z * t10.x - v10.z * t20.x) * scale);
mT[i0] += tangent;
mT[i1] += tangent;
mT[i2] += tangent;
binormals[i0] += binormal;
binormals[i1] += binormal;
binormals[i2] += binormal;
}
}
if (mPrimitive == IGraphics::Primitive::Triangle)
{
i += 3;
}
else if (mPrimitive == IGraphics::Primitive::Quad)
{
if (even) ++i;
else i += 3;
even = !even;
}
else ++i;
}
// If we're calculating normals we need to match all normals with identical vertices
if (calculateNormals)
{
Array<uint> matches;
for (uint i = 0; i < mV.GetSize(); ++i)
{
matches.Clear();
matches.Expand() = i;
Vector3f N = mN[i];
const Vector3f& V (mV[i]);
for (uint b = 0; b < mV.GetSize(); ++b)
{
if (i != b && V == mV[b])
{
matches.Expand() = b;
N += mN[b];
}
}
N.Normalize();
for (uint b = 0; b < matches.GetSize(); ++b)
{
mN[ matches[b] ] = N;
}
}
}
}
if (calculateTangents)
{
// Normalize all tangents
for (uint i = 0; i < mV.GetSize(); ++i)
{
Vector3f& T (mT[i]);
Vector3f& B (binormals[i]);
Vector3f& N (mN[i]);
// In order to avoid visible seams, the tangent should be 90 degrees to the normal
// Note to self: Gram-Schmidt formula for the cross product below: T = T - N * Dot(N, T);
T = Cross(B, N);
T.Normalize();
// Flip the tangent if the handedness is incorrect
if (Dot(Cross(T, B), N) < 0.0f) T.Flip();
}
}
}
// _calibrate_model - perform low level accel calibration
// accel_sample are accelerometer samples collected in 6 different positions
// accel_offsets are output from the calibration routine
// accel_scale are output from the calibration routine
// returns true if successful
bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale )
{
int16_t i;
int16_t num_iterations = 0;
float eps = 0.000000001;
float change = 100.0;
float data[3];
float beta[6];
float delta[6];
float ds[6];
float JS[6][6];
bool success = true;
// reset
beta[0] = beta[1] = beta[2] = 0;
beta[3] = beta[4] = beta[5] = 1.0/GRAVITY;
while( num_iterations < 20 && change > eps ) {
num_iterations++;
_calibrate_reset_matrices(ds, JS);
for( i=0; i<6; i++ ) {
data[0] = accel_sample[i].x;
data[1] = accel_sample[i].y;
data[2] = accel_sample[i].z;
_calibrate_update_matrices(ds, JS, beta, data);
}
_calibrate_find_delta(ds, JS, delta);
change = delta[0]*delta[0] +
delta[0]*delta[0] +
delta[1]*delta[1] +
delta[2]*delta[2] +
delta[3]*delta[3] / (beta[3]*beta[3]) +
delta[4]*delta[4] / (beta[4]*beta[4]) +
delta[5]*delta[5] / (beta[5]*beta[5]);
for( i=0; i<6; i++ ) {
beta[i] -= delta[i];
}
}
// copy results out
accel_scale.x = beta[3] * GRAVITY;
accel_scale.y = beta[4] * GRAVITY;
accel_scale.z = beta[5] * GRAVITY;
accel_offsets.x = beta[0] * accel_scale.x;
accel_offsets.y = beta[1] * accel_scale.y;
accel_offsets.z = beta[2] * accel_scale.z;
// sanity check scale
if( accel_scale.is_nan() || fabs(accel_scale.x-1.0) > 0.1 || fabs(accel_scale.y-1.0) > 0.1 || fabs(accel_scale.z-1.0) > 0.1 ) {
success = false;
}
// sanity check offsets (2.0 is roughly 2/10th of a G, 5.0 is roughly half a G)
if( accel_offsets.is_nan() || fabs(accel_offsets.x) > 2.0 || fabs(accel_offsets.y) > 2.0 || fabs(accel_offsets.z) > 2.0 ) {
success = false;
}
// return success or failure
return success;
}
//==============================
// VRMenuMgrLocal::SubmitForRenderingRecursive
void VRMenuMgrLocal::SubmitForRenderingRecursive( OvrDebugLines & debugLines, BitmapFont const & font,
BitmapFontSurface & fontSurface, VRMenuRenderFlags_t const & flags, VRMenuObjectLocal const * obj,
Posef const & parentModelPose, Vector4f const & parentColor, Vector3f const & parentScale,
Bounds3f & cullBounds, SubmittedMenuObject * submitted, int const maxIndices, int & curIndex ) const
{
if ( curIndex >= maxIndices )
{
// If this happens we're probably not correctly clearing the submitted surfaces each frame
// OR we've got a LOT of surfaces.
LOG( "maxIndices = %i, curIndex = %i", maxIndices, curIndex );
DROID_ASSERT( curIndex < maxIndices, "VrMenu" );
return;
}
// check if this object is hidden
if ( obj->GetFlags() & VRMENUOBJECT_DONT_RENDER )
{
return;
}
Posef const & localPose = obj->GetLocalPose();
Posef curModelPose;
curModelPose.Position = parentModelPose.Position + ( parentModelPose.Orientation * parentScale.EntrywiseMultiply( localPose.Position ) );
curModelPose.Orientation = localPose.Orientation * parentModelPose.Orientation;
Vector4f curColor = parentColor * obj->GetColor();
Vector3f const & localScale = obj->GetLocalScale();
Vector3f scale = parentScale.EntrywiseMultiply( localScale );
OVR_ASSERT( obj != NULL );
/*
VRMenuObject const * parent = ToObject( obj->GetParentHandle() );
if ( parent != NULL )
{
fontParms_t fontParms;
Vector3f itemUp = curModelPose.Orientation * Vector3f( 0.0f, 1.0f, 0.0f );
Vector3f itemNormal = curModelPose.Orientation * Vector3f( 0.0f, 0.0f, 1.0f );
fontSurface.DrawText3D( font, fontParms, curModelPose.Position, itemNormal, itemUp,
1.0f, Vector4f( 1.0f, 0.0f, 1.0f, 1.0f ), parent->GetText().ToCStr() );
}
*/
if ( obj->GetType() != VRMENU_CONTAINER ) // containers never render, but their children may
{
Posef const & hilightPose = obj->GetHilightPose();
Posef itemPose( curModelPose.Orientation * hilightPose.Orientation,
curModelPose.Position + ( curModelPose.Orientation * parentScale.EntrywiseMultiply( hilightPose.Position ) ) );
Matrix4f poseMat( itemPose.Orientation );
Vector3f itemUp = poseMat.GetYBasis();
Vector3f itemNormal = poseMat.GetZBasis();
curModelPose = itemPose; // so children like the slider bar caret use our hilight offset and don't end up clipping behind us!
VRMenuRenderFlags_t rFlags = flags;
VRMenuObjectFlags_t oFlags = obj->GetFlags();
if ( oFlags & VRMENUOBJECT_FLAG_POLYGON_OFFSET )
{
rFlags |= VRMENU_RENDER_POLYGON_OFFSET;
}
if ( oFlags & VRMENUOBJECT_FLAG_NO_DEPTH )
{
rFlags |= VRMENU_RENDER_NO_DEPTH;
}
// the menu object may have zero or more renderable surfaces (if 0, it may draw only text)
Array< VRMenuSurface > const & surfaces = obj->GetSurfaces();
for ( int i = 0; i < surfaces.GetSizeI(); ++i )
{
VRMenuSurface const & surf = surfaces[i];
if ( surf.IsRenderable() )
{
SubmittedMenuObject & sub = submitted[curIndex];
sub.SurfaceIndex = i;
sub.Pose = itemPose;
sub.Scale = scale;
sub.Flags = rFlags;
sub.ColorTableOffset = obj->GetColorTableOffset();
sub.SkipAdditivePass = !obj->IsHilighted();
sub.Handle = obj->GetHandle();
// modulate surface color with parent's current color
sub.Color = surf.GetColor() * curColor;
sub.Offsets = surf.GetAnchorOffsets();
sub.FadeDirection = obj->GetFadeDirection();
#if defined( OVR_BUILD_DEBUG )
sub.SurfaceName = surf.GetName();
#endif
curIndex++;
}
}
OVR::String const & text = obj->GetText();
if ( ( oFlags & VRMENUOBJECT_DONT_RENDER_TEXT ) == 0 && text.GetLengthI() > 0 )
{
Posef const & textLocalPose = obj->GetTextLocalPose();
Posef curTextPose;
curTextPose.Position = itemPose.Position + ( itemPose.Orientation * textLocalPose.Position * scale );
curTextPose.Orientation = textLocalPose.Orientation * itemPose.Orientation;
Vector3f textNormal = curTextPose.Orientation * Vector3f( 0.0f, 0.0f, 1.0f );
Vector3f position = curTextPose.Position + textNormal * 0.001f; // this is simply to prevent z-fighting right now
Vector3f textScale = scale * obj->GetTextLocalScale();
Vector4f textColor = obj->GetTextColor();
// Apply parent's alpha influence
textColor.w *= parentColor.w;
VRMenuFontParms const & fp = obj->GetFontParms();
fontParms_t fontParms;
fontParms.CenterHoriz = fp.CenterHoriz;
fontParms.CenterVert = fp.CenterVert;
fontParms.Billboard = fp.Billboard;
fontParms.TrackRoll = fp.TrackRoll;
fontParms.ColorCenter = fp.ColorCenter;
fontParms.AlphaCenter = fp.AlphaCenter;
fontSurface.DrawText3D( font, fontParms, position, itemNormal, itemUp,
textScale.x * fp.Scale, textColor, text.ToCStr() );
#if 0
// this shows a ruler for the wrap width when rendering text
float const wrapw = obj->GetWrapWidth();
Vector3f pos = position + Vector3f( 0.0f, 0.0f, -0.1f );
debugLines.AddLine( pos - wrapw * 0.5f * scale, pos + wrapw * 0.5f * scale,
Vector4f( 0.0f, 1.0f, 0.0f, 1.0f ), Vector4f( 1.0f, 0.0f, 0.0f, 1.0f ), 0, false );
#endif
}
//DROIDLOG( "Spam", "AddPoint for '%s'", text.ToCStr() );
//GetDebugLines().AddPoint( curModelPose.Position, 0.05f, 1, true );
}
cullBounds = obj->GetLocalBounds( font ) * parentScale;
// submit all children
if ( obj->Children.GetSizeI() > 0 )
{
for ( int i = 0; i < obj->Children.GetSizeI(); ++i )
{
menuHandle_t childHandle = obj->Children[i];
VRMenuObjectLocal const * child = static_cast< VRMenuObjectLocal const * >( ToObject( childHandle ) );
if ( child == NULL )
{
continue;
}
Bounds3f childCullBounds;
SubmitForRenderingRecursive( debugLines, font, fontSurface, flags, child, curModelPose, curColor, scale,
childCullBounds, submitted, maxIndices, curIndex );
Posef pose = child->GetLocalPose();
pose.Position = pose.Position * scale;
childCullBounds = Bounds3f::Transform( pose, childCullBounds );
cullBounds = Bounds3f::Union( cullBounds, childCullBounds );
}
}
obj->SetCullBounds( cullBounds );
#if 0
OvrCollisionPrimitive const * cp = obj->GetCollisionPrimitive();
if ( cp != NULL )
{
cp->DebugRender( debugLines, curModelPose );
}
{
// for debug drawing, put the cull bounds in world space
//LogBounds( obj->GetText().ToCStr(), "Transformed CullBounds", myCullBounds );
debugLines.AddBounds( curModelPose, obj->GetCullBounds(), Vector4f( 0.0f, 1.0f, 1.0f, 1.0f ) );
}
{
Bounds3f localBounds = obj->GetLocalBounds( font ) * parentScale;
//LogBounds( obj->GetText().ToCStr(), "localBounds", localBounds );
debugLines.AddBounds( curModelPose, localBounds, Vector4f( 1.0f, 0.0f, 0.0f, 1.0f ) );
Bounds3f textLocalBounds = obj->GetTextLocalBounds( font );
Posef hilightPose = obj->GetHilightPose();
textLocalBounds = Bounds3f::Transform( Posef( hilightPose.Orientation, hilightPose.Position * scale ), textLocalBounds );
debugLines.AddBounds( curModelPose, textLocalBounds * parentScale, Vector4f( 1.0f, 1.0f, 0.0f, 1.0f ) );
}
#endif
}
Ray Camera::get_primary_ray_dof(float x, float y, float width, float height)
{
// Primary ray
Ray pray, fray;
// Take the aspect ratio into consideration.
scalar_t ratio = (scalar_t)width / (scalar_t)height;
// Set the primary ray's origin at the camera's position.
pray.origin = position;
// Calculate the ray's intersection point on the projection plane.
pray.direction.x = (2.0 * (scalar_t)x / (scalar_t)width) - 1.0;
pray.direction.y = ((2.0 * (scalar_t)y / (scalar_t)height) - 1.0) / ratio;
pray.direction.z = 1.0 / tan(fov * NMath::RADIAN / 2.0);
// Calculate the deviated ray direction
fray.origin = pray.direction;
scalar_t half_aperture = aperture / 2.f;
fray.origin.x += NMath::prng_c(-half_aperture, half_aperture);
fray.origin.y += NMath::prng_c(-half_aperture, half_aperture);
// Find the intersection point on the focal plane
Vector3f fpip = pray.direction + flength * pray.direction.normalized();
fray.direction = fpip - fray.origin;
/*
Setting up the look-at matrix is easy when you consider that a matrix
is basically a rotated unit cube formed by three vectors (the 3x3 part) at a
particular position (the 1x3 part).
We already have one of the three vectors:
- The z-axis of the matrix is simply the view direction.
- The x-axis of the matrix is a bit tricky: if the camera is not tilted,
then the x-axis of the matrix is perpendicular to the z-axis and
the vector (0, 1, 0).
- The y-axis is perpendicular to the other two, so we simply calculate
the cross product of the x-axis and the z-axis to obtain the y-axis.
Note that the y-axis is calculated using the reversed z-axis. The
image will be upside down without this adjustment.
*/
// Calculate the camera direction vector and normalize it.
Vector3f camdir = target - position;
camdir.normalize();
Vector3f rx,ry,rz;
rz = camdir;
rx = cross(up, rz);
rx.normalize();
ry = cross(rx, rz);
ry.normalize();
Matrix4x4f tmat(rx.x, ry.x, rz.x, 0,
rx.y, ry.y, rz.y, 0,
rx.z, ry.z, rz.z, 0,
0, 0, 0, 1);
// Transform the direction vector
fray.direction.transform(tmat);
fray.direction.normalize();
// Transform the origin of the ray
fray.origin.transform(tmat);
fray.origin += position;
return fray;
}
void Update(const Phoenix::Camera &cam)
{
glClearColor(0,0,0,0);
// Cheap collision detection/response
Vector3f vCamera = cam.getPosition();
/*
if(vCamera.magnitude() < m_fInnerRadius + 0.01f)
{
Vector3f N = vCamera / vCamera.Magnitude();
Vector3f I = CCameraTask::GetPtr()->GetVelocity();
float fSpeed = I.magnitude();
I /= fSpeed;
Vector3f R = N * (2.0*(-I | N)) + I;
CCameraTask::GetPtr()->SetVelocity(R * fSpeed);
vCamera = N * (m_fInnerRadius + 0.01f);
CCameraTask::GetPtr()->SetPosition(vCamera);
}
*/
//Vector3f vUnitCamera = vCamera / vCamera.magnitude();
glPolygonMode(GL_FRONT, m_nPolygonMode);
// Draw the groud sphere
CGLShaderObject *pGroundShader;
if(vCamera.magnitude() >= m_fOuterRadius)
pGroundShader = &m_shGroundFromSpace;
else
pGroundShader = &m_shGroundFromAtmosphere;
pGroundShader->Enable();
pGroundShader->SetUniformParameter3f("v3CameraPos", vCamera.x, vCamera.y, vCamera.z);
pGroundShader->SetUniformParameter3f("v3LightPos", m_vLightDirection.x, m_vLightDirection.y, m_vLightDirection.z);
pGroundShader->SetUniformParameter3f("v3InvWavelength", 1/m_fWavelength4[0], 1/m_fWavelength4[1], 1/m_fWavelength4[2]);
pGroundShader->SetUniformParameter1f("fCameraHeight", vCamera.magnitude());
pGroundShader->SetUniformParameter1f("fCameraHeight2", vCamera.magnitudeSquared());
pGroundShader->SetUniformParameter1f("fInnerRadius", m_fInnerRadius);
pGroundShader->SetUniformParameter1f("fInnerRadius2", m_fInnerRadius*m_fInnerRadius);
pGroundShader->SetUniformParameter1f("fOuterRadius", m_fOuterRadius);
pGroundShader->SetUniformParameter1f("fOuterRadius2", m_fOuterRadius*m_fOuterRadius);
pGroundShader->SetUniformParameter1f("fKrESun", m_Kr*m_ESun);
pGroundShader->SetUniformParameter1f("fKmESun", m_Km*m_ESun);
pGroundShader->SetUniformParameter1f("fKr4PI", m_Kr4PI);
pGroundShader->SetUniformParameter1f("fKm4PI", m_Km4PI);
pGroundShader->SetUniformParameter1f("fScale", 1.0f / (m_fOuterRadius - m_fInnerRadius));
pGroundShader->SetUniformParameter1f("fScaleDepth", m_fRayleighScaleDepth);
pGroundShader->SetUniformParameter1f("fScaleOverScaleDepth", (1.0f / (m_fOuterRadius - m_fInnerRadius)) / m_fRayleighScaleDepth);
pGroundShader->SetUniformParameter1f("g", m_g);
pGroundShader->SetUniformParameter1f("g2", m_g*m_g);
pGroundShader->SetUniformParameter1i("nSamples", m_nSamples);
pGroundShader->SetUniformParameter1f("fSamples", m_nSamples);
//pGroundShader->SetUniformParameter1i("s2Test", 0);
GLUquadricObj *pSphere = gluNewQuadric();
//m_tEarth.Enable();
gluSphere(pSphere, m_fInnerRadius, 100, 50);
//m_tEarth.Disable();
gluDeleteQuadric(pSphere);
pGroundShader->Disable();
// Draw the sky sphere
CGLShaderObject *pSkyShader;
if(vCamera.magnitude() >= m_fOuterRadius)
pSkyShader = &m_shSkyFromSpace;
else
pSkyShader = &m_shSkyFromAtmosphere;
pSkyShader->Enable();
pSkyShader->SetUniformParameter3f("v3CameraPos", vCamera.x, vCamera.y, vCamera.z);
pSkyShader->SetUniformParameter3f("v3LightPos", m_vLightDirection.x, m_vLightDirection.y, m_vLightDirection.z);
pSkyShader->SetUniformParameter3f("v3InvWavelength", 1/m_fWavelength4[0], 1/m_fWavelength4[1], 1/m_fWavelength4[2]);
pSkyShader->SetUniformParameter1f("fCameraHeight", vCamera.magnitude());
pSkyShader->SetUniformParameter1f("fCameraHeight2", vCamera.magnitudeSquared());
pSkyShader->SetUniformParameter1f("fInnerRadius", m_fInnerRadius);
pSkyShader->SetUniformParameter1f("fInnerRadius2", m_fInnerRadius*m_fInnerRadius);
pSkyShader->SetUniformParameter1f("fOuterRadius", m_fOuterRadius);
pSkyShader->SetUniformParameter1f("fOuterRadius2", m_fOuterRadius*m_fOuterRadius);
pSkyShader->SetUniformParameter1f("fKrESun", m_Kr*m_ESun);
pSkyShader->SetUniformParameter1f("fKmESun", m_Km*m_ESun);
pSkyShader->SetUniformParameter1f("fKr4PI", m_Kr4PI);
pSkyShader->SetUniformParameter1f("fKm4PI", m_Km4PI);
pSkyShader->SetUniformParameter1f("fScale", 1.0f / (m_fOuterRadius - m_fInnerRadius));
pSkyShader->SetUniformParameter1f("fScaleDepth", m_fRayleighScaleDepth);
pSkyShader->SetUniformParameter1f("fScaleOverScaleDepth", (1.0f / (m_fOuterRadius - m_fInnerRadius)) / m_fRayleighScaleDepth);
pSkyShader->SetUniformParameter1f("g", m_g);
pSkyShader->SetUniformParameter1f("g2", m_g*m_g);
pSkyShader->SetUniformParameter1i("nSamples", m_nSamples);
pSkyShader->SetUniformParameter1f("fSamples", m_nSamples);
//m_tOpticalDepth.Enable();
//pSkyShader->SetUniformParameter1f("tex", 0);
glFrontFace(GL_CW);
glEnable(GL_BLEND);
glBlendFunc(GL_ONE, GL_ONE);
pSphere = gluNewQuadric();
gluSphere(pSphere, m_fOuterRadius, 100, 100);
gluDeleteQuadric(pSphere);
glDisable(GL_BLEND);
glFrontFace(GL_CCW);
//m_tOpticalDepth.Disable();
pSkyShader->Disable();
glPolygonMode(GL_FRONT, GL_FILL);
}
// perform pre-arm checks and set ap.pre_arm_check flag
// return true if the checks pass successfully
bool Copter::pre_arm_checks(bool display_failure)
{
// exit immediately if already armed
if (motors.armed()) {
return true;
}
// check if motor interlock and Emergency Stop aux switches are used
// at the same time. This cannot be allowed.
if (check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Interlock/E-Stop Conflict");
}
return false;
}
// check if motor interlock aux switch is in use
// if it is, switch needs to be in disabled position to arm
// otherwise exit immediately. This check to be repeated,
// as state can change at any time.
if (ap.using_interlock && motors.get_interlock()){
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled");
}
return false;
}
// exit immediately if we've already successfully performed the pre-arm check
if (ap.pre_arm_check) {
// run gps checks because results may change and affect LED colour
// no need to display failures because arm_checks will do that if the pilot tries to arm
pre_arm_gps_checks(false);
return true;
}
// succeed if pre arm checks are disabled
if (g.arming_check == ARMING_CHECK_NONE) {
set_pre_arm_check(true);
set_pre_arm_rc_check(true);
return true;
}
// pre-arm rc checks a prerequisite
pre_arm_rc_checks();
if (!ap.pre_arm_rc_check) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RC not calibrated");
}
return false;
}
// check Baro
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
// barometer health check
if (!barometer.all_healthy()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Barometer not healthy");
}
return false;
}
// Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
// Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
// that may differ from the baro height due to baro drift.
nav_filter_status filt_status = inertial_nav.get_filter_status();
bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
if (using_baro_ref) {
if (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Altitude disparity");
}
return false;
}
}
}
// check Compass
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_COMPASS)) {
//check if compass has calibrated and requires reboot
if (compass.compass_cal_requires_reboot()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass calibrated requires reboot");
}
return false;
}
// check the primary compass is healthy
if (!compass.healthy()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not healthy");
}
return false;
}
// check compass learning is on or offsets have been set
if (!compass.configured()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not calibrated");
}
return false;
}
// check for unreasonable compass offsets
Vector3f offsets = compass.get_offsets();
if (offsets.length() > COMPASS_OFFSETS_MAX) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass offsets too high");
}
return false;
}
// check for unreasonable mag field length
float mag_field = compass.get_field().length();
if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65f || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35f) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check mag field");
}
return false;
}
// check all compasses point in roughly same direction
if (!compass.consistent()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent compasses");
}
return false;
}
}
// check GPS
if (!pre_arm_gps_checks(display_failure)) {
return false;
}
#if AC_FENCE == ENABLED
// check fence is initialised
if (!fence.pre_arm_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check fence");
}
return false;
}
#endif
// check INS
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
// check accelerometers have been calibrated
if (!ins.accel_calibrated_ok_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accels not calibrated");
}
return false;
}
// check accels are healthy
if (!ins.get_accel_health_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accelerometers not healthy");
}
return false;
}
//check if accelerometers have calibrated and require reboot
if (ins.accel_cal_requires_reboot()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accelerometers calibrated requires reboot");
}
return false;
}
// check all accelerometers point in roughly same direction
if (ins.get_accel_count() > 1) {
const Vector3f &prime_accel_vec = ins.get_accel();
for(uint8_t i=0; i<ins.get_accel_count(); i++) {
// get next accel vector
const Vector3f &accel_vec = ins.get_accel(i);
Vector3f vec_diff = accel_vec - prime_accel_vec;
float threshold = PREARM_MAX_ACCEL_VECTOR_DIFF;
if (i >= 2) {
/*
* for boards with 3 IMUs we only use the first two
* in the EKF. Allow for larger accel discrepancy
* for IMU3 as it may be running at a different temperature
*/
threshold *= 2;
}
if (vec_diff.length() > threshold) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Accelerometers");
}
return false;
}
}
}
// check gyros are healthy
if (!ins.get_gyro_health_all()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Gyros not healthy");
}
return false;
}
// check all gyros are consistent
if (ins.get_gyro_count() > 1) {
for(uint8_t i=0; i<ins.get_gyro_count(); i++) {
// get rotation rate difference between gyro #i and primary gyro
Vector3f vec_diff = ins.get_gyro(i) - ins.get_gyro();
if (vec_diff.length() > PREARM_MAX_GYRO_VECTOR_DIFF) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Gyros");
}
return false;
}
}
}
// get ekf attitude (if bad, it's usually the gyro biases)
if (!pre_arm_ekf_attitude_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: gyros still settling");
}
return false;
}
}
#if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN
#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
// check board voltage
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
if (hal.analogin->board_voltage() < BOARD_VOLTAGE_MIN || hal.analogin->board_voltage() > BOARD_VOLTAGE_MAX) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Board Voltage");
}
return false;
}
}
#endif
#endif
// check battery voltage
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Battery");
}
return false;
}
}
// check various parameter values
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) {
// ensure ch7 and ch8 have different functions
if (check_duplicate_auxsw()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Duplicate Aux Switch Options");
}
return false;
}
// failsafe parameter checks
if (g.failsafe_throttle) {
// check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900
if (channel_throttle->radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check FS_THR_VALUE");
}
return false;
}
}
// lean angle parameter check
if (aparm.angle_max < 1000 || aparm.angle_max > 8000) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check ANGLE_MAX");
}
return false;
}
// acro balance parameter check
if ((g.acro_balance_roll > attitude_control.get_angle_roll_p().kP()) || (g.acro_balance_pitch > attitude_control.get_angle_pitch_p().kP())) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH");
}
return false;
}
#if CONFIG_SONAR == ENABLED && OPTFLOW == ENABLED
// check range finder if optflow enabled
if (optflow.enabled() && !sonar.pre_arm_check()) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder");
}
return false;
}
#endif
#if FRAME_CONFIG == HELI_FRAME
// check helicopter parameters
if (!motors.parameter_check(display_failure)) {
return false;
}
#endif // HELI_FRAME
// check for missing terrain data
if (!pre_arm_terrain_check()) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data");
return false;
}
}
// check throttle is above failsafe throttle
// this is near the bottom to allow other failures to be displayed before checking pilot throttle
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) {
if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) {
if (display_failure) {
#if FRAME_CONFIG == HELI_FRAME
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Collective below Failsafe");
#else
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Throttle below Failsafe");
#endif
}
return false;
}
}
return true;
}
void Cube::update(const Vector3f &min, const Vector3f &max) {
std::string v =
std::to_string(max.x()) + " " + std::to_string(min.y()) + " " + std::to_string(min.z()) + "\n" +
std::to_string(max.x()) + " " + std::to_string(min.y()) + " " + std::to_string(max.z()) + "\n" +
std::to_string(min.x()) + " " + std::to_string(min.y()) + " " + std::to_string(max.z()) + "\n" +
std::to_string(min.x()) + " " + std::to_string(min.y()) + " " + std::to_string(min.z()) + "\n" +
std::to_string(max.x()) + " " + std::to_string(max.y()) + " " + std::to_string(min.z()) + "\n" +
std::to_string(max.x()) + " " + std::to_string(max.y()) + " " + std::to_string(max.z()) + "\n" +
std::to_string(min.x()) + " " + std::to_string(max.y()) + " " + std::to_string(max.z()) + "\n" +
std::to_string(min.x()) + " " + std::to_string(max.y()) + " " + std::to_string(min.z()) + "\n";
std::istringstream is(v);
std::string line_str;
std::vector<Vector3f> newPos;
m_bbox.reset();
while (std::getline(is, line_str)) {
std::istringstream line(line_str);
Point3f p;
line >> p.x() >> p.y() >> p.z();
newPos.push_back(p);
m_bbox.expandBy(p);
}
for (uint32_t i = 0; i < m_V.cols(); ++i)
m_V.col(i) = newPos.at(vertices[i].p - 1);
// Update data on GPU
glBindVertexArray(vao);
glBindBuffer(GL_ARRAY_BUFFER, vbo[VERTEX_BUFFER]);
// Update data only
void* ptr = glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY);
if (ptr) {
memcpy(ptr, (const uint8_t *)m_V.data(), 3 * m_V.cols() * sizeof(GLfloat));
glUnmapBuffer(GL_ARRAY_BUFFER);
}
glBindBuffer(GL_ARRAY_BUFFER, 0);
glBindVertexArray(0);
}
//----------------------------------------------------------------------------------------------------
Vector3f Vector3f::Clamp( const Vector3f& A )
{
Vector3f vec = A;
vec.Clamp();
return vec;
}
traceResult_t RtTrace::Trace( const Vector3f & start, const Vector3f & end ) const
{
traceResult_t result;
result.triangleIndex = -1;
result.fraction = 1.0f;
result.uv = Vector2f( 0.0f );
result.normal = Vector3f( 0.0f );
const Vector3f rayDelta = end - start;
const float rayLengthSqr = rayDelta.LengthSq();
const float rayLengthRcp = RcpSqrt( rayLengthSqr );
const float rayLength = rayLengthSqr * rayLengthRcp;
const Vector3f rayDir = rayDelta * rayLengthRcp;
const float rcpRayDirX = ( fabsf( rayDir.x ) > Math<float>::SmallestNonDenormal ) ? ( 1.0f / rayDir.x ) : Math<float>::HugeNumber;
const float rcpRayDirY = ( fabsf( rayDir.y ) > Math<float>::SmallestNonDenormal ) ? ( 1.0f / rayDir.y ) : Math<float>::HugeNumber;
const float rcpRayDirZ = ( fabsf( rayDir.z ) > Math<float>::SmallestNonDenormal ) ? ( 1.0f / rayDir.z ) : Math<float>::HugeNumber;
const float sX = ( header.bounds.GetMins()[0] - start.x ) * rcpRayDirX;
const float sY = ( header.bounds.GetMins()[1] - start.y ) * rcpRayDirY;
const float sZ = ( header.bounds.GetMins()[2] - start.z ) * rcpRayDirZ;
const float tX = ( header.bounds.GetMaxs()[0] - start.x ) * rcpRayDirX;
const float tY = ( header.bounds.GetMaxs()[1] - start.y ) * rcpRayDirY;
const float tZ = ( header.bounds.GetMaxs()[2] - start.z ) * rcpRayDirZ;
const float minX = Alg::Min( sX, tX );
const float minY = Alg::Min( sY, tY );
const float minZ = Alg::Min( sZ, tZ );
const float maxX = Alg::Max( sX, tX );
const float maxY = Alg::Max( sY, tY );
const float maxZ = Alg::Max( sZ, tZ );
const float t0 = Alg::Max( minX, Alg::Max( minY, minZ ) );
const float t1 = Alg::Min( maxX, Alg::Min( maxY, maxZ ) );
if ( t0 >= t1 )
{
return result;
}
float entryDistance = Alg::Max( t0, 0.0f );
float bestDistance = Alg::Min( t1 + 0.00001f, rayLength );
Vector2f uv;
const kdtree_node_t * currentNode = &nodes[0];
for ( int i = 0; i < RT_KDTREE_MAX_ITERATIONS; i++ )
{
const Vector3f rayEntryPoint = start + rayDir * entryDistance;
// Step down the tree until a leaf node is found.
while ( ( currentNode->data & 1 ) == 0 )
{
// Select the child node based on whether the entry point is left or right of the split plane.
// If the entry point is directly at the split plane then choose the side based on the ray direction.
const int nodePlane = ( ( currentNode->data >> 1 ) & 3 );
int child;
if ( rayEntryPoint[nodePlane] - currentNode->dist < 0.00001f ) child = 0;
else if ( rayEntryPoint[nodePlane] - currentNode->dist > 0.00001f ) child = 1;
else child = ( rayDelta[nodePlane] > 0.0f );
currentNode = &nodes[( currentNode->data >> 3 ) + child];
}
// Check for an intersection with a triangle in this leaf.
const kdtree_leaf_t * currentLeaf = &leafs[( currentNode->data >> 3 )];
const int * leafTriangles = currentLeaf->triangles;
int leafTriangleCount = RT_KDTREE_MAX_LEAF_TRIANGLES;
for ( int j = 0; j < leafTriangleCount; j++ )
{
int currentTriangle = leafTriangles[j];
if ( currentTriangle < 0 )
{
if ( currentTriangle == -1 )
{
break;
}
const int offset = ( currentTriangle & 0x7FFFFFFF );
leafTriangles = &overflow[offset];
leafTriangleCount = header.numOverflow - offset;
j = 0;
currentTriangle = leafTriangles[0];
}
float distance;
float u;
float v;
if ( RtIntersect::RayTriangle( start, rayDir,
vertices[indices[currentTriangle * 3 + 0]],
vertices[indices[currentTriangle * 3 + 1]],
vertices[indices[currentTriangle * 3 + 2]], distance, u, v ) )
{
if ( distance >= 0.0f && distance < bestDistance )
{
bestDistance = distance;
result.triangleIndex = currentTriangle * 3;
uv.x = u;
uv.y = v;
}
}
}
// Calculate the distance along the ray where the next leaf is entered.
const float sX = ( currentLeaf->bounds.GetMins()[0] - start.x ) * rcpRayDirX;
const float sY = ( currentLeaf->bounds.GetMins()[1] - start.y ) * rcpRayDirY;
const float sZ = ( currentLeaf->bounds.GetMins()[2] - start.z ) * rcpRayDirZ;
const float tX = ( currentLeaf->bounds.GetMaxs()[0] - start.x ) * rcpRayDirX;
const float tY = ( currentLeaf->bounds.GetMaxs()[1] - start.y ) * rcpRayDirY;
const float tZ = ( currentLeaf->bounds.GetMaxs()[2] - start.z ) * rcpRayDirZ;
const float maxX = Alg::Max( sX, tX );
const float maxY = Alg::Max( sY, tY );
const float maxZ = Alg::Max( sZ, tZ );
entryDistance = Alg::Min( maxX, Alg::Min( maxY, maxZ ) );
if ( entryDistance >= bestDistance )
{
break;
}
// Calculate the exit plane.
const int exitX = ( 0 << 1 ) | ( ( sX < tX ) ? 1 : 0 );
const int exitY = ( 1 << 1 ) | ( ( sY < tY ) ? 1 : 0 );
const int exitZ = ( 2 << 1 ) | ( ( sZ < tZ ) ? 1 : 0 );
const int exitPlane = ( maxX < maxY ) ? ( maxX < maxZ ? exitX : exitZ ) : ( maxY < maxZ ? exitY : exitZ );
// Use a rope to enter the adjacent leaf.
const int exitNodeIndex = currentLeaf->ropes[exitPlane];
if ( exitNodeIndex == -1 )
{
break;
}
currentNode = &nodes[exitNodeIndex];
}
if ( result.triangleIndex != -1 )
{
result.fraction = bestDistance * rayLengthRcp;
result.uv = uvs[indices[result.triangleIndex + 0]] * ( 1.0f - uv.x - uv.y ) +
uvs[indices[result.triangleIndex + 1]] * uv.x +
uvs[indices[result.triangleIndex + 2]] * uv.y;
const Vector3f d1 = vertices[indices[result.triangleIndex + 1]] - vertices[indices[result.triangleIndex + 0]];
const Vector3f d2 = vertices[indices[result.triangleIndex + 2]] - vertices[indices[result.triangleIndex + 0]];
result.normal = d1.Cross( d2 ).Normalized();
}
return result;
}
//----------------------------------------------------------------------------------------------------
float Vector3f::Dot( const Vector3f& A, const Vector3f& B )
{
return A.Dot( B );
}
/**
* Compute the scale factor and bias associated with a vector observer
* according to the equation:
* B_k = (I_{3x3} + D)^{-1} \times (O^T A_k H_k + b + \epsilon_k)
* where k is the sample index,
* B_k is the kth measurement
* I_{3x3} is a 3x3 identity matrix
* D is a 3x3 diagonal matrix of scale factors
* O is the orthogonal alignment matrix
* A_k is the attitude at the kth sample
* b is the bias in the global reference frame
* \epsilon_k is the noise at the kth sample
* This implementation makes the assumption that \epsilon is a constant white,
* gaussian noise source that is common to all k. The misalignment matrix O
* is not computed, and the off-diagonal elements of D, corresponding to the
* misalignment scale factors are not either.
*
* @param bias[out] The computed bias, in the global frame
* @param scale[out] The computed scale factor, in the sensor frame
* @param samples[in] An array of measurement samples.
* @param n_samples The number of samples in the array.
* @param referenceField The magnetic field vector at this location.
* @param noise The one-sigma squared standard deviation of the observed noise
* in the sensor.
*/
void twostep_bias_scale(Vector3f & bias,
Vector3f & scale,
const Vector3f samples[],
const size_t n_samples,
const Vector3f & referenceField,
const float noise)
{
// Initial estimate for gradiant descent starts at eq 37a of ref 2.
// Define L_k by eq 30 and 28 for k = 1 .. n_samples
Matrix<double, Dynamic, 6> fullSamples(n_samples, 6);
// \hbar{L} by eq 33, simplified by obesrving that the
Matrix<double, 1, 6> centerSample = Matrix<double, 1, 6>::Zero();
// Define the sample differences z_k by eq 23 a)
double sampleDeltaMag[n_samples];
// The center value \hbar{z}
double sampleDeltaMagCenter = 0;
double refSquaredNorm = referenceField.squaredNorm();
for (size_t i = 0; i < n_samples; ++i) {
fullSamples.row(i) << 2 * samples[i].transpose().cast<double>(),
-(samples[i][0] * samples[i][0]),
-(samples[i][1] * samples[i][1]),
-(samples[i][2] * samples[i][2]);
centerSample += fullSamples.row(i);
sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm;
sampleDeltaMagCenter += sampleDeltaMag[i];
}
sampleDeltaMagCenter /= n_samples;
centerSample /= n_samples;
// True for all k.
// double mu = -3*noise;
// The center value of mu, \bar{mu}
// double centerMu = mu;
// The center value of mu, \tilde{mu}
// double centeredMu = 0;
// Define \tilde{L}_k for k = 0 .. n_samples
Matrix<double, Dynamic, 6> centeredSamples(n_samples, 6);
// Define \tilde{z}_k for k = 0 .. n_samples
double centeredMags[n_samples];
// Compute the term under the summation of eq 37a
Matrix<double, 6, 1> estimateSummation = Matrix<double, 6, 1>::Zero();
for (size_t i = 0; i < n_samples; ++i) {
centeredSamples.row(i) = fullSamples.row(i) - centerSample;
centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter;
estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose();
}
estimateSummation /= noise; // note: paper supplies 1/noise
// By eq 37 b). Note, paper supplies 1/noise here
Matrix<double, 6, 6> P_theta_theta_inv = (1.0f / noise) *
centeredSamples.transpose() * centeredSamples;
#ifdef PRINTF_DEBUGGING
SelfAdjointEigenSolver<Matrix<double, 6, 6> > eig(P_theta_theta_inv);
std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n";
std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose()
<< "\n";
std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n";
#endif
// The Fisher information matrix has a small eigenvalue, with a
// corresponding eigenvector of about [1e-2 1e-2 1e-2 0.55, 0.55,
// 0.55]. This means that there is relatively little information
// about the common gain on the scale factor, which has a
// corresponding effect on the bias, too. The fixup is performed by
// the first iteration of the second stage of TWOSTEP, as documented in
// [1].
Matrix<double, 6, 1> estimate;
// By eq 37 a), \tilde{Fisher}^-1
P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate);
#ifdef PRINTF_DEBUGGING
Matrix<double, 6, 6> P_theta_theta;
P_theta_theta_inv.ldlt().solve(Matrix<double, 6, 6>::Identity(), &P_theta_theta);
SelfAdjointEigenSolver<Matrix<double, 6, 6> > eig2(P_theta_theta);
std::cout << "eigenvalues: " << eig2.eigenvalues().transpose() << "\n";
std::cout << "eigenvectors: \n" << eig2.eigenvectors() << "\n";
std::cout << "estimate summation: \n" << estimateSummation.normalized()
<< "\n";
#endif
// estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradiant(theta)
// Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1}
size_t count = 3;
while (count-- > 0) {
// eq 40
Matrix<double, 1, 6> db_dtheta = dnormb_dtheta(estimate);
Matrix<double, 6, 1> dJ_dtheta = dJdb(centerSample,
sampleDeltaMagCenter,
estimate,
db_dtheta,
-3 * noise,
noise / n_samples);
// Eq 39 (with double-negation on term inside parens)
db_dtheta = centerSample - db_dtheta;
Matrix<double, 6, 6> scale = P_theta_theta_inv +
(double(n_samples) / noise) * db_dtheta.transpose() * db_dtheta;
// eq 14b, mutatis mutandis (grumble, grumble)
Matrix<double, 6, 1> increment;
scale.ldlt().solve(dJ_dtheta, &increment);
estimate -= increment;
}
// Transform the estimated parameters from [c | e] back into [b | d].
for (size_t i = 0; i < 3; ++i) {
scale.coeffRef(i) = -1 + sqrt(1 + estimate.coeff(3 + i));
bias.coeffRef(i) = estimate.coeff(i) / sqrt(1 + estimate.coeff(3 + i));
}
}
//----------------------------------------------------------------------------------------------------
Vector3f Vector3f::Perpendicular( const Vector3f& A )
{
Vector3f vec = A;
return vec.Perpendicular();
}
/**
* Compute the scale factor and bias associated with a vector observer
* according to the equation:
* B_k = (I_{3x3} + D)^{-1} \times (O^T A_k H_k + b + \epsilon_k)
* where k is the sample index,
* B_k is the kth measurement
* I_{3x3} is a 3x3 identity matrix
* D is a 3x3 symmetric matrix of scale factors
* O is the orthogonal alignment matrix
* A_k is the attitude at the kth sample
* b is the bias in the global reference frame
* \epsilon_k is the noise at the kth sample
*
* After computing the scale factor and bias matrices, the optimal estimate is
* given by
* \tilde{B}_k = (I_{3x3} + D)B_k - b
*
* This implementation makes the assumption that \epsilon is a constant white,
* gaussian noise source that is common to all k. The misalignment matrix O
* is not computed.
*
* @param bias[out] The computed bias, in the global frame
* @param scale[out] The computed scale factor matrix, in the sensor frame.
* @param samples[in] An array of measurement samples.
* @param n_samples The number of samples in the array.
* @param referenceField The magnetic field vector at this location.
* @param noise The one-sigma squared standard deviation of the observed noise
* in the sensor.
*/
void twostep_bias_scale(Vector3f & bias,
Matrix3f & scale,
const Vector3f samples[],
const size_t n_samples,
const Vector3f & referenceField,
const float noise)
{
// Define L_k by eq 51 for k = 1 .. n_samples
Matrix<double, Dynamic, 9> fullSamples(n_samples, 9);
// \hbar{L} by eq 52, simplified by observing that the common noise term
// makes this a simple average.
Matrix<double, 1, 9> centerSample = Matrix<double, 1, 9>::Zero();
// Define the sample differences z_k by eq 23 a)
double sampleDeltaMag[n_samples];
// The center value \hbar{z}
double sampleDeltaMagCenter = 0;
// The squared norm of the reference vector
double refSquaredNorm = referenceField.squaredNorm();
for (size_t i = 0; i < n_samples; ++i) {
fullSamples.row(i) << 2 * samples[i].transpose().cast<double>(),
-(samples[i][0] * samples[i][0]),
-(samples[i][1] * samples[i][1]),
-(samples[i][2] * samples[i][2]),
-2 * (samples[i][0] * samples[i][1]),
-2 * (samples[i][0] * samples[i][2]),
-2 * (samples[i][1] * samples[i][2]);
centerSample += fullSamples.row(i);
sampleDeltaMag[i] = samples[i].squaredNorm() - refSquaredNorm;
sampleDeltaMagCenter += sampleDeltaMag[i];
}
sampleDeltaMagCenter /= n_samples;
centerSample /= n_samples;
// Define \tilde{L}_k for k = 0 .. n_samples
Matrix<double, Dynamic, 9> centeredSamples(n_samples, 9);
// Define \tilde{z}_k for k = 0 .. n_samples
double centeredMags[n_samples];
// Compute the term under the summation of eq 57a
Matrix<double, 9, 1> estimateSummation = Matrix<double, 9, 1>::Zero();
for (size_t i = 0; i < n_samples; ++i) {
centeredSamples.row(i) = fullSamples.row(i) - centerSample;
centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter;
estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose();
}
estimateSummation /= noise;
// By eq 57b
Matrix<double, 9, 9> P_theta_theta_inv = (1.0f / noise) *
centeredSamples.transpose() * centeredSamples;
#ifdef PRINTF_DEBUGGING
SelfAdjointEigenSolver<Matrix<double, 9, 9> > eig(P_theta_theta_inv);
std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n";
std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose()
<< "\n";
std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n";
#endif
// The current value of the estimate. Initialized to \tilde{\theta}^*
Matrix<double, 9, 1> estimate;
// By eq 57a
P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate);
// estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradient(theta)
// Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1}
size_t count = 0;
double eta = 10000;
while (count++ < 200 && eta > 1e-8) {
static bool warned = false;
if (hasNaN(estimate)) {
std::cout << "WARNING: found NaN at time " << count << "\n";
warned = true;
}
#if 0
SelfAdjointEigenSolver<Matrix3d> eig_E(E_theta(estimate));
Vector3d S = eig_E.eigenvalues();
Vector3d W; W << -1 + sqrt(1 + S.coeff(0)),
-1 + sqrt(1 + S.coeff(1)),
-1 + sqrt(1 + S.coeff(2));
scale = (eig_E.eigenvectors() * W.asDiagonal() *
eig_E.eigenvectors().transpose()).cast<float>();
(Matrix3f::Identity() + scale).ldlt().solve(
estimate.start<3>().cast<float>(), &bias);
std::cout << "\n\nestimated bias: " << bias.transpose()
<< "\nestimated scale:\n" << scale;
#endif
Matrix<double, 1, 9> db_dtheta = dnormb_dtheta(estimate);
Matrix<double, 9, 1> dJ_dtheta = ::dJ_dtheta(centerSample,
sampleDeltaMagCenter,
estimate,
db_dtheta,
-3 * noise,
noise / n_samples);
// Eq 59, with reused storage on db_dtheta
db_dtheta = centerSample - db_dtheta;
Matrix<double, 9, 9> scale = P_theta_theta_inv +
(double(n_samples) / noise) * db_dtheta.transpose() * db_dtheta;
// eq 14b, mutatis mutandis (grumble, grumble)
Matrix<double, 9, 1> increment;
scale.ldlt().solve(dJ_dtheta, &increment);
estimate -= increment;
eta = increment.dot(scale * increment);
std::cout << "eta: " << eta << "\n";
}
std::cout << "terminated at eta = " << eta
<< " after " << count << " iterations\n";
if (!isnan(eta) && !isinf(eta)) {
// Transform the estimated parameters from [c | E] back into [b | D].
// See eq 63-65
SelfAdjointEigenSolver<Matrix3d> eig_E(E_theta(estimate));
Vector3d S = eig_E.eigenvalues();
Vector3d W; W << -1 + sqrt(1 + S.coeff(0)),
-1 + sqrt(1 + S.coeff(1)),
-1 + sqrt(1 + S.coeff(2));
scale = (eig_E.eigenvectors() * W.asDiagonal() *
eig_E.eigenvectors().transpose()).cast<float>();
(Matrix3f::Identity() + scale).ldlt().solve(
estimate.start<3>().cast<float>(), &bias);
} else {
// return nonsense data. The eigensolver can fall ingo
// an infinite loop otherwise.
// TODO: Add error code return
scale = Matrix3f::Ones() * std::numeric_limits<float>::quiet_NaN();
bias = Vector3f::Zero();
}
}
void PointCloudInput::read1()
{
if(port1.isNew()){
do {
port1.read();
} while(port1.isNew());
bool rgb = false;
for(size_t i=0; i < pointCloud1.fields.length(); ++i){
string name = string(pointCloud1.fields[i].name);
pointCloudField[name].offset = pointCloud1.fields[i].offset;
pointCloudField[name].type = pointCloud1.fields[i].data_type;
pointCloudField[name].count = pointCloud1.fields[i].count;
if(i==5){
rgb = true;
}
}
int n = pointCloud1.height * pointCloud1.width;
int m = pointCloud1.data.length() / pointCloud1.point_step;
int numPoints = std::min(n, m);
unsigned char* src = (unsigned char*)pointCloud1.data.get_buffer();
auto tmpPoints = std::make_shared<vector<Vector3f>>();
std::shared_ptr<Image> tmpImage;
unsigned char* pixels = nullptr;
if(rgb){
tmpImage = std::make_shared<Image>();
tmpImage->setSize(pointCloud1.width, pointCloud1.height, 3);
pixels = tmpImage->pixels();
}
tmpPoints->clear();
tmpPoints->reserve(numPoints);
PointCloudField& fex = pointCloudField["x"];
PointCloudField& fey = pointCloudField["y"];
PointCloudField& fez = pointCloudField["z"];
PointCloudField& fer = pointCloudField["r"];
PointCloudField& feg = pointCloudField["g"];
PointCloudField& feb = pointCloudField["b"];
for(int i=0; i < numPoints; ++i, src+=pointCloud1.point_step){
Vector3f point;
point.x() = readFloatPointCloudData(&src[fex.offset], fex, pointCloud1.is_bigendian );
point.y() = readFloatPointCloudData(&src[fey.offset], fey, pointCloud1.is_bigendian );
point.z() = readFloatPointCloudData(&src[fez.offset], fez, pointCloud1.is_bigendian );
tmpPoints->push_back(point);
if(rgb && pixels){
pixels[0] = src[fer.offset];
pixels[1] = src[feg.offset];
pixels[2] = src[feb.offset];
pixels += 3;
}
}
if(!rgb || !pixels){
tmpImage.reset();
}
RangeCameraPtr tmpRangeCamera = rangeCamera;
callLater([tmpRangeCamera, tmpPoints, tmpImage]() mutable {
tmpRangeCamera->setPoints(tmpPoints);
if(tmpImage){
tmpRangeCamera->setImage(tmpImage);
}
tmpRangeCamera->notifyStateChange();
});
}
}
void BoundingAxis::_createAxisTicks(const Vector3f& start, const Vector3f& end,
const Vector3f& normal,
const Vector3f& normal2, bool flipTick,
BoundingAxisData& bbAxisData)
{
size_t currentAxis;
if (Vector3f(end - start).find_max_index() >
std::numeric_limits<float>::min())
currentAxis = Vector3f(end - start).find_max_index();
else
currentAxis = Vector3f(end - start).find_min_index();
const Vector2f range = _computeRange(bbAxisData.volInfo, currentAxis);
const float rangeLength = range.y() - range.x();
const float worldSpaceLength = start.distance(end);
const float dataToWorldScale = worldSpaceLength / rangeLength;
const float startTickValue = bbAxisData.tickDistance *
std::ceil(range.x() / bbAxisData.tickDistance);
const float worldSpaceOffset =
(startTickValue - range.x()) * dataToWorldScale;
const float worldSpaceTickDistance =
bbAxisData.tickDistance * dataToWorldScale;
const int32_t numberOfTick =
(range.y() - startTickValue) / bbAxisData.tickDistance + 1;
const Vector3f axisDir = vmml::normalize(end - start);
Vector3f cross = vmml::normalize(vmml::cross(axisDir, normal));
size_t tickAxis;
if (cross.find_max() <= std::numeric_limits<float>::min())
tickAxis = cross.find_min_index();
else
tickAxis = cross.find_max_index();
for (int32_t i = 0; i < numberOfTick; ++i)
{
float tickHeight = 0.014f;
Vector4f color(1.0, 0.69, 0.69, 1.0);
const int32_t startTickValueInteger =
std::round(startTickValue / bbAxisData.factor);
const int32_t tickDistanceInteger =
std::round(bbAxisData.tickDistance / bbAxisData.factor);
if (((startTickValueInteger + tickDistanceInteger * i) %
(tickDistanceInteger * 5)) == 0)
{
tickHeight *= 1.5;
color = Vector4f(1.0, 1.0, 0.69, 1.0);
}
int sign = (0.0f < cross[tickAxis]) - (cross[tickAxis] < 0.0f);
if (flipTick)
sign = -sign;
Vector3f posOnAxis =
start + axisDir * (worldSpaceOffset + worldSpaceTickDistance * i);
append3dVector(bbAxisData.vertices, posOnAxis);
posOnAxis[tickAxis] = posOnAxis[tickAxis] + sign * tickHeight;
append3dVector(bbAxisData.vertices, posOnAxis);
for (uint32_t j = 0; j < 2; ++j)
{
append3dVector(bbAxisData.normals, normal);
append3dVector(bbAxisData.normals2, normal2);
append4dVector(bbAxisData.colors, color);
bbAxisData.types.push_back(AXIS);
}
_nVertices += 2;
}
}
// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros.
//
// This function also updates _omega_yaw_P with a yaw correction term
// from our yaw reference vector
void
AP_AHRS_DCM::drift_correction(float deltat)
{
float error_course = 0;
Vector3f accel;
Vector3f error;
float error_norm = 0;
float yaw_deltat = 0;
accel = _accel_vector;
// if enabled, use the GPS to correct our accelerometer vector
// for centripetal forces
if(_centripetal &&
_gps != NULL &&
_gps->status() == GPS::GPS_OK) {
accel_adjust(accel);
}
//*****Roll and Pitch***************
// normalise the accelerometer vector to a standard length
// this is important to reduce the impact of noise on the
// drift correction, as very noisy vectors tend to have
// abnormally high lengths. By normalising the length we
// reduce their impact.
float accel_length = accel.length();
accel *= (_gravity / accel_length);
if (accel.is_inf()) {
// we can't do anything useful with this sample
_omega_P.zero();
return;
}
// calculate the error, in m/s^2, between the attitude
// implied by the accelerometers and the attitude
// in the current DCM matrix
error = _dcm_matrix.c % accel;
// Limit max error to limit the effect of noisy values
// on the algorithm. This limits the error to about 11
// degrees
error_norm = error.length();
if (error_norm > 2) {
error *= (2 / error_norm);
}
// we now want to calculate _omega_P and _omega_I. The
// _omega_P value is what drags us quickly to the
// accelerometer reading.
_omega_P = error * _kp_roll_pitch;
// the _omega_I is the long term accumulated gyro
// error. This determines how much gyro drift we can
// handle.
_omega_I_sum += error * (_ki_roll_pitch * deltat);
_omega_I_sum_time += deltat;
// these sums support the reporting of the DCM state via MAVLink
_error_rp_sum += error_norm;
_error_rp_count++;
// yaw drift correction
// we only do yaw drift correction when we get a new yaw
// reference vector. In between times we rely on the gyros for
// yaw. Avoiding this calculation on every call to
// update_DCM() saves a lot of time
if (_compass && _compass->use_for_yaw()) {
if (_compass->last_update != _compass_last_update) {
yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
if (_have_initial_yaw && yaw_deltat < 2.0) {
// Equation 23, Calculating YAW error
// We make the gyro YAW drift correction based
// on compass magnetic heading
error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);
_compass_last_update = _compass->last_update;
} else {
// this is our first estimate of the yaw,
// or the compass has come back online after
// no readings for 2 seconds.
//
// construct a DCM matrix based on the current
// roll/pitch and the compass heading.
// First ensure the compass heading has been
// calculated
_compass->calculate(_dcm_matrix);
// now construct a new DCM matrix
_compass->null_offsets_disable();
_dcm_matrix.from_euler(roll, pitch, _compass->heading);
_compass->null_offsets_enable();
_have_initial_yaw = true;
_compass_last_update = _compass->last_update;
error_course = 0;
}
}
} else if (_gps && _gps->status() == GPS::GPS_OK) {
if (_gps->last_fix_time != _gps_last_update) {
// Use GPS Ground course to correct yaw gyro drift
if (_gps->ground_speed >= GPS_SPEED_MIN) {
yaw_deltat = 1.0e-3*(_gps->last_fix_time - _gps_last_update);
if (_have_initial_yaw && yaw_deltat < 2.0) {
float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
float course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
// Equation 23, Calculating YAW error
error_course = (_dcm_matrix.a.x * course_over_ground_y) - (_dcm_matrix.b.x * course_over_ground_x);
_gps_last_update = _gps->last_fix_time;
} else {
// when we first start moving, set the
// DCM matrix to the current
// roll/pitch values, but with yaw
// from the GPS
if (_compass) {
_compass->null_offsets_disable();
}
_dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course));
if (_compass) {
_compass->null_offsets_enable();
}
_have_initial_yaw = true;
error_course = 0;
_gps_last_update = _gps->last_fix_time;
}
} else if (_gps->ground_speed >= GPS_SPEED_RESET) {
// we are not going fast enough to use GPS for
// course correction, but we won't reset
// _have_initial_yaw yet, instead we just let
// the gyro handle yaw
error_course = 0;
} else {
// we are moving very slowly. Reset
// _have_initial_yaw and adjust our heading
// rapidly next time we get a good GPS ground
// speed
error_course = 0;
_have_initial_yaw = false;
}
}
}
// see if there is any error in our heading relative to the
// yaw reference. This will be zero most of the time, as we
// only calculate it when we get new data from the yaw
// reference source
if (yaw_deltat == 0 || error_course == 0) {
// we don't have a new reference heading. Slowly
// decay the _omega_yaw_P to ensure that if we have
// lost the yaw reference sensor completely we don't
// keep using a stale offset
_omega_yaw_P *= 0.97;
goto check_sum_time;
}
// ensure the course error is scaled from -PI to PI
if (error_course > PI) {
error_course -= 2*PI;
} else if (error_course < -PI) {
error_course += 2*PI;
}
// Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft
// this gives us an error in radians
error = _dcm_matrix.c * error_course;
// Adding yaw correction to proportional correction vector. We
// allow the yaw reference source to affect all 3 components
// of _omega_yaw_P as we need to be able to correctly hold a
// heading when roll and pitch are non-zero
_omega_yaw_P = error * _kp_yaw;
// add yaw correction to integrator correction vector, but
// only for the z gyro. We rely on the accelerometers for x
// and y gyro drift correction. Using the compass or GPS for
// x/y drift correction is too inaccurate, and can lead to
// incorrect builups in the x/y drift. We rely on the
// accelerometers to get the x/y components right
_omega_I_sum.z += error.z * (_ki_yaw * yaw_deltat);
// we keep the sum of yaw error for reporting via MAVLink.
_error_yaw_sum += error_course;
_error_yaw_count++;
check_sum_time:
if (_omega_I_sum_time > 10) {
// every 10 seconds we apply the accumulated
// _omega_I_sum changes to _omega_I. We do this to
// prevent short term feedback between the P and I
// terms of the controller. The _omega_I vector is
// supposed to refect long term gyro drift, but with
// high noise it can start to build up due to short
// term interactions. By summing it over 10 seconds we
// prevent the short term interactions and can apply
// the slope limit more accurately
float drift_limit = _gyro_drift_limit * _omega_I_sum_time;
_omega_I_sum.x = constrain(_omega_I_sum.x, -drift_limit, drift_limit);
_omega_I_sum.y = constrain(_omega_I_sum.y, -drift_limit, drift_limit);
_omega_I_sum.z = constrain(_omega_I_sum.z, -drift_limit, drift_limit);
_omega_I += _omega_I_sum;
// zero the sum
_omega_I_sum.zero();
_omega_I_sum_time = 0;
}
}
void Renderer::renderSLG() {
FileUtils::createDir("slg");
std::string currentDir = FileUtils::getCurrentDir();
FileUtils::changeCurrentDir("slg");
std::string imageFilename = tfm::format("../%s-images/frame-%04d.png", _animationName, _frameIndex);
int width = 1280;
int height = 720;
int spp = 64;
{
std::ofstream os("render.cfg");
os << "opencl.platform.index = -1" << std::endl;
os << "opencl.cpu.use = 0" << std::endl;
os << "opencl.gpu.use = 1" << std::endl;
os << "opencl.gpu.workgroup.size = 64" << std::endl;
os << "scene.epsilon.min = 9.99999971718068536574719e-10" << std::endl;
os << "scene.epsilon.max = 0.100000001490116119384766" << std::endl;
os << "renderengine.type = PATHOCL" << std::endl;
os << "accelerator.instances.enable = 0" << std::endl;
os << "path.maxdepth = 12" << std::endl;
os << tfm::format("film.width = %d", width) << std::endl;
os << tfm::format("film.height = %d", height) << std::endl;
os << tfm::format("image.filename = %s", imageFilename) << std::endl;
os << "sampler.type = RANDOM" << std::endl;
os << "film.filter.type = GAUSSIAN" << std::endl;
os << "film.filter.xwidth = 1" << std::endl;
os << "film.filter.ywidth = 1" << std::endl;
os << "scene.file = scene.scn" << std::endl;
os << "film.imagepipeline.0.type = TONEMAP_LINEAR" << std::endl;
os << "film.imagepipeline.1.type = GAMMA_CORRECTION" << std::endl;
os << "film.imagepipeline.1.value = 2.2" << std::endl;
os << tfm::format("batch.haltspp = %d", spp) << std::endl;
}
{
std::ofstream os("scene.scn");
Vector3f origin = _engine.camera().position();
Vector3f target = _engine.camera().target();
Vector3f up = _engine.camera().up();
float fov = _engine.camera().fov() * 1.6f;
os << tfm::format("scene.camera.lookat.orig = %f %f %f", origin.x(), origin.y(), origin.z()) << std::endl;
os << tfm::format("scene.camera.lookat.target = %f %f %f", target.x(), target.y(), target.z()) << std::endl;
os << tfm::format("scene.camera.up = %f %f %f", up.x(), up.y(), up.z()) << std::endl;
os << tfm::format("scene.camera.fieldofview = %f", fov) << std::endl;
#if 0
#scene.camera.screenwindow = -1 1 -1 1
scene.camera.cliphither = 0.0010000000474974513053894
scene.camera.clipyon = 1.00000001504746621987669e+30
scene.camera.lensradius = 0.00218750000931322574615479
scene.camera.focaldistance = 0.28457677364349365234375
scene.camera.horizontalstereo.enable = 0
scene.camera.horizontalstereo.oculusrift.barrelpostpro.enable = 0
scene.camera.autofocus.enable = 1
#endif
Vector3f dir = Vector3f(1.f).normalized();
#if 0
os << tfm::format("scene.sunlight.dir = %f %f %f", dir.x(), dir.y(), dir.z()) << std::endl;
os << "scene.sunlight.turbidity = 2.2" << std::endl;
os << "scene.sunlight.relsize = 1" << std::endl;
os << "scene.sunlight.gain = 0.0001 0.0001 0.0001" << std::endl;
#endif
os << tfm::format("scene.skylight.dir = %f %f %f", dir.x(), dir.y(), dir.z()) << std::endl;
os << "scene.skylight.turbidity = 2.2" << std::endl;
os << "scene.skylight.gain = 0.00007 0.00007 0.00007" << std::endl;
os << "scene.materials.fluid.type = matte" << std::endl;
os << "scene.materials.fluid.kd = 0.3 0.3 1.0" << std::endl;
os << "scene.materials.boundary.type = matte" << std::endl;
os << "scene.materials.boundary.kd = 0.5 0.5 0.5" << std::endl;
os << "scene.materials.floor.type = matte" << std::endl;
os << "scene.materials.floor.kd = 0.2 0.2 0.2" << std::endl;
// export fluid
PlyWriter::save(_engine.fluidMesh(), "fluid.ply");
os << "scene.objects.fluid.material = fluid" << std::endl;
os << "scene.objects.fluid.ply = fluid.ply" << std::endl;
// export boundaries
const auto &boundaryMeshes = _engine.sph().boundaryMeshes();
for (size_t i = 0; i < boundaryMeshes.size(); ++i) {
std::string name = tfm::format("boundary-%03d", i);
std::string filename = name + ".ply";
PlyWriter::save(boundaryMeshes[i], filename);
os << tfm::format("scene.objects.%s.material = boundary", name) << std::endl;
os << tfm::format("scene.objects.%s.ply = %s", name, filename) << std::endl;
}
// export floor
Box3f box = _engine.scene().world.bounds;
box.max.y() = box.min.y();
box.min.y() -= 0.01f * box.extents().norm();
Mesh floorMesh = Mesh::createBox(box);
PlyWriter::save(floorMesh, "floor.ply");
os << "scene.objects.floor.material = floor" << std::endl;
os << "scene.objects.floor.ply = floor.ply" << std::endl;
}
std::cout << std::endl;
std::cout << tfm::format("Rendering frame %d with SLG4 ...", _frameIndex) << std::endl;
try {
std::string slg = "/home/freak/luxmark-v3.1/slg4";
std::string arguments = "-o render.cfg";
exec_stream_t es;
// Set 1 minutes timeout
es.set_wait_timeout(exec_stream_t::s_out | exec_stream_t::s_err | exec_stream_t::s_child, 60*1000);
es.start(slg, arguments);
std::string s;
while (std::getline(es.err(), s).good()) {
std::cout << s << std::endl;
if (StringUtils::endsWith(StringUtils::trim(s), "Done.")) {
DBG("detected finish");
break;
}
}
DBG("closing slg");
if (!es.close()) {
std::cerr << "SLG4 timeout!" << std::endl;
}
DBG("closed");
} catch (const std::exception &e) {
std::cerr << "exec-stream error: " << e.what() << "\n";
}
FileUtils::changeCurrentDir(currentDir);
}