void EdgePosition::advance(Vector2f& adv, Node*& next_node) { // FIXME: This might be optimizable Vector2f p1 = edge->get_node1()->get_pos(); Vector2f p2 = edge->get_node2()->get_pos(); Vector2f edge_v = p2 - p1; Vector2f proj = adv.project(edge_v); float angle = atan2f(edge_v.y, edge_v.x) - atan2f(proj.y, proj.x); // Check if we are going forward or backward float advf; if (angle > M_PI/2 || angle < -M_PI/2) advf = -proj.length(); else advf = proj.length(); // Move forward advance(advf, next_node); // Calculate the rest Vector2f // Calculate the rest Vector2f if (advf == 0.0f) adv = Vector2f(0,0); else adv -= (proj * ((proj.length() - advf)/proj.length())); }
void Cannon::aimAtPoint(float timeElapsed, Vector2f pos, Vector2f vel) { float myPosX = myStation->getPos().x + ((cos(myStation->getRotation()*M_PI/180)*position.x)-(sin(myStation->getRotation()*M_PI/180)*position.y)); float myPosY = myStation->getPos().y + ((sin(myStation->getRotation()*M_PI/180)*position.x)+(cos(myStation->getRotation()*M_PI/180)*position.y)); Vector2f distBetween = Vector2f(pos.x-myPosX,pos.y-myPosY); float time = 826.0f/distBetween.length(); Vector2f fDist = (myStation->getVelocity()/time)*-1; Vector2f fDist2 = (vel/time); float rot = rotation; if(rot < 0) { rot+=360; } if(rot>=360) { rot -= 360; } float dir = 0; distBetween = Vector2f((pos.x+fDist.x+fDist2.x)-(myPosX),(pos.y+fDist.y+fDist2.y)-(myPosY)); distBetween /= distBetween.length(); dir = ( ( SDL_cos((rot)*M_PI/180)*180/M_PI) * (distBetween.y) - ( SDL_sin((rot)*M_PI/180)*180/M_PI) * (distBetween.x)); if(dir <-2) { rotation -=20*timeElapsed; } else if(dir > 2) { rotation += 20*timeElapsed; } if( dir <=2 && dir >= -2) { action(timeElapsed); } }
// write the raw optical flow measurements // this needs to be called externally. void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas) { // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update // The PX4Flow sensor outputs flow rates with the following axis and sign conventions: // A positive X rate is produced by a positive sensor rotation about the X axis // A positive Y rate is produced by a positive sensor rotation about the Y axis // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a // negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate flowMeaTime_ms = imuSampleTime_ms; // calculate bias errors on flow sensor gyro rates, but protect against spikes in data // reset the accumulated body delta angle and time // don't do the calculation if not enough time lapsed for a reliable body rate measurement if (delTimeOF > 0.01f) { flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f); flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f); delAngBodyOF.zero(); delTimeOF = 0.0f; } // check for takeoff if relying on optical flow and zero measurements until takeoff detected // if we haven't taken off - constrain position and velocity states if (frontend->_fusionModeGPS == 3) { detectOptFlowTakeoff(); } // calculate rotation matrices at mid sample time for flow observations stateStruct.quat.rotation_matrix(Tbn_flow); Tnb_flow = Tbn_flow.transposed(); // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data) if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) { // correct flow sensor rates for bias omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x; omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y; // write uncorrected flow rate measurements that will be used by the focal length scale factor estimator // note correction for different axis and sign conventions used by the px4flow sensor ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec) // write flow rate measurements corrected for body rates ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + omegaAcrossFlowTime.x; ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + omegaAcrossFlowTime.y; // record time last observation was received so we can detect loss of data elsewhere flowValidMeaTime_ms = imuSampleTime_ms; // estimate sample time of the measurement ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2; // Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame // This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors ofDataNew.time_ms = roundToNearest(ofDataNew.time_ms, frontend->fusionTimeStep_ms); // Prevent time delay exceeding age of oldest IMU data in the buffer ofDataNew.time_ms = max(ofDataNew.time_ms,imuDataDelayed.time_ms); // Save data to buffer StoreOF(); // Check for data at the fusion time horizon newDataFlow = RecallOF(); } }
void FlightTaskAuto::_set_heading_from_mode() { Vector2f v; // Vector that points towards desired location switch (MPC_YAW_MODE.get()) { case 0: // Heading points towards the current waypoint. v = Vector2f(_target) - Vector2f(_position); break; case 1: // Heading points towards home. if (_sub_home_position->get().valid_hpos) { v = Vector2f(&_sub_home_position->get().x) - Vector2f(_position); } break; case 2: // Heading point away from home. if (_sub_home_position->get().valid_hpos) { v = Vector2f(_position) - Vector2f(&_sub_home_position->get().x); } break; case 3: // Along trajectory. // The heading depends on the kind of setpoint generation. This needs to be implemented // in the subclasses where the velocity setpoints are generated. v.setAll(NAN); break; } if (PX4_ISFINITE(v.length())) { // We only adjust yaw if vehicle is outside of acceptance radius. Once we enter acceptance // radius, lock yaw to current yaw. // This prevents excessive yawing. if (v.length() > _target_acceptance_radius) { _compute_heading_from_2D_vector(_yaw_setpoint, v); _yaw_lock = false; } else { if (!_yaw_lock) { _yaw_setpoint = _yaw; _yaw_lock = true; } } } else { _yaw_lock = false; _yaw_setpoint = NAN; } }
// write the raw optical flow measurements // this needs to be called externally. void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas) { // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update // The PX4Flow sensor outputs flow rates with the following axis and sign conventions: // A positive X rate is produced by a positive sensor rotation about the X axis // A positive Y rate is produced by a positive sensor rotation about the Y axis // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a // negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate flowMeaTime_ms = imuSampleTime_ms; // calculate bias errors on flow sensor gyro rates, but protect against spikes in data // reset the accumulated body delta angle and time // don't do the calculation if not enough time lapsed for a reliable body rate measurement if (delTimeOF > 0.01f) { flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f); flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f); delAngBodyOF.zero(); delTimeOF = 0.0f; } // by definition if this function is called, then flow measurements have been provided so we // need to run the optical flow takeoff detection detectOptFlowTakeoff(); // calculate rotation matrices at mid sample time for flow observations stateStruct.quat.rotation_matrix(Tbn_flow); // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data) if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) { // correct flow sensor rates for bias omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x; omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y; // write uncorrected flow rate measurements that will be used by the focal length scale factor estimator // note correction for different axis and sign conventions used by the px4flow sensor ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec) // write flow rate measurements corrected for body rates ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + omegaAcrossFlowTime.x; ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + omegaAcrossFlowTime.y; // record time last observation was received so we can detect loss of data elsewhere flowValidMeaTime_ms = imuSampleTime_ms; // estimate sample time of the measurement ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2; // Correct for the average intersampling delay due to the filter updaterate ofDataNew.time_ms -= localFilterTimeStep_ms/2; // Prevent time delay exceeding age of oldest IMU data in the buffer ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms); // Save data to buffer storedOF.push(ofDataNew); // Check for data at the fusion time horizon flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms); } }
bool FlightTaskAuto::_compute_heading_from_2D_vector(float &heading, Vector2f v) { if (PX4_ISFINITE(v.length()) && v.length() > SIGMA_NORM) { v.normalize(); // To find yaw: take dot product of x = (1,0) and v // and multiply by the sign given of cross product of x and v. // Dot product: (x(0)*v(0)+(x(1)*v(1)) = v(0) // Cross product: x(0)*v(1) - v(0)*x(1) = v(1) heading = math::sign(v(1)) * wrap_pi(acosf(v(0))); return true; } // heading unknown and therefore do not change heading return false; }
// calculate vehicle stopping point using current location, velocity and maximum acceleration bool AR_WPNav::get_stopping_location(Location& stopping_loc) { Location current_loc; if (!AP::ahrs().get_position(current_loc)) { return false; } // get current velocity vector and speed const Vector2f velocity = AP::ahrs().groundspeed_vector(); const float speed = velocity.length(); // avoid divide by zero if (!is_positive(speed)) { stopping_loc = current_loc; return true; } // get stopping distance in meters const float stopping_dist = _atc.get_stopping_distance(speed); // calculate stopping position from current location in meters const Vector2f stopping_offset = velocity.normalized() * stopping_dist; stopping_loc = current_loc; stopping_loc.offset(stopping_offset.x, stopping_offset.y); return true; }
// produce a yaw error value. The returned value is proportional // to sin() of the current heading error in earth frame float AP_AHRS_DCM::yaw_error_compass(void) { const Vector3f &mag = _compass->get_field(); // get the mag vector in the earth frame Vector2f rb = _dcm_matrix.mulXY(mag); if (rb.length() < FLT_EPSILON) { return 0.0f; } rb.normalize(); if (rb.is_inf()) { // not a valid vector return 0.0f; } // update vector holding earths magnetic field (if required) if( !is_equal(_last_declination,_compass->get_declination()) ) { _last_declination = _compass->get_declination(); _mag_earth.x = cosf(_last_declination); _mag_earth.y = sinf(_last_declination); } // calculate the error term in earth frame // calculate the Z component of the cross product of rb and _mag_earth return rb % _mag_earth; }
bool Bullet::outOfBounds() { Vector2f camv = Vector2f(); camv.x = cam.getX(); camv.y = cam.getY(); Vector2f diff = getShortestDiffVectorBullet(camv, position); return diff.length() > CHUNK_SIZE * 1.4; }
void RigidBody::addForce(Vector2f _force, Vector2f point) { Vector2f dir = (point - position); forces += _force; aTorque += dir.length() * _force.scalarProjectAt(dir.rightNormal()); }
void Cannon::crewControl(float timeElapsed,vector<Entity*> *nightmares) { float dist = 2000*((Unit*)controller)->getHatStrength(); float num = -1; for(int j = 0; j < nightmares->size(); j++) { if(((Station*)nightmares->at(j))!=myStation) { if(((Station*)nightmares->at(j))->getAssignedUnits()->size()>0&&((Station*)nightmares->at(j))->getAssignedCrew()->size()==0) { if(num==-1) { Vector2f ePos = ((Station*)nightmares->at(j))->getPos(); float myPosX = myStation->getPos().x + ((cos(myStation->getRotation()*M_PI/180)*position.x)-(sin(myStation->getRotation()*M_PI/180)*position.y)); float myPosY = myStation->getPos().y + ((sin(myStation->getRotation()*M_PI/180)*position.x)+(cos(myStation->getRotation()*M_PI/180)*position.y)); Vector2f distBetween = Vector2f(ePos.x-myPosX,ePos.y-myPosY); if(distBetween.length()<dist) { num=j; dist=distBetween.length(); } } else { Vector2f ePos = ((Station*)nightmares->at(j))->getPos(); float myPosX = myStation->getPos().x + ((cos(myStation->getRotation()*M_PI/180)*position.x)-(sin(myStation->getRotation()*M_PI/180)*position.y)); float myPosY = myStation->getPos().y + ((sin(myStation->getRotation()*M_PI/180)*position.x)+(cos(myStation->getRotation()*M_PI/180)*position.y)); Vector2f distBetween = Vector2f(ePos.x-myPosX,ePos.y-myPosY); if(distBetween.length()<dist) { num=j; dist=distBetween.length(); } } } } } if(num != -1) { Vector2f ePos = ((Station*)nightmares->at(num))->getPos(); Vector2f vel = ((Station*)nightmares->at(num))->getVelocity(); aimAtPoint(timeElapsed,ePos,vel); } }
void CPlayerDot::update(float dt, vector<CPlanetDot*> * planetList) { if(isGravityOn) { for(vector<CPlanetDot*>::iterator it = planetList->begin(); it != planetList->end(); it++) { /** (*it) is the planet dot */ CPlanetDot * pPlanet = (CPlanetDot*)(*it); /** distance = planet.position - myposition */ Vector2f distanceVector = pPlanet->mVecPosition - this->mVecPosition + pPlanet->radius + this->radius; float distance = distanceVector.length(); /** Lets say the planet is 500 units away from us or less */ //if(distance < 5000) { /** force = distanceVector * planetRadius / (distance squared) */ double G = 6.67e-11; //Vector2f forceVector = distanceVector * pPlanet->radius / (distance * distance); float force = (G*((pPlanet->mass * this->mass)/(distance*distance))); //distanceVector.normalize(); Vector2f forceVector = distanceVector * force; /** Make the vector of length 1 */ //forceVector.normalize(); mVecAcceleration += forceVector; } } //if(mVecAcceleration.length() > 0.0001) //mVecAcceleration.normalize(); } else { } /** try not to delete the stuff below */ /** add acceleration impulse to velocity */ mVecVelocity += mVecAcceleration; /** if velocity > our maximum velocity, then normalize the velocity vector making it length /** 1, and then multiply it by our max speed making it equal to the max speed */ if(mVecVelocity.length() > maxSpeed) { mVecVelocity.normalize(); mVecVelocity *= maxSpeed; } /** add velocity to position to update position */ mVecPosition += mVecVelocity*dt; /** reset acceleration impulse */ mVecAcceleration.x = 0; mVecAcceleration.y = 0; }
void AP_OSD_Screen::draw_speed_vector(uint8_t x, uint8_t y,Vector2f v, int32_t yaw) { float v_length = v.length(); char arrow = SYM_ARROW_START; if (v_length > 1.0f) { int32_t angle = wrap_360_cd(DEGX100 * atan2f(v.y, v.x) - yaw); int32_t interval = 36000 / SYM_ARROW_COUNT; arrow = SYM_ARROW_START + ((angle + interval / 2) / interval) % SYM_ARROW_COUNT; } backend->write(x, y, false, "%c%3d%c", arrow, (int)u_scale(SPEED, v_length), u_icon(SPEED)); }
void AP_OSD_Screen::draw_eff(uint8_t x, uint8_t y) { AP_BattMonitor &battery = AP::battery(); AP_AHRS &ahrs = AP::ahrs(); Vector2f v = ahrs.groundspeed_vector(); float speed = u_scale(SPEED,v.length()); if (speed > 2.0){ backend->write(x, y, false, "%c%3d%c", SYM_EFF,int(1000*battery.current_amps()/speed),SYM_MAH); } else { backend->write(x, y, false, "%c---%c", SYM_EFF,SYM_MAH); } }
/* * Adjusts the desired velocity for the circular fence. */ void AC_Avoid::adjust_velocity_circle(const float kP, const float accel_cmss, Vector2f &desired_vel) { // exit if circular fence is not enabled if ((_fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) == 0) { return; } // exit if the circular fence has already been breached if ((_fence.get_breaches() & AC_FENCE_TYPE_CIRCLE) != 0) { return; } // get position as a 2D offset in cm from ahrs home const Vector2f position_xy = get_position(); float speed = desired_vel.length(); // get the fence radius in cm const float fence_radius = _fence.get_radius() * 100.0f; // get the margin to the fence in cm const float margin = get_margin(); if (!is_zero(speed) && position_xy.length() <= fence_radius) { // Currently inside circular fence Vector2f stopping_point = position_xy + desired_vel*(get_stopping_distance(kP, accel_cmss, speed)/speed); float stopping_point_length = stopping_point.length(); if (stopping_point_length > fence_radius - margin) { // Unsafe desired velocity - will not be able to stop before fence breach // Project stopping point radially onto fence boundary // Adjusted velocity will point towards this projected point at a safe speed Vector2f target = stopping_point * ((fence_radius - margin) / stopping_point_length); Vector2f target_direction = target - position_xy; float distance_to_target = target_direction.length(); float max_speed = get_max_speed(kP, accel_cmss, distance_to_target); desired_vel = target_direction * (MIN(speed,max_speed) / distance_to_target); } } }
bool AC_Fence::check_fence_circle() { if (!(_enabled_fences & AC_FENCE_TYPE_CIRCLE)) { // not enabled; no breach return false; } Vector2f home; if (_ahrs.get_relative_position_NE_home(home)) { // we (may) remain breached if we can't update home _home_distance = home.length(); } // check if we are outside the fence if (_home_distance >= _circle_radius) { // record distance outside the fence _circle_breach_distance = _home_distance - _circle_radius; // check for a new breach or a breach of the backup fence if (!(_breached_fences & AC_FENCE_TYPE_CIRCLE) || (!is_zero(_circle_radius_backup) && _home_distance >= _circle_radius_backup)) { // new breach // create a backup fence 20m further out record_breach(AC_FENCE_TYPE_CIRCLE); _circle_radius_backup = _home_distance + AC_FENCE_CIRCLE_RADIUS_BACKUP_DISTANCE; return true; } return false; } // not currently breached // clear circle breach if present if (_breached_fences & AC_FENCE_TYPE_CIRCLE) { clear_breach(AC_FENCE_TYPE_CIRCLE); _circle_radius_backup = 0.0f; _circle_breach_distance = 0.0f; } return false; }
void ChannelBase<Vector2f>::set(Vector2f newValue) { if (isDigital) { newValue.x = newValue.x >= actPoint ? 1.0f : 0.0f; newValue.y = newValue.y >= actPoint ? 1.0f : 0.0f; } else { float length = newValue.length(); if (deadZone > 0) { if (length <= deadZone) { newValue = Vector2f::ZERO; } else { newValue *= (length - deadZone) / (length * (1 - deadZone)); } } if (hasBound) { newValue.x = newValue.x >= bound ? 1.0f : newValue.x; newValue.y = newValue.y >= bound ? 1.0f : newValue.y; newValue.x = newValue.x <= -bound ? -1.0f : newValue.x; newValue.y = newValue.y <= -bound ? -1.0f : newValue.y; } newValue *= sensitivity; } if (alwaysNotifyListener or !newValue.fuzzyEqual(value, 0.0001)) { value = newValue; if (not listeners.empty()) { notifyListeners(); } } else { return; } if (false) { value = Vector2f(0.0f); } }
// calculate vehicle stopping point using current location, velocity and maximum acceleration void Mode::calc_stopping_location(Location& stopping_loc) { // default stopping location stopping_loc = rover.current_loc; // get current velocity vector and speed const Vector2f velocity = ahrs.groundspeed_vector(); const float speed = velocity.length(); // avoid divide by zero if (!is_positive(speed)) { stopping_loc = rover.current_loc; return; } // get stopping distance in meters const float stopping_dist = attitude_control.get_stopping_distance(speed); // calculate stopping position from current location in meters const Vector2f stopping_offset = velocity.normalized() * stopping_dist; location_offset(stopping_loc, stopping_offset.x, stopping_offset.y); }
void ParticleState::updateParticle( Particle &particle ) { Vector2f vel = particle.m_state.m_velocity; float speed = vel.length(); particle.m_position += vel; float alpha = min(1.0f, min(particle.m_percentLife * 2, speed * 1.0f)); alpha *= alpha; particle.m_color.a = alpha; if(particle.m_state.m_type == kBullet) { particle.m_scale.x = particle.m_state.m_lengthMultiplier * min(min(1.0f, 0.1f * speed + 0.1f), alpha); } else { particle.m_scale.x = particle.m_state.m_lengthMultiplier * min(min(1.0f, 0.2f * speed + 0.1f), alpha); } particle.m_orientation = Extensions::toAngle(vel); Vector2f pos = particle.m_position; int width = (int) constants::WINDOW_WIDTH; int height = (int) constants::WINDOW_HEIGHT; if(pos.x < 0) { vel.x = (float) fabs(vel.x); } else if(pos.x > width) { vel.x = (float) -fabs(vel.x); } if(pos.y < 0) { vel.y = (float) fabs(vel.y); } else if(pos.x > width) { vel.y = (float) -fabs(vel.y); } if(particle.m_state.m_type != kIgnoreGravity) { for(std::list<BlackHole*>::iterator j = EntityManager::getInstance()->m_blackHoles.begin(); j != EntityManager::getInstance()->m_blackHoles.end(); j++) { Vector2f pos = (*j)->getPosition() - pos; float distance = pos.length(); Vector2f n = pos / distance; vel += 10000.0f * n / (distance * distance + 10000.0f); if(distance < 400) { vel += 45.0f * Vector2f(n.y, -n.x) / (distance + 100.0f); } } } if(fabs(vel.x) + fabs(vel.y) < 0.00000000001f) { vel = Vector2f(0, 0); } else if(particle.m_state.m_type == kEnemy) { vel *= 0.94f; } else { vel *= 0.96f + (float) fmod(fabs(pos.x), 0.94f); } particle.m_state.m_velocity = vel; }
int ModelViewController::handle(int event) { Vector2f Clickpoint; // NEW: Current Mouse Point static float Click_y; static float old_zoom; Clickpoint.x = (GLfloat)Fl::event_x();; Clickpoint.y = (GLfloat)Fl::event_y();; switch(event) { case FL_PUSH: //mouse down event position in Fl::event_x() and Fl::event_y() { switch(Fl::event_button()) { case FL_LEFT_MOUSE: MousePt.T[0] = (GLfloat)Clickpoint.x; MousePt.T[1] = (GLfloat)Clickpoint.y; ArcBall->click(&MousePt); // Update Start Vector And Prepare For Dragging break; case FL_MIDDLE_MOUSE: downPoint = Clickpoint; /* Matrix3fSetIdentity(&LastRot); // Reset Rotation Matrix3fSetIdentity(&ThisRot); // Reset Rotation Matrix4fSetRotationFromMatrix3f(&Transform, &ThisRot); // Reset Rotation */ break; case FL_RIGHT_MOUSE: Click_y = Clickpoint.y; old_zoom = zoom; break; } LastRot = ThisRot; // Set Last Static Rotation To Last Dynamic One redraw(); return 1; } case FL_DRAG: //mouse moved while down event ... switch(Fl::event_button()) { case FL_LEFT_MOUSE: Quat4fT ThisQuat; MousePt.T[0] = Clickpoint.x; MousePt.T[1] = Clickpoint.y; ArcBall->drag(&MousePt, &ThisQuat); // Update End Vector And Get Rotation As Quaternion Matrix3fSetRotationFromQuat4f(&ThisRot, &ThisQuat); // Convert Quaternion Into Matrix3fT Matrix3fMulMatrix3f(&ThisRot, &LastRot); // Accumulate Last Rotation Into This One Matrix4fSetRotationFromMatrix3f(&Transform, &ThisRot); // Set Our Final Transform's Rotation From This One redraw(); break; case FL_MIDDLE_MOUSE: { Vector2f dragp = Clickpoint; Vector2f delta = downPoint-dragp; Matrix4f matrix; memcpy(&matrix.m00, &Transform.M[0], sizeof(Matrix4f)); Vector3f X(delta.x,0,0); X = matrix * X; Vector3f Y(0,-delta.y,0); Y = matrix * Y; ProcessControl.Center += X*delta.length()*0.01f; ProcessControl.Center += Y*delta.length()*0.01f; redraw(); downPoint=Clickpoint; } break; case FL_RIGHT_MOUSE: float y = Click_y - Clickpoint.y; zoom = old_zoom + y*0.1; redraw(); break; } return 1; case FL_RELEASE: //mouse up event ... MousePt.T[0] = (GLfloat)Fl::event_x(); MousePt.T[1] = (GLfloat)Fl::event_y(); redraw(); return 1; case FL_MOUSEWHEEL: { //mouse scroll event int mwscrolled = Fl::event_dy(); zoom += mwscrolled*1; redraw(); return 1; } case FL_FOCUS : case FL_UNFOCUS : // Return 1 if you want keyboard events, 0 otherwise return 0; case FL_KEYBOARD: //keypress, key is in Fl::event_key(), ascii in Fl::event_text() .. Return 1 if you understand/use the keyboard event, 0 otherwise... return 1; case FL_SHORTCUT: // shortcut, key is in Fl::event_key(), ascii in Fl::event_text() ... Return 1 if you understand/use the shortcut event, 0 otherwise... return 1; case FL_CLOSE: if (fl_choice("Save settings ?", "Exit", "Save then exit", NULL)) { // int a=0; // ProcessControl.SaveXML(); } break; default: break; } // pass other events to the base class... return Fl_Gl_Window::handle(event); }
inline Vector2f normalize(const Vector2f &v) { return v / v.length(); }
float scoreForWidget(GuiWidget const &widget, ui::Direction dir) const { if (!widget.canBeFocused() || &widget == thisPublic) { return -1; } Rectanglef const viewRect = self().root().viewRule().rect(); Rectanglef const selfRect = self().hitRule().rect(); Rectanglef const otherRect = widget.hitRule().rect(); Vector2f const otherMiddle = (dir == ui::Up? otherRect.midBottom() : dir == ui::Down? otherRect.midTop() : dir == ui::Left? otherRect.midRight() : otherRect.midLeft() ); //otherRect.middle(); if (!viewRect.contains(otherMiddle)) { return -1; } bool const axisOverlap = (isHorizontal(dir) && !selfRect.vertical() .intersection(otherRect.vertical()) .isEmpty()) || (isVertical(dir) && !selfRect.horizontal().intersection(otherRect.horizontal()).isEmpty()); // Check for contacting edges. float edgeDistance = 0; // valid only if axisOverlap if (axisOverlap) { switch (dir) { case ui::Left: edgeDistance = selfRect.left() - otherRect.right(); break; case ui::Up: edgeDistance = selfRect.top() - otherRect.bottom(); break; case ui::Right: edgeDistance = otherRect.left() - selfRect.right(); break; default: edgeDistance = otherRect.top() - selfRect.bottom(); break; } // Very close edges are considered contacting. if (edgeDistance >= 0 && edgeDistance < toDevicePixels(5)) { return edgeDistance; } } Vector2f const middle = (dir == ui::Up? selfRect.midTop() : dir == ui::Down? selfRect.midBottom() : dir == ui::Left? selfRect.midLeft() : selfRect.midRight() ); Vector2f const delta = otherMiddle - middle; Vector2f const dirVector = directionVector(dir); auto dotProd = delta.normalize().dot(dirVector); if (dotProd <= 0) { // On the wrong side. return -1; } float distance = delta.length(); if (axisOverlap) { dotProd = 1.0; if (edgeDistance > 0) { distance = de::min(distance, edgeDistance); } } float favorability = 1; if (widget.parentWidget() == self().parentWidget()) { favorability = .1f; // Siblings are much preferred. } else if (self().hasAncestor(widget) || widget.hasAncestor(self())) { favorability = .2f; // Ancestry is also good. } // Prefer widgets that are nearby, particularly in the specified direction. return distance * (.5f + acos(dotProd)) * favorability; }
void testNormalize() { v5.normalize(); CPPUNIT_ASSERT(std::abs(v5.length() - 1.0) < EPSILON); }
// update L1 control for waypoint navigation // this function costs about 3.5 milliseconds on a AVR2560 void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP) { struct Location _current_loc; float Nu; float xtrackVel; float ltrackVel; // Calculate L1 gain required for specified damping float K_L1 = 4.0f * _L1_damping * _L1_damping; // Get current position and velocity _ahrs.get_position(_current_loc); Vector2f _groundspeed_vector = _ahrs.groundspeed_vector(); // update _target_bearing_cd _target_bearing_cd = get_bearing_cd(_current_loc, next_WP); //Calculate groundspeed float groundSpeed = _groundspeed_vector.length(); if (groundSpeed < 0.1f) { // use a small ground speed vector in the right direction, // allowing us to use the compass heading at zero GPS velocity groundSpeed = 0.1f; _groundspeed_vector = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw)) * groundSpeed; } // Calculate time varying control parameters // Calculate the L1 length required for specified period // 0.3183099 = 1/1/pipi _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed; // Calculate the NE position of WP B relative to WP A Vector2f AB = location_diff(prev_WP, next_WP); // Check for AB zero length and track directly to the destination // if too small if (AB.length() < 1.0e-6f) { AB = location_diff(_current_loc, next_WP); } AB.normalize(); // Calculate the NE position of the aircraft relative to WP A Vector2f A_air = location_diff(prev_WP, _current_loc); // calculate distance to target track, for reporting _crosstrack_error = AB % A_air; //Determine if the aircraft is behind a +-135 degree degree arc centred on WP A //and further than L1 distance from WP A. Then use WP A as the L1 reference point //Otherwise do normal L1 guidance float WP_A_dist = A_air.length(); float alongTrackDist = A_air * AB; if (WP_A_dist > _L1_dist && alongTrackDist/max(WP_A_dist, 1.0f) < -0.7071f) { //Calc Nu to fly To WP A Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line Nu = atan2f(xtrackVel,ltrackVel); _prevent_indecision(Nu); _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point } else { //Calc Nu to fly along AB line //Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints) xtrackVel = _groundspeed_vector % AB; // Velocity cross track ltrackVel = _groundspeed_vector * AB; // Velocity along track float Nu2 = atan2f(xtrackVel,ltrackVel); //Calculate Nu1 angle (Angle to L1 reference point) float xtrackErr = A_air % AB; float sine_Nu1 = xtrackErr/max(_L1_dist, 0.1f); //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f); float Nu1 = asinf(sine_Nu1); Nu = Nu1 + Nu2; _nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point } _last_Nu = Nu; //Limit Nu to +-pi Nu = constrain_float(Nu, -1.5708f, +1.5708f); _latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu); // Waypoint capture status is always false during waypoint following _WPcircle = false; _bearing_error = Nu; // bearing error angle (radians), +ve to left of track }
// update L1 control for waypoint navigation void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP) { struct Location _current_loc; float Nu; float xtrackVel; float ltrackVel; uint32_t now = AP_HAL::micros(); float dt = (now - _last_update_waypoint_us) * 1.0e-6f; if (dt > 0.1) { dt = 0.1; } _last_update_waypoint_us = now; // Calculate L1 gain required for specified damping float K_L1 = 4.0f * _L1_damping * _L1_damping; // Get current position and velocity _ahrs.get_position(_current_loc); Vector2f _groundspeed_vector = _ahrs.groundspeed_vector(); // update _target_bearing_cd _target_bearing_cd = get_bearing_cd(_current_loc, next_WP); //Calculate groundspeed float groundSpeed = _groundspeed_vector.length(); if (groundSpeed < 0.1f) { // use a small ground speed vector in the right direction, // allowing us to use the compass heading at zero GPS velocity groundSpeed = 0.1f; _groundspeed_vector = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw)) * groundSpeed; } // Calculate time varying control parameters // Calculate the L1 length required for specified period // 0.3183099 = 1/1/pipi _L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed; // Calculate the NE position of WP B relative to WP A Vector2f AB = location_diff(prev_WP, next_WP); // Check for AB zero length and track directly to the destination // if too small if (AB.length() < 1.0e-6f) { AB = location_diff(_current_loc, next_WP); if (AB.length() < 1.0e-6f) { AB = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw)); } } AB.normalize(); // Calculate the NE position of the aircraft relative to WP A Vector2f A_air = location_diff(prev_WP, _current_loc); // calculate distance to target track, for reporting _crosstrack_error = A_air % AB; //Determine if the aircraft is behind a +-135 degree degree arc centred on WP A //and further than L1 distance from WP A. Then use WP A as the L1 reference point //Otherwise do normal L1 guidance float WP_A_dist = A_air.length(); float alongTrackDist = A_air * AB; if (WP_A_dist > _L1_dist && alongTrackDist/MAX(WP_A_dist, 1.0f) < -0.7071f) { //Calc Nu to fly To WP A Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line Nu = atan2f(xtrackVel,ltrackVel); _nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point } else { //Calc Nu to fly along AB line //Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints) xtrackVel = _groundspeed_vector % AB; // Velocity cross track ltrackVel = _groundspeed_vector * AB; // Velocity along track float Nu2 = atan2f(xtrackVel,ltrackVel); //Calculate Nu1 angle (Angle to L1 reference point) float sine_Nu1 = _crosstrack_error/MAX(_L1_dist, 0.1f); //Limit sine of Nu1 to provide a controlled track capture angle of 45 deg sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f); float Nu1 = asinf(sine_Nu1); // compute integral error component to converge to a crosstrack of zero when traveling // straight but reset it when disabled or if it changes. That allows for much easier // tuning by having it re-converge each time it changes. if (_L1_xtrack_i_gain <= 0 || !is_equal(_L1_xtrack_i_gain, _L1_xtrack_i_gain_prev)) { _L1_xtrack_i = 0; _L1_xtrack_i_gain_prev = _L1_xtrack_i_gain; } else if (fabsf(Nu1) < radians(5)) { _L1_xtrack_i += Nu1 * _L1_xtrack_i_gain * dt; // an AHRS_TRIM_X=0.1 will drift to about 0.08 so 0.1 is a good worst-case to clip at _L1_xtrack_i = constrain_float(_L1_xtrack_i, -0.1f, 0.1f); } // to converge to zero we must push Nu1 harder Nu1 += _L1_xtrack_i; Nu = Nu1 + Nu2; _nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point } _prevent_indecision(Nu); _last_Nu = Nu; //Limit Nu to +-pi Nu = constrain_float(Nu, -1.5708f, +1.5708f); _latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu); // Waypoint capture status is always false during waypoint following _WPcircle = false; _bearing_error = Nu; // bearing error angle (radians), +ve to left of track }