bool CCameraTracking::IdentifyObstacle(const Vec3 &vCamDir, const CPlayer &hero) { //check player direction Vec3 newDir = -hero.GetEntity()->GetForwardDir(); newDir.z += vCamDir.z; newDir.normalize(); //compute rotation speed const float fHeroSpeedModifier = clamp(hero.GetActorStats()->speedFlat / 4.0f, 0.3f, 1.0f); const float fNewSpeed = g_pGameCVars->cl_cam_tracking_rotation_speed * m_fFrameTime * fHeroSpeedModifier; m_fSpeed = InterpolateTo(m_fSpeed, fNewSpeed, (fNewSpeed>m_fSpeed)?0.1f:0.3f); //m_fSpeed = (g_fInterpolationRate * m_fSpeed + speed) * g_fInterpolationWeight; //get ray data from camera ray tests ray_hit *pRayHit = m_pCamRayScan->GetHit(eRAY_TOP_RIGHT); if(!pRayHit || pRayHit->dist == 0.0f) pRayHit = m_pCamRayScan->GetHit(eRAY_BOTTOM_RIGHT); bool bHitsRight = (pRayHit && pRayHit->dist > 0.0f); Vec3 dirRight = (pRayHit)?-(m_pCamRayScan->GetRayDir(eRAY_TOP_RIGHT)):Vec3(ZERO); //ray data left side pRayHit = m_pCamRayScan->GetHit(eRAY_TOP_LEFT); if(!pRayHit || pRayHit->dist == 0.0f) pRayHit = m_pCamRayScan->GetHit(eRAY_BOTTOM_LEFT); bool bHitsLeft = (pRayHit && pRayHit->dist > 0.0f); Vec3 dirLeft = (pRayHit)?-(m_pCamRayScan->GetRayDir(eRAY_TOP_LEFT)):Vec3(ZERO); //left or right if(bHitsRight ^ bHitsLeft) { //find rotation direction if(!bHitsRight && !bHitsLeft) { if(m_eLastDirYaw == eTD_LEFT) //continue last direction newDir = dirLeft; else newDir = dirRight; } else if(!bHitsRight) { m_eLastDirYaw = eTD_RIGHT; newDir = dirRight; } else { m_eLastDirYaw = eTD_LEFT; newDir = dirLeft; } //compute yaw/pitch for target position float newYaw = 0.0f; float newPitch = 0.0f; float newDist = 0.0f; CartesianToSpherical(newDir * m_curCamOrientation.m_fDist, newYaw, newPitch, newDist); newYaw += gf_PI; //now interpolate to target //compute delta yaw m_fYawDelta = (newYaw - m_curCamOrientation.m_fYaw) * m_fSpeed; if(m_eLastDirYaw == eTD_RIGHT && m_fYawDelta < 0.0f || m_eLastDirYaw == eTD_LEFT && m_fYawDelta > 0.0f) m_fYawDelta *= -1.0f; } //compute top/bottom rotation //ray data top side pRayHit = m_pCamRayScan->GetHit(eRAY_TOP_CENTER); bool bHitsTop = (pRayHit && pRayHit->dist > 0.0f)?true:false; Vec3 vDirTop = (pRayHit)?-(m_pCamRayScan->GetRayDir(eRAY_TOP_CENTER)):Vec3(ZERO); //ray data bottom side pRayHit = m_pCamRayScan->GetHit(eRAY_BOTTOM_CENTER); bool bHitsBottom = (pRayHit && pRayHit->dist > 0.0f)?true:false; Vec3 vDirBottom = (pRayHit)?-(m_pCamRayScan->GetRayDir(eRAY_BOTTOM_CENTER)):Vec3(ZERO); //top or bottom (if not left or right) if(g_pGameCVars->cl_cam_tracking_allow_pitch && (bHitsTop ^ bHitsBottom) && !(bHitsRight ^ bHitsLeft)) { //find rotation direction if(!bHitsTop && !bHitsBottom) { if(m_eLastDirPitch == eTD_TOP) //continue last direction newDir = vDirTop; else newDir = vDirBottom; } else if(!bHitsBottom) { m_eLastDirPitch = eTD_BOTTOM; newDir = vDirBottom; } else { m_eLastDirPitch = eTD_TOP; newDir = vDirTop; } //compute yaw/pitch for target position float newYaw = 0.0f; float newPitch = 0.0f; float newDist = 0.0f; //newdist (raydist) will be ignored CartesianToSpherical(newDir, newYaw, newPitch, newDist); //compute delta pitch m_fPitchDelta = (newPitch - m_curCamOrientation.m_fPitch) * m_fSpeed * 10.0f; } //if all rays hit - don't bother! //this is a termination condition when the camera is pulled through geometry if(bHitsLeft & bHitsRight & bHitsBottom & bHitsTop) { if(m_bViewCovered) { //if obstacle behind player //if(g_rHit.dist > 0.0f) //this is a strange fix, but it's working better and is much cheaper than a raycast if(fabsf(m_fYawDelta) < 0.01f && fabsf(m_fPitchDelta) > 0.001f) return false; } m_bViewCovered = true; } else m_bViewCovered = false; return true; }
void btKart::updateFriction(btScalar timeStep) { //calculate the impulse, so that the wheels don't move sidewards for (int i=0;i<getNumWheels();i++) { m_sideImpulse[i] = btScalar(0.); btWheelInfo& wheelInfo = m_wheelInfo[i]; btRigidBody* groundObject = (btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; if(!groundObject) continue; const btTransform& wheelTrans = getWheelTransformWS( i ); btMatrix3x3 wheelBasis0 = wheelTrans.getBasis(); m_axle[i] = btVector3(wheelBasis0[0][m_indexRightAxis], wheelBasis0[1][m_indexRightAxis], wheelBasis0[2][m_indexRightAxis] ); const btVector3& surfNormalWS = wheelInfo.m_raycastInfo.m_contactNormalWS; btScalar proj = m_axle[i].dot(surfNormalWS); m_axle[i] -= surfNormalWS * proj; m_axle[i] = m_axle[i].normalize(); m_forwardWS[i] = surfNormalWS.cross(m_axle[i]); m_forwardWS[i].normalize(); resolveSingleBilateral(*m_chassisBody, wheelInfo.m_raycastInfo.m_contactPointWS, *groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, btScalar(0.), m_axle[i],m_sideImpulse[i], timeStep); btScalar sideFrictionStiffness2 = btScalar(1.0); m_sideImpulse[i] *= sideFrictionStiffness2; } btScalar sideFactor = btScalar(1.); btScalar fwdFactor = 0.5; bool sliding = false; for (int wheel=0; wheel<getNumWheels(); wheel++) { btWheelInfo& wheelInfo = m_wheelInfo[wheel]; m_wheelInfo[wheel].m_skidInfo = btScalar(1.); m_forwardImpulse[wheel] = btScalar(0.); btRigidBody* groundObject = (btRigidBody*) wheelInfo.m_raycastInfo.m_groundObject; if(!groundObject) continue; if(m_zipper_active && m_zipper_velocity > 0) { if (wheel==2 || wheel==3) { // The zipper velocity is the speed that should be // reached. So compute the impulse to accelerate the // kart up to that speed: m_forwardImpulse[wheel] = 0.5f*(m_zipper_velocity - getRigidBody()->getLinearVelocity().length()) / m_chassisBody->getInvMass(); } } else { btScalar rollingFriction = 0.f; if (wheelInfo.m_engineForce != 0.f) { rollingFriction = wheelInfo.m_engineForce* timeStep; } else { btScalar defaultRollingFrictionImpulse = 0.f; btScalar maxImpulse = wheelInfo.m_brake ? wheelInfo.m_brake : defaultRollingFrictionImpulse; btWheelContactPoint contactPt(m_chassisBody, groundObject, wheelInfo.m_raycastInfo.m_contactPointWS, m_forwardWS[wheel],maxImpulse); rollingFriction = calcRollingFriction(contactPt); // This is a work around for the problem that a kart shakes // if it is braking: we get a minor impulse forward, which // bullet then tries to offset by applying a backward // impulse - which is a bit too big, causing a impulse // backwards, ... till the kart is shaking backwards and // forwards. By onlyu applying half of the impulse in cae // of low friction this goes away. if(wheelInfo.m_brake && fabsf(rollingFriction)<10) rollingFriction*=0.5f; } m_forwardImpulse[wheel] = rollingFriction; } if(m_time_additional_impulse>0) { sliding = true; m_wheelInfo[wheel].m_skidInfo = 0.0f; } else { btScalar maximp = wheelInfo.m_wheelsSuspensionForce * timeStep * wheelInfo.m_frictionSlip; btScalar maximpSide = maximp; btScalar maximpSquared = maximp * maximpSide; btScalar x = (m_forwardImpulse[wheel] ) * fwdFactor; btScalar y = (m_sideImpulse[wheel] ) * sideFactor; btScalar impulseSquared = (x*x + y*y); if (impulseSquared > maximpSquared) { sliding = true; btScalar factor = maximp / btSqrt(impulseSquared); m_wheelInfo[wheel].m_skidInfo *= factor; } // if impulseSquared > maximpSquared } // else (!m_timed_impulse } // for (int wheel=0; wheel<getNumWheels(); wheel++) m_zipper_active = false; m_zipper_velocity = 0; // The kart just stopped skidding. Adjust the velocity so that // it points in the right direction. // FIXME: this is not good enough, we need some smooth interpolation here. if(m_is_skidding && m_skid_angular_velocity == 0) { btVector3 v = m_chassisBody->getLinearVelocity(); v.setZ(sqrt(v.getX()*v.getX()+v.getZ()*v.getZ())); v.setX(0); btVector3 v_new = m_chassisBody->getWorldTransform().getBasis()*v; m_chassisBody->setLinearVelocity(v_new); m_is_skidding = false; } if(m_skid_angular_velocity!=0) { m_is_skidding = true; // Skidding is implemented by not having any forward impulse, // but only add a side impulse for(unsigned int i=0; i<4; i++) { m_forwardImpulse[i] = 0; m_sideImpulse[i] = 0; } btVector3 av = m_chassisBody->getAngularVelocity(); av.setY(m_skid_angular_velocity); m_chassisBody->setAngularVelocity(av); } else if (sliding && (m_allow_sliding || m_time_additional_impulse>0) ) { for (int wheel = 0; wheel < getNumWheels(); wheel++) { if (m_sideImpulse[wheel] != btScalar(0.) && m_wheelInfo[wheel].m_skidInfo< btScalar(1.) ) { m_forwardImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; m_sideImpulse[wheel] *= m_wheelInfo[wheel].m_skidInfo; } } // for wheel <getNumWheels } // if sliding // Apply the impulses // ------------------ for (int wheel = 0;wheel<getNumWheels() ; wheel++) { btWheelInfo& wheelInfo = m_wheelInfo[wheel]; btVector3 rel_pos = wheelInfo.m_raycastInfo.m_contactPointWS - m_chassisBody->getCenterOfMassPosition(); if (m_forwardImpulse[wheel] != btScalar(0.)) { m_chassisBody->applyImpulse( m_forwardWS[wheel]*(m_forwardImpulse[wheel]), #define COMPATIBLE_0_7_3 #ifdef COMPATIBLE_0_7_3 // This was apparently done to help hexley btVector3(0,0,0)); #else rel_pos); #endif } if (m_sideImpulse[wheel] != btScalar(0.)) { btRigidBody* groundObject = (btRigidBody*) m_wheelInfo[wheel].m_raycastInfo.m_groundObject; btVector3 rel_pos2 = wheelInfo.m_raycastInfo.m_contactPointWS - groundObject->getCenterOfMassPosition(); //adjust relative position above ground so that force only // acts sideways btVector3 delta_vec = (wheelInfo.m_raycastInfo.m_hardPointWS - wheelInfo.m_raycastInfo.m_contactPointWS); if (delta_vec.length() != btScalar (0)) { delta_vec = delta_vec.normalize(); rel_pos -= delta_vec * rel_pos.dot(delta_vec); } btVector3 sideImp = m_axle[wheel] * m_sideImpulse[wheel]; #if defined ROLLING_INFLUENCE_FIX && !defined COMPATIBLE_0_7_3 // fix. It only worked if car's up was along Y - VT. btVector3 vChassisWorldUp = getRigidBody()->getCenterOfMassTransform() .getBasis().getColumn(m_indexUpAxis); rel_pos -= vChassisWorldUp * (vChassisWorldUp.dot(rel_pos) * (1.f-wheelInfo.m_rollInfluence) ); #else rel_pos[m_indexUpAxis] *= wheelInfo.m_rollInfluence; #endif m_chassisBody->applyImpulse(sideImp,rel_pos); //apply friction impulse on the ground groundObject->applyImpulse(-sideImp,rel_pos2); } // if (m_sideImpulse[wheel] != btScalar(0.)) } // for wheel<getNumWheels()
/* Monitor GPS data to see if quality is good enough to initialise the EKF Monitor magnetometer innovations to see if the heading is good enough to use GPS Return true if all criteria pass for 10 seconds We also record the failure reason so that prearm_failure_reason() can give a good report to the user on why arming is failing */ bool NavEKF2_core::calcGpsGoodToAlign(void) { const AP_GPS &gps = AP::gps(); if (inFlight && assume_zero_sideslip() && !use_compass()) { // this is a special case where a plane has launched without magnetometer // is now in the air and needs to align yaw to the GPS and start navigating as soon as possible return true; } // User defined multiplier to be applied to check thresholds float checkScaler = 0.01f*(float)frontend->_gpsCheckScaler; // If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states // This enables us to handle large changes to the external magnetic field environment that occur before arming if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f && yawTestRatio <= 1.0f) || !consistentMagData) { magYawResetTimer_ms = imuSampleTime_ms; } if ((imuSampleTime_ms - magYawResetTimer_ms > 5000) && !motorsArmed) { // request reset of heading and magnetic field states magYawResetRequest = true; // reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds magYawResetTimer_ms = imuSampleTime_ms; } // Check for significant change in GPS position if disarmed which indicates bad GPS // This check can only be used when the vehicle is stationary const struct Location &gpsloc = gps.location(); // Current location const float posFiltTimeConst = 10.0f; // time constant used to decay position drift // calculate time lapsed since last update and limit to prevent numerical errors float deltaTime = constrain_float(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst); lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms; // Sum distance moved gpsDriftNE += gpsloc_prev.get_distance(gpsloc); gpsloc_prev = gpsloc; // Decay distance moved exponentially to zero gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst); // Clamp the filter state to prevent excessive persistence of large transients gpsDriftNE = MIN(gpsDriftNE,10.0f); // Fail if more than 3 metres drift after filtering whilst on-ground // This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m bool gpsDriftFail = (gpsDriftNE > 3.0f*checkScaler) && onGround && (frontend->_gpsCheck & MASK_GPS_POS_DRIFT); // Report check result as a text string and bitmask if (gpsDriftFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler)); gpsCheckStatus.bad_horiz_drift = true; } else { gpsCheckStatus.bad_horiz_drift = false; } // Check that the vertical GPS vertical velocity is reasonable after noise filtering bool gpsVertVelFail; if (gps.have_vertical_velocity() && onGround) { // check that the average vertical GPS velocity is close to zero gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt; gpsVertVelFilt = constrain_float(gpsVertVelFilt,-10.0f,10.0f); gpsVertVelFail = (fabsf(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD); } else if ((frontend->_fusionModeGPS == 0) && !gps.have_vertical_velocity()) { // If the EKF settings require vertical GPS velocity and the receiver is not outputting it, then fail gpsVertVelFail = true; // if we have a 3D fix with no vertical velocity and // EK2_GPS_TYPE=0 then change it to 1. It means the GPS is not // capable of giving a vertical velocity if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { frontend->_fusionModeGPS.set(1); gcs().send_text(MAV_SEVERITY_WARNING, "EK2: Changed EK2_GPS_TYPE to 1"); } } else { gpsVertVelFail = false; } // Report check result as a text string and bitmask if (gpsVertVelFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsf(gpsVertVelFilt), (double)(0.3f*checkScaler)); gpsCheckStatus.bad_vert_vel = true; } else { gpsCheckStatus.bad_vert_vel = false; } // Check that the horizontal GPS vertical velocity is reasonable after noise filtering // This check can only be used if the vehicle is stationary bool gpsHorizVelFail; if (onGround) { gpsHorizVelFilt = 0.1f * norm(gpsDataDelayed.vel.x,gpsDataDelayed.vel.y) + 0.9f * gpsHorizVelFilt; gpsHorizVelFilt = constrain_float(gpsHorizVelFilt,-10.0f,10.0f); gpsHorizVelFail = (fabsf(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD); } else { gpsHorizVelFail = false; } // Report check result as a text string and bitmask if (gpsHorizVelFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler)); gpsCheckStatus.bad_horiz_vel = true; } else { gpsCheckStatus.bad_horiz_vel = false; } // fail if horiziontal position accuracy not sufficient float hAcc = 0.0f; bool hAccFail; if (gps.horizontal_accuracy(hAcc)) { hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR); } else { hAccFail = false; } // Report check result as a text string and bitmask if (hAccFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler)); gpsCheckStatus.bad_hAcc = true; } else { gpsCheckStatus.bad_hAcc = false; } // Check for vertical GPS accuracy float vAcc = 0.0f; bool vAccFail = false; if (gps.vertical_accuracy(vAcc)) { vAccFail = (vAcc > 7.5f * checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR); } // Report check result as a text string and bitmask if (vAccFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS vert error %.1fm (needs < %.1f)", (double)vAcc, (double)(7.5f * checkScaler)); gpsCheckStatus.bad_vAcc = true; } else { gpsCheckStatus.bad_vAcc = false; } // fail if reported speed accuracy greater than threshold bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_SPD_ERR); // Report check result as a text string and bitmask if (gpsSpdAccFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS speed error %.1f (needs < %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler)); gpsCheckStatus.bad_sAcc = true; } else { gpsCheckStatus.bad_sAcc = false; } // fail if satellite geometry is poor bool hdopFail = (gps.get_hdop() > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP); // Report check result as a text string and bitmask if (hdopFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS HDOP %.1f (needs 2.5)", (double)(0.01f * gps.get_hdop())); gpsCheckStatus.bad_hdop = true; } else { gpsCheckStatus.bad_hdop = false; } // fail if not enough sats bool numSatsFail = (gps.num_sats() < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS); // Report check result as a text string and bitmask if (numSatsFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "GPS numsats %u (needs 6)", gps.num_sats()); gpsCheckStatus.bad_sats = true; } else { gpsCheckStatus.bad_sats = false; } // fail if magnetometer innovations are outside limits indicating bad yaw // with bad yaw we are unable to use GPS bool yawFail; if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f || yawTestRatio > 1.0f) && (frontend->_gpsCheck & MASK_GPS_YAW_ERR)) { yawFail = true; } else { yawFail = false; } // Report check result as a text string and bitmask if (yawFail) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Mag yaw error x=%.1f y=%.1f", (double)magTestRatio.x, (double)magTestRatio.y); gpsCheckStatus.bad_yaw = true; } else { gpsCheckStatus.bad_yaw = false; } // assume failed first time through and notify user checks have started if (lastGpsVelFail_ms == 0) { hal.util->snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF starting GPS checks"); lastGpsVelFail_ms = imuSampleTime_ms; } // record time of fail if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || vAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) { lastGpsVelFail_ms = imuSampleTime_ms; } // continuous period without fail required to return a healthy status if (imuSampleTime_ms - lastGpsVelFail_ms > 10000) { return true; } return false; }
// sends commands to the motors void AP_MotorsCoax::output_armed_stabilizing() { float roll_thrust; // roll thrust input value, +/- 1.0 float pitch_thrust; // pitch thrust input value, +/- 1.0 float yaw_thrust; // yaw thrust input value, +/- 1.0 float throttle_thrust; // throttle thrust input value, 0.0 - 1.0 float thrust_min_rpy; // the minimum throttle setting that will not limit the roll and pitch output float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy float thrust_out; // float rp_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits float actuator_allowed = 0.0f; // amount of yaw we can fit in // apply voltage and air pressure compensation roll_thrust = _roll_in * get_compensation_gain(); pitch_thrust = _pitch_in * get_compensation_gain(); yaw_thrust = _yaw_in * get_compensation_gain(); throttle_thrust = get_throttle() * get_compensation_gain(); // sanity check throttle is above zero and below current limited throttle if (throttle_thrust <= 0.0f) { throttle_thrust = 0.0f; limit.throttle_lower = true; } if (throttle_thrust >= _throttle_thrust_max) { throttle_thrust = _throttle_thrust_max; limit.throttle_upper = true; } _throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max); float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust)); // calculate how much roll and pitch must be scaled to leave enough range for the minimum yaw if (is_zero(rp_thrust_max)) { rp_scale = 1.0f; } else { rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), 0.5f*(float)_yaw_headroom/1000.0f)) / rp_thrust_max, 0.0f, 1.0f); if (rp_scale < 1.0f) { limit.roll_pitch = true; } } actuator_allowed = 2.0f * (1.0f - rp_scale * rp_thrust_max); if (fabsf(yaw_thrust) > actuator_allowed) { yaw_thrust = constrain_float(yaw_thrust, -actuator_allowed, actuator_allowed); limit.yaw = true; } // calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces thrust_min_rpy = MAX(fabsf(rp_scale * rp_thrust_max), fabsf(yaw_thrust)); thr_adj = throttle_thrust - _throttle_avg_max; if (thr_adj < (thrust_min_rpy - _throttle_avg_max)) { // Throttle can't be reduced to the desired level because this would mean roll or pitch control // would not be able to reach the desired level because of lack of thrust. thr_adj = MIN(thrust_min_rpy, _throttle_avg_max) - _throttle_avg_max; } // calculate the throttle setting for the lift fan thrust_out = _throttle_avg_max + thr_adj; if (fabsf(yaw_thrust) > thrust_out) { yaw_thrust = constrain_float(yaw_thrust, -thrust_out, thrust_out); limit.yaw = true; } _thrust_yt_ccw = thrust_out + 0.5f * yaw_thrust; _thrust_yt_cw = thrust_out - 0.5f * yaw_thrust; // limit thrust out for calculation of actuator gains float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,thrust_out), 0.1f, 1.0f); if (is_zero(thrust_out)) { limit.roll_pitch = true; } // force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared // static thrust is proportional to the airflow velocity squared // therefore the torque of the roll and pitch actuators should be approximately proportional to // the angle of attack multiplied by the static thrust. _actuator_out[0] = roll_thrust/thrust_out_actuator; _actuator_out[1] = pitch_thrust/thrust_out_actuator; if (fabsf(_actuator_out[0]) > 1.0f) { limit.roll_pitch = true; _actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f); } if (fabsf(_actuator_out[1]) > 1.0f) { limit.roll_pitch = true; _actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f); } _actuator_out[2] = -_actuator_out[0]; _actuator_out[3] = -_actuator_out[1]; }
__complex__ float __csqrtf (__complex__ float x) { __complex__ float res; int rcls = fpclassify (__real__ x); int icls = fpclassify (__imag__ x); if (__glibc_unlikely (rcls <= FP_INFINITE || icls <= FP_INFINITE)) { if (icls == FP_INFINITE) { __real__ res = HUGE_VALF; __imag__ res = __imag__ x; } else if (rcls == FP_INFINITE) { if (__real__ x < 0.0) { __real__ res = icls == FP_NAN ? __nanf ("") : 0; __imag__ res = __copysignf (HUGE_VALF, __imag__ x); } else { __real__ res = __real__ x; __imag__ res = (icls == FP_NAN ? __nanf ("") : __copysignf (0.0, __imag__ x)); } } else { __real__ res = __nanf (""); __imag__ res = __nanf (""); } } else { if (__glibc_unlikely (icls == FP_ZERO)) { if (__real__ x < 0.0) { __real__ res = 0.0; __imag__ res = __copysignf (__ieee754_sqrtf (-__real__ x), __imag__ x); } else { __real__ res = fabsf (__ieee754_sqrtf (__real__ x)); __imag__ res = __copysignf (0.0, __imag__ x); } } else if (__glibc_unlikely (rcls == FP_ZERO)) { float r; if (fabsf (__imag__ x) >= 2.0f * FLT_MIN) r = __ieee754_sqrtf (0.5f * fabsf (__imag__ x)); else r = 0.5f * __ieee754_sqrtf (2.0f * fabsf (__imag__ x)); __real__ res = r; __imag__ res = __copysignf (r, __imag__ x); } else { float d, r, s; int scale = 0; if (fabsf (__real__ x) > FLT_MAX / 4.0f) { scale = 1; __real__ x = __scalbnf (__real__ x, -2 * scale); __imag__ x = __scalbnf (__imag__ x, -2 * scale); } else if (fabsf (__imag__ x) > FLT_MAX / 4.0f) { scale = 1; if (fabsf (__real__ x) >= 4.0f * FLT_MIN) __real__ x = __scalbnf (__real__ x, -2 * scale); else __real__ x = 0.0f; __imag__ x = __scalbnf (__imag__ x, -2 * scale); } else if (fabsf (__real__ x) < FLT_MIN && fabsf (__imag__ x) < FLT_MIN) { scale = -(FLT_MANT_DIG / 2); __real__ x = __scalbnf (__real__ x, -2 * scale); __imag__ x = __scalbnf (__imag__ x, -2 * scale); } d = __ieee754_hypotf (__real__ x, __imag__ x); /* Use the identity 2 Re res Im res = Im x to avoid cancellation error in d +/- Re x. */ if (__real__ x > 0) { r = __ieee754_sqrtf (0.5f * (d + __real__ x)); if (scale == 1 && fabsf (__imag__ x) < 1.0f) { /* Avoid possible intermediate underflow. */ s = __imag__ x / r; r = __scalbnf (r, scale); scale = 0; } else s = 0.5f * (__imag__ x / r); } else { s = __ieee754_sqrtf (0.5f * (d - __real__ x)); if (scale == 1 && fabsf (__imag__ x) < 1.0f) { /* Avoid possible intermediate underflow. */ r = fabsf (__imag__ x / s); s = __scalbnf (s, scale); scale = 0; } else r = fabsf (0.5f * (__imag__ x / s)); } if (scale) { r = __scalbnf (r, scale); s = __scalbnf (s, scale); } __real__ res = r; __imag__ res = __copysignf (s, __imag__ x); } } return res; }
// 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) { 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 * _accel_vector; // 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; 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); 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; // limit vertical acceleration correction to 0.5 gravities. The // barometer sometimes gives crazy acceleration changes. vdelta.z = constrain_float(vdelta.z, -0.5f, 0.5f); GA_e += vdelta; GA_e.normalize(); if (GA_e.is_inf()) { // wait for some non-zero acceleration information return; } } // calculate the error term in earth frame. Vector3f GA_b = _ra_sum / (_ra_deltat * GRAVITY_MSS); float length = GA_b.length(); if (length > 1.0f) { GA_b /= length; 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; } } // 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 && _accel_vector.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); } }
void Standard::update_transition_state() { // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { if (_params_standard.front_trans_dur <= 0.0f) { // just set the final target throttle value _pusher_throttle = _params_standard.pusher_trans; } else if (_pusher_throttle <= _params_standard.pusher_trans) { // ramp up throttle to the target throttle value _pusher_throttle = _params_standard.pusher_trans * (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f); } // do blending of mc and fw controls if a blending airspeed has been provided if (_airspeed_trans_blend_margin > 0.0f && _airspeed->true_airspeed_m_s >= _params_standard.airspeed_blend) { float weight = 1.0f - fabsf(_airspeed->true_airspeed_m_s - _params_standard.airspeed_blend) / _airspeed_trans_blend_margin; _mc_roll_weight = weight; _mc_pitch_weight = weight; _mc_yaw_weight = weight; _mc_throttle_weight = weight; } else { // at low speeds give full weight to mc _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; _mc_throttle_weight = 1.0f; } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) { // continually increase mc attitude control as we transition back to mc mode if (_params_standard.back_trans_dur > 0.0f) { float weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.back_trans_dur * 1000000.0f); _mc_roll_weight = weight; _mc_pitch_weight = weight; _mc_yaw_weight = weight; _mc_throttle_weight = weight; } else { _mc_roll_weight = 1.0f; _mc_pitch_weight = 1.0f; _mc_yaw_weight = 1.0f; _mc_throttle_weight = 1.0f; } // in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again if (_flag_enable_mc_motors) { set_max_mc(2000); set_idle_mc(); _flag_enable_mc_motors = false; } } _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); _mc_pitch_weight = math::constrain(_mc_pitch_weight, 0.0f, 1.0f); _mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f); _mc_throttle_weight = math::constrain(_mc_throttle_weight, 0.0f, 1.0f); }
//------------------------------------------------------------------------------ void CTsmMatrixBuilder::BuildMatrix(CShadowClipper* clipper) { //这个函数优化空间很大,把矩阵下面手动算完,推导出公式最后放到程序中写成一个简单结果就行 //目前这种没有必要的矩阵乘法太多,有时间建议优化下 Ast(NULL != m_Camera); Ast(NULL != clipper); m_ViewMatrix = m_Camera->getViewMatrix(); // get the near and the far plane (points) in eye space. CVector3f frustumPnts[CFrustum::FrustumCornerNum]; CFrustum eyeFrustum(m_Camera->getProjectionMatrix()); for (index_t i = 0; i < 4; ++i) { frustumPnts[i] = *(CVector3f*)&(eyeFrustum.GetCorner((i<<1))); // far plane frustumPnts[i+4] = *(CVector3f*)&(eyeFrustum.GetCorner((i<<1) | 0x1)); // near plane } // we need to transform the eye into the light's post-projective space. // however, the sun is a directional light, so we first need to find an appropriate // rotate/translate matrix, before constructing an ortho projection. // this matrix is a variant of "light space" from LSPSMs, with the Y and Z axes permuted CVector3f leftVector, upVector, viewVector; const CVector3f eyeVector( 0.f, 0.f, -1.f ); // eye is always -Z in eye space // lightDir is defined in eye space, so xform it upVector = m_LightDir; upVector.MultiplyNormal(m_ViewMatrix); leftVector = upVector.Cross(eyeVector); leftVector.Normalize(); viewVector = upVector.Cross(leftVector); CMatrix lightSpaceBasis; lightSpaceBasis._11 = leftVector.x,lightSpaceBasis._12 = viewVector.x,lightSpaceBasis._13 = -upVector.x,lightSpaceBasis._14 = 0.f; lightSpaceBasis._21 = leftVector.y,lightSpaceBasis._22 = viewVector.y,lightSpaceBasis._23 = -upVector.y,lightSpaceBasis._24 = 0.f; lightSpaceBasis._31 = leftVector.z,lightSpaceBasis._32 = viewVector.z,lightSpaceBasis._33 = -upVector.z,lightSpaceBasis._34 = 0.f; lightSpaceBasis._41 = 0.f,lightSpaceBasis._42 = 0.f,lightSpaceBasis._43 = 0.f,lightSpaceBasis._44 = 1.f; // rotate the view frustum into light space for ( int i = 0; i< CFrustum::FrustumCornerNum; ++i ) frustumPnts[i] *= lightSpaceBasis; // build an off-center ortho projection that translates and scales the eye frustum's 3D AABB to the unit cube CAxisAlignedBox frustumBox; for (index_t i = 0; i < CFrustum::FrustumCornerNum; ++i) frustumBox.mergeBox(frustumPnts[i]); const CAxisAlignedBox& casterBox = clipper->GetCastersAABB(lightSpaceBasis); float min_z = min(frustumBox.getMinimum().z, casterBox.getMinimum().z); float max_z = max(frustumBox.getMaximum().z, casterBox.getMaximum().z); // virtually forward if ( min_z <= 0.f ) { CMatrix lightSpaceTranslate; lightSpaceTranslate.SetTranslate( 0.f, 0.f, -min_z + 1.f ); max_z = -min_z + max_z + 1.f; min_z = 1.f; lightSpaceBasis *= lightSpaceTranslate; for ( int i = 0; i< CFrustum::FrustumCornerNum; ++i ) frustumPnts[i] *= lightSpaceTranslate; frustumBox.setNull(); for (index_t i = 0; i < CFrustum::FrustumCornerNum; ++i) frustumBox.mergeBox(frustumPnts[i]); } // build orthogonal projection matrix CMatrix lightSpaceOrtho; lightSpaceOrtho.SetOrthoOffCenterLH(frustumBox.getMinimum().x, frustumBox.getMaximum().x, frustumBox.getMinimum().y, frustumBox.getMaximum().y, min_z, max_z ); // transform the view frustum by the new matrix for ( int i = 0; i< CFrustum::FrustumCornerNum; ++i ) frustumPnts[i] *= lightSpaceOrtho; CVector2f centerPts[2]; // near plane centerPts[0].x = 0.25f * (frustumPnts[4].x + frustumPnts[5].x + frustumPnts[6].x + frustumPnts[7].x); centerPts[0].y = 0.25f * (frustumPnts[4].y + frustumPnts[5].y + frustumPnts[6].y + frustumPnts[7].y); // far plane centerPts[1].x = 0.25f * (frustumPnts[0].x + frustumPnts[1].x + frustumPnts[2].x + frustumPnts[3].x); centerPts[1].y = 0.25f * (frustumPnts[0].y + frustumPnts[1].y + frustumPnts[2].y + frustumPnts[3].y); CVector2f centerOrig = (centerPts[0] + centerPts[1])*0.5f; CMatrix xlate_center(1.f, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, -centerOrig.x, -centerOrig.y, 0.f, 1.f ); float half_center_len = CVector2f(centerPts[1] - centerOrig).Mag(); float x_len = centerPts[1].x - centerOrig.x; float y_len = centerPts[1].y - centerOrig.y; float cos_theta = x_len / half_center_len; float sin_theta = y_len / half_center_len; CMatrix rot_center( cos_theta, -sin_theta, 0.f, 0.f, sin_theta, cos_theta, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 1.f ); // this matrix transforms the center line to y=0. // since Top and Base are orthogonal to Center, we can skip computing the convex hull, and instead // just find the view frustum X-axis extrema. The most negative is Top, the most positive is Base // Point Q (trapezoid projection point) will be a point on the y=0 line. m_ProjMatrix = xlate_center * rot_center; for ( int i = 0; i< CFrustum::FrustumCornerNum; ++i ) frustumPnts[i] *= m_ProjMatrix; CAxisAlignedBox frustumAABB2D; for (index_t i = 0; i < CFrustum::FrustumCornerNum; ++i) frustumAABB2D.mergeBox(frustumPnts[i]); float x_scale = max( fabsf(frustumAABB2D.getMaximum().x), fabsf(frustumAABB2D.getMinimum().x) ); float y_scale = max( fabsf(frustumAABB2D.getMaximum().y), fabsf(frustumAABB2D.getMinimum().y) ); x_scale = 1.f/x_scale; y_scale = 1.f/y_scale; // maximize the area occupied by the bounding box CMatrix scale_center(x_scale, 0.f, 0.f, 0.f, 0.f, y_scale, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 1.f); m_ProjMatrix *= scale_center; // scale the frustum AABB up by these amounts (keep all values in the same space) frustumAABB2D.scaleBox( CVector3f( x_scale, y_scale, 1.0f ) ); // compute eta. float lambda = frustumAABB2D.getMaximum().x - frustumAABB2D.getMinimum().x; float delta_proj = m_FocusRegion * lambda; //focusPt.x - frustumAABB2D.minPt.x; const float xi = -0.6f; // 80% line float eta = (lambda*delta_proj*(1.f+xi)) / (lambda*(1.f-xi)-2.f*delta_proj); // compute the projection point a distance eta from the top line. this point is on the center line, y=0 CVector2f projectionPtQ( frustumAABB2D.getMaximum().x + eta, 0.f ); // find the maximum slope from the projection point to any point in the frustum. this will be the // projection field-of-view float max_slope = -1e32f; float min_slope = 1e32f; for ( index_t i = 0; i < CFrustum::FrustumCornerNum; ++i ) { CVector2f tmp( frustumPnts[i].x*x_scale, frustumPnts[i].y*y_scale ); float x_dist = tmp.x - projectionPtQ.x; if ( !(gIsZero(tmp.y) || gIsZero(x_dist))) { max_slope = max(max_slope, tmp.y/x_dist); min_slope = min(min_slope, tmp.y/x_dist); } } float xn = eta; float xf = lambda + eta; CMatrix ptQ_xlate(-1.f, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, projectionPtQ.x, 0.f, 0.f, 1.f ); m_ProjMatrix *= ptQ_xlate; // this shear balances the "trapezoid" around the y=0 axis (no change to the projection pt position) // since we are redistributing the trapezoid, this affects the projection field of view (shear_amt) float shear_amt = (max_slope + fabsf(min_slope))*0.5f - max_slope; max_slope = max_slope + shear_amt; CMatrix trapezoid_shear( 1.f, shear_amt, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 1.f ); m_ProjMatrix *= trapezoid_shear; float z_aspect = (frustumBox.getMaximum().z-frustumBox.getMinimum().z) / (frustumAABB2D.getMaximum().y-frustumAABB2D.getMinimum().y); // perform a 2DH projection to 'unsqueeze' the top line. CMatrix trapezoid_projection( xf/(xf-xn), 0.f, 0.f, 1.f, 0.f, 1.f/max_slope, 0.f, 0.f, 0.f, 0.f, 1.f/(z_aspect*max_slope), 0.f, -xn*xf/(xf-xn),0.f, 0.f, 0.f ); m_ProjMatrix *= trapezoid_projection; // the x axis is compressed to [0..1] as a result of the projection, so expand it to [-1,1] CMatrix biasedScaleX( 2.f, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, -1.f, 0.f, 0.f, 1.f ); m_ProjMatrix *= biasedScaleX; m_ProjMatrix = lightSpaceOrtho * m_ProjMatrix; m_ProjMatrix = lightSpaceBasis * m_ProjMatrix; // now, focus on shadow receivers. m_ShadowMatrix = m_ViewMatrix * m_ProjMatrix; CAxisAlignedBox rcvrBox = clipper->GetCastersAABBWithProj(m_ShadowMatrix); rcvrBox.m_vMaximum.x = min( 1.f, rcvrBox.m_vMaximum.x ); rcvrBox.m_vMinimum.x = max(-1.f, rcvrBox.m_vMinimum.x ); rcvrBox.m_vMaximum.y = min( 1.f, rcvrBox.m_vMaximum.y ); rcvrBox.m_vMinimum.y = max(-1.f, rcvrBox.m_vMinimum.y ); float boxWidth = rcvrBox.m_vMaximum.x - rcvrBox.m_vMinimum.x; float boxHeight = rcvrBox.m_vMaximum.y - rcvrBox.m_vMinimum.y; // the receiver box is degenerate, this will generate specials (and there shouldn't be any shadows, anyway). if ( !(gIsZero(boxWidth) || gIsZero(boxHeight)) ) { // the divide by two's cancel out in the translation, but included for clarity float boxX = (rcvrBox.m_vMaximum.x+rcvrBox.m_vMinimum.x) / 2.f; float boxY = (rcvrBox.m_vMaximum.y+rcvrBox.m_vMinimum.y) / 2.f; CMatrix trapezoidUnitCube( 2.f/boxWidth, 0.f, 0.f, 0.f, 0.f, 2.f/boxHeight, 0.f, 0.f, 0.f, 0.f, 1.f, 0.f, -2.f*boxX/boxWidth, -2.f*boxY/boxHeight, 0.f, 1.f ); m_ProjMatrix *= trapezoidUnitCube; } m_ShadowMatrix = m_ViewMatrix * m_ProjMatrix; //set special texture matrix for shadow mapping float fOffsetX = 0.5f + (0.5f / (float)m_ShadowMapSize); float fOffsetY = 0.5f + (0.5f / (float)m_ShadowMapSize); CMatrix texCoordMatrix( 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, -0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, fOffsetX, fOffsetY, 0.0f, 1.0f ); m_ShadowMatrix *= texCoordMatrix; }
bool seekInternal(double t, int depth) { ResetRetries(); emptyFrameQueue(); audioHandler->clearQueue(); int64_t firstTs = getFirstSeekTs(); double backSeek = (double)depth * 2.0f + 1.0f; int64_t minTs = tsFromTime(t - backSeek - 2.5) + firstTs; int64_t ts = tsFromTime(t - backSeek) + firstTs; int64_t maxTs = tsFromTime(t - backSeek) + firstTs; // There is no discernible way to determine if negative timestamps are allowed // (or even required) to seek to low timestamps. // On some files you must seek to negative timestamps to be able to seek to 0 // but on other files you get weird results from seeking to below 0. // So, every other try, we will allow seeking to negative timestamps. if((depth % 2) == 1){ minTs = std::max((int64_t)0, minTs); ts = std::max((int64_t)0, minTs); maxTs = std::max((int64_t)0, minTs); } FlogD("Trying to seek to minTs: " << minTs << " ts: " << ts << " maxTs: " << maxTs << " with firsTs: " << firstTs); int flags = 0; if(ts < pFormatCtx->streams[videoStream]->cur_dts) flags |= AVSEEK_FLAG_BACKWARD; int seekRet = avformat_seek_file(pFormatCtx, videoStream, minTs, ts, maxTs, flags); if(seekRet > 0){ FlogD("avformat_seek_file failed, returned " << seekRet); return false; } avcodec_flush_buffers(pCodecCtx); double newTime = t + timeFromTs(firstPts); double actualTime = skipToTs(newTime); // consider the seek failed and try again if the actual time diffs more than .5 seconds // from the desired new time. FlogD("wanted to seek to " << newTime << " and ended up at " << actualTime); bool ret = true; if(fabsf(newTime - actualTime) > .5){ if(depth < 5){ FlogD("not good enough, trying again"); return seekInternal(t, depth + 1); } else{ ret = false; FlogW("seek failed, wanted to seek to " << newTime << " and ended up at " << actualTime); } } timeHandler->SetTime(actualTime); stepIntoQueue = true; audioHandler->onSeek(); return ret; }
magma_int_t magma_slaln2( magma_int_t trans, magma_int_t na, magma_int_t nw, float smin, float ca, const float *A, magma_int_t lda, float d1, float d2, const float *B, magma_int_t ldb, float wr, float wi, float *X, magma_int_t ldx, float *scale, float *xnorm, magma_int_t *info) { // normally, we use A(i,j) to be a pointer, A + i + j*lda, but // in this function it is more convenient to be an element, A[i + j*lda]. #define A(i_,j_) (A[ (i_) + (j_)*lda ]) #define B(i_,j_) (B[ (i_) + (j_)*ldb ]) #define X(i_,j_) (X[ (i_) + (j_)*ldx ]) /* Initialized data */ magma_int_t zswap[4] = { false, false, true, true }; magma_int_t rswap[4] = { false, true, false, true }; magma_int_t ipivot[16] = { 1, 2, 3, 4, 2, 1, 4, 3, 3, 4, 1, 2, 4, 3, 2, 1 }; /* was [4][4] */ /* System generated locals */ float d__1, d__2; /* Local variables */ magma_int_t j; float bi1, bi2, br1, br2, xi1, xi2, xr1, xr2, ci21, ci22, cr21, cr22, li21, csi, ui11, lr21, ui12, ui22; float csr, ur11, ur12, ur22; float bbnd, cmax, ui11r, ui12s, temp, ur11r, ur12s, u22abs; magma_int_t icmax; float bnorm, cnorm, smini; float bignum, smlnum; float equiv_0[4], equiv_1[4]; #define ci (equiv_0) /* EQUIVALENCE CI(2,2) and CIV(4) */ #define civ (equiv_0) #define cr (equiv_1) /* EQUIVALENCE CR(2,2) and CRV(4) */ #define crv (equiv_1) /* Parameter adjustments */ A -= 1 + lda; B -= 1 + ldb; X -= 1 + ldx; /* Compute BIGNUM */ smlnum = 2. * lapackf77_slamch("Safe minimum"); bignum = 1. / smlnum; smini = max(smin, smlnum); /* Don't check for input errors */ *info = 0; /* Standard Initializations */ *scale = 1.; if (na == 1) { /* 1 x 1 (i.e., scalar) system C X = B */ if (nw == 1) { /* Real 1x1 system. */ /* C = ca A - w D */ csr = ca * A(1,1) - wr * d1; cnorm = fabsf(csr); /* If | C | < SMINI, use C = SMINI */ if (cnorm < smini) { csr = smini; cnorm = smini; *info = 1; } /* Check scaling for X = B / C */ bnorm = fabsf( B(1,1) ); if (cnorm < 1. && bnorm > 1.) { if (bnorm > bignum * cnorm) { *scale = 1. / bnorm; } } /* Compute X */ X(1,1) = B(1,1) * *scale / csr; *xnorm = fabsf( X(1,1) ); } else { /* Complex 1x1 system (w is complex) */ /* C = ca A - w D */ csr = ca * A(1,1) - wr * d1; csi = -(wi) * d1; cnorm = fabsf(csr) + fabsf(csi); /* If | C | < SMINI, use C = SMINI */ if (cnorm < smini) { csr = smini; csi = 0.; cnorm = smini; *info = 1; } /* Check scaling for X = B / C */ bnorm = fabsf( B(1,1) ) + fabsf( B(1,2) ); if (cnorm < 1. && bnorm > 1.) { if (bnorm > bignum * cnorm) { *scale = 1. / bnorm; } } /* Compute X */ d__1 = *scale * B(1,1); d__2 = *scale * B(1,2); lapackf77_sladiv( &d__1, &d__2, &csr, &csi, &X(1,1), &X(1,2) ); *xnorm = fabsf( X(1,1) ) + fabsf( X(1,2) ); } } else { /* 2x2 System */ /* Compute the real part of C = ca A - w D (or ca A**T - w D ) */ cr[0] = ca * A(1,1) - wr * d1; cr[3] = ca * A(2,2) - wr * d2; if (trans) { cr[2] = ca * A(2,1); cr[1] = ca * A(1,2); } else { cr[1] = ca * A(2,1); cr[2] = ca * A(1,2); } if (nw == 1) { /* Real 2x2 system (w is real) */ /* Find the largest element in C */ cmax = 0.; icmax = 0; for (j = 1; j <= 4; ++j) { if (fabsf( crv[j - 1] ) > cmax) { cmax = fabsf( crv[j - 1] ); icmax = j; } /* L10: */ } /* If norm(C) < SMINI, use SMINI*identity. */ if (cmax < smini) { bnorm = max( fabsf( B(1,1) ), fabsf( B(2,1) ) ); if (smini < 1. && bnorm > 1.) { if (bnorm > bignum * smini) { *scale = 1. / bnorm; } } temp = *scale / smini; X(1,1) = temp * B(1,1); X(2,1) = temp * B(2,1); *xnorm = temp * bnorm; *info = 1; return *info; } /* Gaussian elimination with complete pivoting. */ ur11 = crv[icmax - 1]; cr21 = crv[ipivot[(icmax << 2) - 3] - 1]; ur12 = crv[ipivot[(icmax << 2) - 2] - 1]; cr22 = crv[ipivot[(icmax << 2) - 1] - 1]; ur11r = 1. / ur11; lr21 = ur11r * cr21; ur22 = cr22 - ur12 * lr21; /* If smaller pivot < SMINI, use SMINI */ if (fabsf(ur22) < smini) { ur22 = smini; *info = 1; } if (rswap[icmax - 1]) { br1 = B(2,1); br2 = B(1,1); } else { br1 = B(1,1); br2 = B(2,1); } br2 -= lr21 * br1; bbnd = max( fabsf( br1 * (ur22 * ur11r) ), fabsf(br2) ); if (bbnd > 1. && fabsf(ur22) < 1.) { if (bbnd >= bignum * fabsf(ur22)) { *scale = 1. / bbnd; } } xr2 = br2 * *scale / ur22; xr1 = *scale * br1 * ur11r - xr2 * (ur11r * ur12); if (zswap[icmax - 1]) { X(1,1) = xr2; X(2,1) = xr1; } else { X(1,1) = xr1; X(2,1) = xr2; } *xnorm = max( fabsf(xr1), fabsf(xr2) ); /* Further scaling if norm(A) norm(X) > overflow */ if (*xnorm > 1. && cmax > 1.) { if (*xnorm > bignum / cmax) { temp = cmax / bignum; X(1,1) = temp * X(1,1); X(2,1) = temp * X(2,1); *xnorm = temp * *xnorm; *scale = temp * *scale; } } } else { /* Complex 2x2 system (w is complex) */ /* Find the largest element in C */ ci[0] = -(wi) * d1; ci[1] = 0.; ci[2] = 0.; ci[3] = -(wi) * d2; cmax = 0.; icmax = 0; for (j = 1; j <= 4; ++j) { if ( fabsf( crv[j - 1] ) + fabsf( civ[j - 1] ) > cmax) { cmax = fabsf( crv[j - 1] ) + fabsf( civ[j - 1] ); icmax = j; } /* L20: */ } /* If norm(C) < SMINI, use SMINI*identity. */ if (cmax < smini) { bnorm = max( fabsf( B(1,1) ) + fabsf( B(1,2) ), fabsf( B(2,1) ) + fabsf( B(2,2) ) ); if (smini < 1. && bnorm > 1.) { if (bnorm > bignum * smini) { *scale = 1. / bnorm; } } temp = *scale / smini; X(1,1) = temp * B(1,1); X(2,1) = temp * B(2,1); X(1,2) = temp * B(1,2); X(2,2) = temp * B(2,2); *xnorm = temp * bnorm; *info = 1; return *info; } /* Gaussian elimination with complete pivoting. */ ur11 = crv[icmax - 1]; ui11 = civ[icmax - 1]; cr21 = crv[ipivot[(icmax << 2) - 3] - 1]; ci21 = civ[ipivot[(icmax << 2) - 3] - 1]; ur12 = crv[ipivot[(icmax << 2) - 2] - 1]; ui12 = civ[ipivot[(icmax << 2) - 2] - 1]; cr22 = crv[ipivot[(icmax << 2) - 1] - 1]; ci22 = civ[ipivot[(icmax << 2) - 1] - 1]; if (icmax == 1 || icmax == 4) { /* Code when off-diagonals of pivoted C are real */ if (fabsf(ur11) > fabsf(ui11)) { temp = ui11 / ur11; ur11r = 1. / (ur11 * (temp * temp + 1.)); ui11r = -temp * ur11r; } else { temp = ur11 / ui11; ui11r = -1. / (ui11 * (temp * temp + 1.)); ur11r = -temp * ui11r; } lr21 = cr21 * ur11r; li21 = cr21 * ui11r; ur12s = ur12 * ur11r; ui12s = ur12 * ui11r; ur22 = cr22 - ur12 * lr21; ui22 = ci22 - ur12 * li21; } else { /* Code when diagonals of pivoted C are real */ ur11r = 1. / ur11; ui11r = 0.; lr21 = cr21 * ur11r; li21 = ci21 * ur11r; ur12s = ur12 * ur11r; ui12s = ui12 * ur11r; ur22 = cr22 - ur12 * lr21 + ui12 * li21; ui22 = -ur12 * li21 - ui12 * lr21; } u22abs = fabsf(ur22) + fabsf(ui22); /* If smaller pivot < SMINI, use SMINI */ if (u22abs < smini) { ur22 = smini; ui22 = 0.; *info = 1; } if (rswap[icmax - 1]) { br2 = B(1,1); br1 = B(2,1); bi2 = B(1,2); bi1 = B(2,2); } else { br1 = B(1,1); br2 = B(2,1); bi1 = B(1,2); bi2 = B(2,2); } br2 = br2 - lr21 * br1 + li21 * bi1; bi2 = bi2 - li21 * br1 - lr21 * bi1; bbnd = max( (fabsf(br1) + fabsf(bi1)) * (u22abs * (fabsf(ur11r) + fabsf(ui11r))), fabsf(br2) + fabsf(bi2) ); if (bbnd > 1. && u22abs < 1.) { if (bbnd >= bignum * u22abs) { *scale = 1. / bbnd; br1 = *scale * br1; bi1 = *scale * bi1; br2 = *scale * br2; bi2 = *scale * bi2; } } lapackf77_sladiv( &br2, &bi2, &ur22, &ui22, &xr2, &xi2 ); xr1 = ur11r * br1 - ui11r * bi1 - ur12s * xr2 + ui12s * xi2; xi1 = ui11r * br1 + ur11r * bi1 - ui12s * xr2 - ur12s * xi2; if (zswap[icmax - 1]) { X(1,1) = xr2; X(2,1) = xr1; X(1,2) = xi2; X(2,2) = xi1; } else { X(1,1) = xr1; X(2,1) = xr2; X(1,2) = xi1; X(2,2) = xi2; } *xnorm = max( fabsf(xr1) + fabsf(xi1), fabsf(xr2) + fabsf(xi2) ); /* Further scaling if norm(A) norm(X) > overflow */ if (*xnorm > 1. && cmax > 1.) { if (*xnorm > bignum / cmax) { temp = cmax / bignum; X(1,1) = temp * X(1,1); X(2,1) = temp * X(2,1); X(1,2) = temp * X(1,2); X(2,2) = temp * X(2,2); *xnorm = temp * *xnorm; *scale = temp * *scale; } } } } return *info; } /* slaln2_ */
static float fclamp0(float x, float max) { x = (x + max - fabsf(x - max))/2.0f; return (x + fabsf(x))/2.0f; }
float RangeImageBorderExtractor::updatedScoreAccordingToNeighborValues(int x, int y, const float* border_scores) const { float max_score_bonus = 0.5f; float border_score = border_scores[y*range_image_->width+x]; // Check if an update can bring the score to a value higher than the minimum if (border_score + max_score_bonus*(1.0f-border_score) < parameters_.minimum_border_probability) return border_score; float average_neighbor_score=0.0f, weight_sum=0.0f; for (int y2=y-1; y2<=y+1; ++y2) { for (int x2=x-1; x2<=x+1; ++x2) { if (!range_image_->isInImage(x2, y2) || (x2==x&&y2==y)) continue; average_neighbor_score += border_scores[y2*range_image_->width+x2]; weight_sum += 1.0f; } } average_neighbor_score /=weight_sum; if (average_neighbor_score*border_score < 0.0f) return border_score; float new_border_score = border_score + max_score_bonus * average_neighbor_score * (1.0f-fabsf(border_score)); //std::cout << PVARC(border_score)<<PVARN(new_border_score); return new_border_score; }
void CCBManager::Update() { //チョコボールを生成していく。 const float deltaTime = 1.0f / 60.0f; m_timer += deltaTime; if (m_interval < m_timer){ int createCount = 0; while ( m_numCreate < CHOCO_NUM ){ if (createCount == 10){ break; } float rate = 100.0f / (rand() % 100 + 1); rate /= 50.0f; if (rand() % 2){ rate *= -1.0f; } D3DXVECTOR3 pos(GetStartPosition()); pos.x += rate; pos.z += fabsf(rate); pos.y += rate; D3DXVECTOR3 Epos(GetEndPosition()); Epos.x += rate; Epos.z += fabsf(rate); Epos.y += rate; m_Choco[m_numCreate].Initialize(pos, Epos); //if (m_IsFirst){ // D3DXMESHCONTAINER_DERIVED* pMeshContainer = static_cast<D3DXMESHCONTAINER_DERIVED*>(m_Choco[m_numCreate].GetImage()->pModel->GetFrameRoot()->pMeshContainer); // LPDIRECT3DVERTEXBUFFER9 buffer; // pMeshContainer->MeshData.pMesh->GetVertexBuffer(&buffer)/*->GetBufferPointer()*/; // VOID* pVertices; // buffer->Lock(0, sizeof(buffer), (void**)&pVertices, 0); // D3DVERTEXBUFFER_DESC desc; // buffer->GetDesc(&desc); // int stride = D3DXGetFVFVertexSize(desc.FVF); // char* p = (char*)pVertices; // int offset = -1; // D3DVERTEXELEMENT9 pDecl[MAX_FVF_DECL_SIZE]; // memset(pDecl, -1, sizeof(pDecl)); // pMeshContainer->MeshData.pMesh->GetDeclaration(pDecl); // for (int idx = 0; idx < MAX_FVF_DECL_SIZE; idx++){ // if (pDecl[idx].Stream == 255){ // //終わり // break; // } // if (pDecl[idx].Usage == D3DDECLUSAGE_BINORMAL){ // offset = pDecl[idx].Offset; // } // } // for (int vertNo = 0; vertNo < pMeshContainer->MeshData.pMesh->GetNumVertices(); vertNo++){ // D3DXVECTOR3* pBinomal = (D3DXVECTOR3*)&p[offset]; // pBinomal->x = // p += stride; // } // // // //buffer // //m_pVertexBuffer-> // //memcpy(pVertices, buffer, sizeof(buffer)); // //pVertices // buffer->Unlock(); // (*graphicsDevice()).CreateVertexBuffer(sizeof(buffer), 0, D3DFVF_CUSTOMVERTEX, D3DPOOL_DEFAULT, &m_pVertexBuffer, nullptr); // m_IsFirst = false; //} SINSTANCE(CShadowRender)->Entry(&m_Choco[m_numCreate]); createCount++; m_numCreate++; } } for (int i = 0; i < m_numCreate; i++) { m_Choco[i].Update(); } }
bool CCameraTracking::Update(SViewParams &viewParams, float &fHOffObstacleStrength, const SCamModeSettings &camMode, const CPlayer &hero, bool bObstacleFound /* = false */) { if(!g_pGameCVars->cl_cam_tracking || !m_pCamRayScan) return false; m_fFrameTime = max(g_fCamError, gEnv->pTimer->GetFrameTime()); //in combat mode this function doesn't really avoid obstacles, it avoids clipping float fCombatModeWeight = 5.0f; //default angle and minimum const float fNow = gEnv->pTimer->GetFrameStartTime().GetSeconds(); CCameraInputHelper *pCamHelper = hero.GetCameraInputHelper(); CRY_ASSERT(pCamHelper); float fLastUserInput = pCamHelper->GetLastUserInputTime(); //user input overrides auto-follow if(fNow - fLastUserInput < 0.5f) return false; bool bTrackingActive = camMode.camType == ECT_CamFollow && (camMode.collisionType == ECCT_CollisionTrack || camMode.collisionType == ECCT_CollisionTrackOrCut); //get current values Vec3 curCamDir = viewParams.position-viewParams.targetPos; m_curCamOrientation.Set(0.0f, 0.0f, 0.0f); CartesianToSpherical(curCamDir, m_curCamOrientation); curCamDir.Normalize(); if(m_curCamOrientation.m_fDist < g_pGameCVars->cl_cam_min_distance) m_curCamOrientation.m_fDist = g_pGameCVars->cl_cam_min_distance; //work in 0 .. 2PI m_curCamOrientation.m_fYaw += gf_PI; //if there is something in the way if(bObstacleFound) { //re-start fadeout m_fTimeCovered = 0.5f; //set last obstacle pos m_vLastObstaclePos = viewParams.position; //scan obstacle if(!IdentifyObstacle(curCamDir, hero)) return false; } else if(fabsf(m_fYawDelta) > g_fCamError || fabsf(m_fPitchDelta) > g_fCamError) { //if there is nothing in the way, fade out the movement //time based fade if(m_fTimeCovered > 0) { m_fTimeCovered = max(m_fTimeCovered - m_fFrameTime, 0.0f); //these interpolators should be time and not frame based m_fYawDelta = (g_fInterpolationRate * m_fYawDelta) * g_fInterpolationWeight; m_fPitchDelta = (g_fInterpolationRate * m_fPitchDelta) * g_fInterpolationWeight; m_fSpeed = (g_fInterpolationRate * m_fSpeed) * g_fInterpolationWeight; } else { m_fYawDelta = 0.0f; m_fPitchDelta = 0.0f; m_fSpeed = 0.0f; } } //apply delta rotation for obstacle avoidance if(fabsf(m_fYawDelta) > g_fCamError || fabsf(m_fPitchDelta) > g_fCamError) { if(bTrackingActive) { //set new yaw float newYaw = m_curCamOrientation.m_fYaw + m_fYawDelta; //re-align yaw //the camera direction is 90 degrees off and flipped compared to entity space newYaw = (newYaw - gf_PI * 0.5f) * -1.0f; //set new pitch float newPitch = m_curCamOrientation.m_fPitch + m_fPitchDelta; if(g_pGameCVars->cl_cam_orbit != 0) { //pCamHelper->SetTrackingDelta(-m_fYawDelta, m_fPitchDelta); pCamHelper->SetYawDelta(-m_fYawDelta); pCamHelper->SetPitchDelta(m_fPitchDelta); } else { //apply yaw/pitch on camera //pCamHelper->SetInterpolationTarget(newYaw, newPitch, gf_PI, 0.1f, 0.0f); //this will always reset follow cam interpolation pCamHelper->SetYawDelta(m_fYawDelta); pCamHelper->SetPitchDelta(m_fPitchDelta); } } else { //in orbit mode we basically simulate user input //pCamHelper->SetTrackingDelta(-fCombatModeWeight*g_fYawDelta, fCombatModeWeight*g_fPitchDelta); //in cutting mode we offset the camera to avoid clipping float offsetStrength = 0.0f; float offsetSpeed = 2.0f; if(bObstacleFound) { offsetStrength = (m_fYawDelta < 0.0f)?-g_fOffsetTrackingDistance:g_fOffsetTrackingDistance; offsetSpeed = 0.5f; } fHOffObstacleStrength = InterpolateTo(fHOffObstacleStrength, offsetStrength, offsetSpeed); } //CryLogAlways("new yaw %f, yawDelta %f", newYaw, g_yawDelta); return true; } else UpdateAutoFollow(viewParams, hero); return false; }
void LOPuzzle::checkTheConnection(char number) { const float gresh = 0.1; char * cParts; char cPartsCount = getConnectedParts(number, cParts); //left side if (number%puzzleHorizontalSize!=0 && connection[number][0]<0) { char part = number-1; if (puzzleParts[part].active && puzzleParts[number].rotation == puzzleParts[part].rotation) { float dx = (puzzleParts[number].position.x - puzzleParts[part].position.x); float dy = (puzzleParts[number].position.y - puzzleParts[part].position.y); bool q = false; switch (puzzleParts[number].rotation) { case 0: q = (fabsf(dx-puzzlePartSizeX)<gresh*puzzlePartSizeX && fabsf(dy)<gresh*puzzlePartSizeY); if (q) moveParts(SunnyVector2D(-(dx-puzzlePartSizeX),-dy), cParts, cPartsCount); break; case 1: q = (fabsf(-dy-puzzlePartSizeX)<gresh*puzzlePartSizeX && fabsf(dx)<gresh*puzzlePartSizeY); if (q) moveParts(SunnyVector2D(-dx,(-dy-puzzlePartSizeX)), cParts, cPartsCount); break; case 2: q = (fabsf(-dx-puzzlePartSizeX)<gresh*puzzlePartSizeX && fabsf(dy)<gresh*puzzlePartSizeY); if (q) moveParts(SunnyVector2D((-dx-puzzlePartSizeX),-dy), cParts, cPartsCount); break; case 3: q = (fabsf(dy-puzzlePartSizeX)<gresh*puzzlePartSizeX && fabsf(dx)<gresh*puzzlePartSizeY); if (q) moveParts(SunnyVector2D(-dx,-(dy-puzzlePartSizeX)), cParts, cPartsCount); break; default: break; } if (q) { connection[number][0] = part; connection[part][2] = number; playConnectSound = true; } } } //top side if (number>=puzzleHorizontalSize && connection[number][1]<0) { char part = number-puzzleHorizontalSize; if (puzzleParts[part].active && puzzleParts[number].rotation == puzzleParts[part].rotation) { float dx = (puzzleParts[number].position.x - puzzleParts[part].position.x); float dy = (puzzleParts[number].position.y - puzzleParts[part].position.y); bool q = false; switch (puzzleParts[number].rotation) { case 0: q = (fabsf(-dy-puzzlePartSizeY)<gresh*puzzlePartSizeY && fabsf(dx)<gresh*puzzlePartSizeX); if (q) moveParts(SunnyVector2D(-dx,(-dy-puzzlePartSizeY)), cParts, cPartsCount); break; case 1: q = (fabsf(-dx-puzzlePartSizeY)<gresh*puzzlePartSizeY && fabsf(dy)<gresh*puzzlePartSizeX); if (q) moveParts(SunnyVector2D((-dx-puzzlePartSizeY),-dy), cParts, cPartsCount); break; case 2: q = (fabsf(dy-puzzlePartSizeY)<gresh*puzzlePartSizeY && fabsf(dx)<gresh*puzzlePartSizeX); if (q) moveParts(SunnyVector2D(-dx,-(dy-puzzlePartSizeY)), cParts, cPartsCount); break; case 3: q = (fabsf(dx-puzzlePartSizeY)<gresh*puzzlePartSizeY && fabsf(dy)<gresh*puzzlePartSizeX); if (q) moveParts(SunnyVector2D(-(dx-puzzlePartSizeY),-dy), cParts, cPartsCount); break; default: break; } if (q) { connection[number][1] = part; connection[part][3] = number; playConnectSound = true; } } } //right side if (number%puzzleHorizontalSize!=puzzleHorizontalSize-1 && connection[number][2]<0) { char part = number+1; if (puzzleParts[part].active && puzzleParts[number].rotation == puzzleParts[part].rotation) { float dx = (puzzleParts[number].position.x - puzzleParts[part].position.x); float dy = (puzzleParts[number].position.y - puzzleParts[part].position.y); bool q = false; switch (puzzleParts[number].rotation) { case 0: q = (fabsf(-dx-puzzlePartSizeX)<gresh*puzzlePartSizeX && fabsf(dy)<gresh*puzzlePartSizeY); if (q) moveParts(SunnyVector2D((-dx-puzzlePartSizeX),-dy), cParts, cPartsCount); break; case 1: q = (fabsf(dy-puzzlePartSizeX)<gresh*puzzlePartSizeX && fabsf(dx)<gresh*puzzlePartSizeY); if (q) moveParts(SunnyVector2D(-dx,-(dy-puzzlePartSizeX)), cParts, cPartsCount); break; case 2: q = (fabsf(dx-puzzlePartSizeX)<gresh*puzzlePartSizeX && fabsf(dy)<gresh*puzzlePartSizeY); if (q) moveParts(SunnyVector2D(-(dx-puzzlePartSizeX),-dy), cParts, cPartsCount); break; case 3: q = (fabsf(-dy-puzzlePartSizeX)<gresh*puzzlePartSizeX && fabsf(dx)<gresh*puzzlePartSizeY); if (q) moveParts(SunnyVector2D(-dx,(-dy-puzzlePartSizeX)), cParts, cPartsCount); break; default: break; } if (q) { connection[number][2] = part; connection[part][0] = number; playConnectSound = true; } } } //bottom side if (number/puzzleHorizontalSize+1 < puzzleVerticalSize && connection[number][3]<0) { char part = number+puzzleHorizontalSize; if (puzzleParts[part].active && puzzleParts[number].rotation == puzzleParts[part].rotation) { float dx = (puzzleParts[number].position.x - puzzleParts[part].position.x); float dy = (puzzleParts[number].position.y - puzzleParts[part].position.y); bool q = false; switch (puzzleParts[number].rotation) { case 0: q = (fabsf(dy-puzzlePartSizeY)<gresh*puzzlePartSizeY && fabsf(dx)<gresh*puzzlePartSizeX); if (q) moveParts(SunnyVector2D(-dx,-(dy-puzzlePartSizeY)), cParts, cPartsCount); break; case 1: q = (fabsf(dx-puzzlePartSizeY)<gresh*puzzlePartSizeY && fabsf(dy)<gresh*puzzlePartSizeX); if (q) moveParts(SunnyVector2D(-(dx-puzzlePartSizeY),-dy), cParts, cPartsCount); break; case 2: q = (fabsf(-dy-puzzlePartSizeY)<gresh*puzzlePartSizeY && fabsf(dx)<gresh*puzzlePartSizeX); if (q) moveParts(SunnyVector2D(-dx,(-dy-puzzlePartSizeY)), cParts, cPartsCount); break; case 3: q = (fabsf(-dx-puzzlePartSizeY)<gresh*puzzlePartSizeY && fabsf(dy)<gresh*puzzlePartSizeX); if (q) moveParts(SunnyVector2D((-dx-puzzlePartSizeY),-dy), cParts, cPartsCount); break; default: break; } if (q) { connection[number][3] = part; connection[part][1] = number; playConnectSound = true; } } } free(cParts); }
static void Mag_Calibration(void) // Called from XHz loop normally.... { float xyz[3], x_sumplain = 0.0f, x_sumsq = 0.0f, x_sumcube = 0.0f, y_sumplain = 0.0f, y_sumsq = 0.0f; float y_sumcube = 0.0f, z_sumplain = 0.0f, z_sumsq = 0.0f, z_sumcube = 0.0f, xy_sum = 0.0f; float xz_sum = 0.0f, yz_sum = 0.0f, x2y_sum = 0.0f, x2z_sum = 0.0f, y2x_sum = 0.0f, y2z_sum = 0.0f; float z2x_sum = 0.0f, z2y_sum = 0.0f, x2, y2, z2, x_sum, x_sum2, x_sum3, y_sum, y_sum2, y_sum3, z_sum; float z_sum2, z_sum3, XY, XZ, YZ, X2Y, X2Z, Y2X, Y2Z, Z2X, Z2Y, F0, F1, F2, F3, F4, A, B, C, A2, B2; float C2, QS, QB, Rsq, Q0, Q1, Q2, aA, aB, aC, nA, nB, nC, dA, dB, dC, fltsize = (float)MAGmaxcount; uint16_t i, gathercnt = (uint16_t)cfg.mag_time * 5; LD0_OFF(); // Green LED OFF LD1_ON(); // Red LED ON Mag_42Hz_AVG(xyz, MAGdiscardcnt); // Discard some for (i = 0; i < MAGmaxcount; i++) // Gather up Mag Data. Freeze FC. Adjust Mag readout JUST by GAIN/SCALE { Mag_42Hz_AVG(xyz, gathercnt); x2 = xyz[0] * xyz[0]; // http://imaginaryz.blogspot.de/2011/04/least-squares-fit-sphere-to-3d-data.html y2 = xyz[1] * xyz[1]; z2 = xyz[2] * xyz[2]; x_sumplain += xyz[0]; x_sumsq += x2; x_sumcube += x2 * xyz[0]; y_sumplain += xyz[1]; y_sumsq += y2; y_sumcube += y2 * xyz[1]; z_sumplain += xyz[2]; z_sumsq += z2; z_sumcube += z2 * xyz[2]; xy_sum += xyz[0] * xyz[1]; xz_sum += xyz[0] * xyz[2]; yz_sum += xyz[1] * xyz[2]; x2y_sum += x2 * xyz[1]; x2z_sum += x2 * xyz[2]; y2x_sum += y2 * xyz[0]; y2z_sum += y2 * xyz[2]; z2x_sum += z2 * xyz[0]; z2y_sum += z2 * xyz[1]; LED0_TOGGLE LED1_TOGGLE } x_sum = x_sumplain / fltsize; x_sum2 = x_sumsq / fltsize; x_sum3 = x_sumcube / fltsize; y_sum = y_sumplain / fltsize; y_sum2 = y_sumsq / fltsize; y_sum3 = y_sumcube / fltsize; z_sum = z_sumplain / fltsize; z_sum2 = z_sumsq / fltsize; z_sum3 = z_sumcube / fltsize; XY = xy_sum / fltsize; XZ = xz_sum / fltsize; YZ = yz_sum / fltsize; X2Y = x2y_sum / fltsize; X2Z = x2z_sum / fltsize; Y2X = y2x_sum / fltsize; Y2Z = y2z_sum / fltsize; Z2X = z2x_sum / fltsize; Z2Y = z2y_sum / fltsize; F0 = x_sum2 + y_sum2 + z_sum2; F1 = 0.5f * F0; F2 = -8.0f * (x_sum3 + Y2X + Z2X); F3 = -8.0f * (X2Y + y_sum3 + Z2Y); F4 = -8.0f * (X2Z + Y2Z + z_sum3); A = x_sum; B = y_sum; C = z_sum; A2 = A * A; B2 = B * B; C2 = C * C; QS = A2 + B2 + C2; QB = -2.0f * QS; Rsq = F0 + QB + QS; Q0 = 0.5f * (QS - Rsq); Q1 = F1 + Q0; Q2 = 8.0f * (QS - Rsq + QB + F0); for (i = 0; i < maxiterations; i++) { aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); aA = (aA == 0.0f) ? 1.0f : aA; aB = (aB == 0.0f) ? 1.0f : aB; aC = (aC == 0.0f) ? 1.0f : aC; nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB); nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC); dA = (nA - A); dB = (nB - B); dC = (nC - C); if ((dA * dA + dB * dB + dC * dC) <= sflsdelta) break; A = nA; B = nB; C = nC; A2 = A * A; B2 = B * B; C2 = C * C; QS = A2 + B2 + C2; QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum); Rsq = F0 + QB + QS; Q0 = 0.5f * (QS - Rsq); Q1 = F1 + Q0; Q2 = 8.0f * (QS - Rsq + QB + F0); } cfg.magZero[0] = A; cfg.magZero[1] = B; cfg.magZero[2] = C; cfg.mag_calibrated = 1; for (i = 0; i < 3; i++) { if (fabsf(cfg.magZero[i]) > MAGerror) { cfg.mag_calibrated = 0; // Supress GPS functions & Guicrazymag cfg.magZero[i] = 0; } } writeParams(1); // Calibration done, save whatever result systemReset(false); }
// yaw drift correction using the compass or GPS // this function prodoces the _omega_yaw_P vector, and also // contributes to the _omega_I.z long term yaw drift estimate void AP_AHRS_DCM::drift_correction_yaw(void) { bool new_value = false; float yaw_error; float yaw_deltat; if (use_compass()) { /* we are using compass for yaw */ if (_compass->last_update != _compass_last_update) { yaw_deltat = (_compass->last_update - _compass_last_update) * 1.0e-6f; _compass_last_update = _compass->last_update; // we force an additional compass read() // here. This has the effect of throwing away // the first compass value, which can be bad if (!_flags.have_initial_yaw && _compass->read()) { float heading = _compass->calculate_heading(_dcm_matrix); _dcm_matrix.from_euler(roll, pitch, heading); _omega_yaw_P.zero(); _flags.have_initial_yaw = true; } new_value = true; yaw_error = yaw_error_compass(); } } else if (_flags.fly_forward && have_gps()) { /* we are using GPS for yaw */ if (_gps->last_fix_time != _gps_last_update && _gps->ground_speed_cm >= GPS_SPEED_MIN) { yaw_deltat = (_gps->last_fix_time - _gps_last_update) * 1.0e-3f; _gps_last_update = _gps->last_fix_time; new_value = true; float gps_course_rad = ToRad(_gps->ground_course_cd * 0.01f); float yaw_error_rad = gps_course_rad - yaw; yaw_error = sinf(yaw_error_rad); /* reset yaw to match GPS heading under any of the following 3 conditions: 1) if we have reached GPS_SPEED_MIN and have never had yaw information before 2) if the last time we got yaw information from the GPS is more than 20 seconds ago, which means we may have suffered from considerable gyro drift 3) if we are over 3*GPS_SPEED_MIN (which means 9m/s) and our yaw error is over 60 degrees, which means very poor yaw. This can happen on bungee launch when the operator pulls back the plane rapidly enough then on release the GPS heading changes very rapidly */ if (!_flags.have_initial_yaw || yaw_deltat > 20 || (_gps->ground_speed_cm >= 3*GPS_SPEED_MIN && fabsf(yaw_error_rad) >= 1.047f)) { // reset DCM matrix based on current yaw _dcm_matrix.from_euler(roll, pitch, gps_course_rad); _omega_yaw_P.zero(); _flags.have_initial_yaw = true; yaw_error = 0; } } } if (!new_value) { // we don't have any new yaw information // slowly decay _omega_yaw_P to cope with loss // of our yaw source _omega_yaw_P *= 0.97f; return; } // convert the error vector to body frame float error_z = _dcm_matrix.c.z * yaw_error; // the spin rate changes the P gain, and disables the // integration at higher rates float spin_rate = _omega.length(); // update the proportional control to drag the // yaw back to the right value. We use a gain // that depends on the spin rate. See the fastRotations.pdf // paper from Bill Premerlani _omega_yaw_P.z = error_z * _P_gain(spin_rate) * _kp_yaw; if (_flags.fast_ground_gains) { _omega_yaw_P.z *= 8; } // don't update the drift term if we lost the yaw reference // for more than 2 seconds if (yaw_deltat < 2.0f && spin_rate < ToRad(SPIN_RATE_LIMIT)) { // also add to the I term _omega_I_sum.z += error_z * _ki_yaw * yaw_deltat; } _error_yaw_sum += fabsf(yaw_error); _error_yaw_count++; }
// This works by converting the SVG arc to "simple" beziers. // Partly adapted from Niko's code in kdelibs/kdecore/svgicons. // See also SVG implementation notes: http://www.w3.org/TR/SVG/implnote.html#ArcConversionEndpointToCenter bool SVGPathNormalizer::decomposeArcToCubic(const FloatPoint& currentPoint, const PathSegmentData& arcSegment) { // If rx = 0 or ry = 0 then this arc is treated as a straight line segment (a "lineto") joining the endpoints. // http://www.w3.org/TR/SVG/implnote.html#ArcOutOfRangeParameters float rx = fabsf(arcSegment.arcRadii().x()); float ry = fabsf(arcSegment.arcRadii().y()); if (!rx || !ry) return false; // If the current point and target point for the arc are identical, it should be treated as a zero length // path. This ensures continuity in animations. if (arcSegment.targetPoint == currentPoint) return false; float angle = arcSegment.arcAngle(); FloatSize midPointDistance = currentPoint - arcSegment.targetPoint; midPointDistance.scale(0.5f); AffineTransform pointTransform; pointTransform.rotate(-angle); FloatPoint transformedMidPoint = pointTransform.mapPoint(FloatPoint(midPointDistance.width(), midPointDistance.height())); float squareRx = rx * rx; float squareRy = ry * ry; float squareX = transformedMidPoint.x() * transformedMidPoint.x(); float squareY = transformedMidPoint.y() * transformedMidPoint.y(); // Check if the radii are big enough to draw the arc, scale radii if not. // http://www.w3.org/TR/SVG/implnote.html#ArcCorrectionOutOfRangeRadii float radiiScale = squareX / squareRx + squareY / squareRy; if (radiiScale > 1) { rx *= sqrtf(radiiScale); ry *= sqrtf(radiiScale); } pointTransform.makeIdentity(); pointTransform.scale(1 / rx, 1 / ry); pointTransform.rotate(-angle); FloatPoint point1 = pointTransform.mapPoint(currentPoint); FloatPoint point2 = pointTransform.mapPoint(arcSegment.targetPoint); FloatSize delta = point2 - point1; float d = delta.width() * delta.width() + delta.height() * delta.height(); float scaleFactorSquared = std::max(1 / d - 0.25f, 0.f); float scaleFactor = sqrtf(scaleFactorSquared); if (arcSegment.arcSweep == arcSegment.arcLarge) scaleFactor = -scaleFactor; delta.scale(scaleFactor); FloatPoint centerPoint = point1 + point2; centerPoint.scale(0.5f, 0.5f); centerPoint.move(-delta.height(), delta.width()); float theta1 = FloatPoint(point1 - centerPoint).slopeAngleRadians(); float theta2 = FloatPoint(point2 - centerPoint).slopeAngleRadians(); float thetaArc = theta2 - theta1; if (thetaArc < 0 && arcSegment.arcSweep) thetaArc += twoPiFloat; else if (thetaArc > 0 && !arcSegment.arcSweep) thetaArc -= twoPiFloat; pointTransform.makeIdentity(); pointTransform.rotate(angle); pointTransform.scale(rx, ry); // Some results of atan2 on some platform implementations are not exact enough. So that we get more // cubic curves than expected here. Adding 0.001f reduces the count of sgements to the correct count. int segments = ceilf(fabsf(thetaArc / (piOverTwoFloat + 0.001f))); for (int i = 0; i < segments; ++i) { float startTheta = theta1 + i * thetaArc / segments; float endTheta = theta1 + (i + 1) * thetaArc / segments; float t = (8 / 6.f) * tanf(0.25f * (endTheta - startTheta)); if (!std::isfinite(t)) return false; float sinStartTheta = sinf(startTheta); float cosStartTheta = cosf(startTheta); float sinEndTheta = sinf(endTheta); float cosEndTheta = cosf(endTheta); point1 = FloatPoint(cosStartTheta - t * sinStartTheta, sinStartTheta + t * cosStartTheta); point1.move(centerPoint.x(), centerPoint.y()); FloatPoint targetPoint = FloatPoint(cosEndTheta, sinEndTheta); targetPoint.move(centerPoint.x(), centerPoint.y()); point2 = targetPoint; point2.move(t * sinEndTheta, -t * cosEndTheta); PathSegmentData cubicSegment; cubicSegment.command = PathSegCurveToCubicAbs; cubicSegment.point1 = pointTransform.mapPoint(point1); cubicSegment.point2 = pointTransform.mapPoint(point2); cubicSegment.targetPoint = pointTransform.mapPoint(targetPoint); m_consumer->emitSegment(cubicSegment); } return true; }
BOOL LLVOSurfacePatch::lineSegmentIntersect(const LLVector3& start, const LLVector3& end, S32 face, BOOL pick_transparent, S32 *face_hitp, LLVector3* intersection,LLVector2* tex_coord, LLVector3* normal, LLVector3* bi_normal) { if (!lineSegmentBoundingBox(start, end)) { return FALSE; } LLVector3 delta = end-start; LLVector3 pdelta = delta; pdelta.mV[2] = 0; F32 plength = pdelta.length(); F32 tdelta = 1.f/plength; LLVector3 origin = start - mRegionp->getOriginAgent(); if (mRegionp->getLandHeightRegion(origin) > origin.mV[2]) { //origin is under ground, treat as no intersection return FALSE; } //step one meter at a time until intersection point found //VECTORIZE THIS const LLVector4a* exta = mDrawable->getSpatialExtents(); LLVector3 ext[2]; ext[0].set(exta[0].getF32ptr()); ext[1].set(exta[1].getF32ptr()); F32 rad = (delta*tdelta).magVecSquared(); F32 t = 0.f; while ( t <= 1.f) { LLVector3 sample = origin + delta*t; if (AABBSphereIntersectR2(ext[0], ext[1], sample+mRegionp->getOriginAgent(), rad)) { F32 height = mRegionp->getLandHeightRegion(sample); if (height > sample.mV[2]) { //ray went below ground, positive intersection //quick and dirty binary search to get impact point tdelta = -tdelta*0.5f; F32 err_dist = 0.001f; F32 dist = fabsf(sample.mV[2] - height); while (dist > err_dist && tdelta*tdelta > 0.0f) { t += tdelta; sample = origin+delta*t; height = mRegionp->getLandHeightRegion(sample); if ((tdelta < 0 && height < sample.mV[2]) || (height > sample.mV[2] && tdelta > 0)) { //jumped over intersection point, go back tdelta = -tdelta; } tdelta *= 0.5f; dist = fabsf(sample.mV[2] - height); } if (intersection) { F32 height = mRegionp->getLandHeightRegion(sample); if (fabsf(sample.mV[2]-height) < delta.length()*tdelta) { sample.mV[2] = mRegionp->getLandHeightRegion(sample); } *intersection = sample + mRegionp->getOriginAgent(); } if (normal) { *normal = mRegionp->getLand().resolveNormalGlobal(mRegionp->getPosGlobalFromRegion(sample)); } return TRUE; } } t += tdelta; if (t > 1 && t < 1.f+tdelta*0.99f) { //make sure end point is checked (saves vertical lines coming up negative) t = 1.f; } } return FALSE; }
// Public Methods ////////////////////////////////////////////////////////////// bool AP_Compass_HMC5843::init() { int numAttempts = 0, good_count = 0; bool success = false; uint8_t calibration_gain = 0x20; uint16_t expected_x = 715; uint16_t expected_yz = 715; float gain_multiple = 1.0; hal.scheduler->delay(10); _i2c_sem = hal.i2c->get_semaphore(); if (!_i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) { hal.scheduler->panic(PSTR("Failed to get HMC5843 semaphore")); } // determine if we are using 5843 or 5883L _base_config = 0; if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) || !read_register(ConfigRegA, &_base_config)) { _healthy[0] = false; _i2c_sem->give(); return false; } if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) { // a 5883L supports the sample averaging config product_id = AP_COMPASS_TYPE_HMC5883L; calibration_gain = 0x60; expected_x = 766; expected_yz = 713; gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain } else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) { product_id = AP_COMPASS_TYPE_HMC5843; } else { // not behaving like either supported compass type _i2c_sem->give(); return false; } calibration[0] = 0; calibration[1] = 0; calibration[2] = 0; while ( success == 0 && numAttempts < 20 && good_count < 5) { // record number of attempts at initialisation numAttempts++; // force positiveBias (compass should return 715 for all channels) if (!write_register(ConfigRegA, PositiveBiasConfig)) continue; // compass not responding on the bus hal.scheduler->delay(50); // set gains if (!write_register(ConfigRegB, calibration_gain) || !write_register(ModeRegister, SingleConversion)) continue; // read values from the compass hal.scheduler->delay(50); if (!read_raw()) continue; // we didn't read valid values hal.scheduler->delay(10); float cal[3]; cal[0] = fabsf(expected_x / (float)_mag_x); cal[1] = fabsf(expected_yz / (float)_mag_y); cal[2] = fabsf(expected_yz / (float)_mag_z); if (cal[0] > 0.7f && cal[0] < 1.35f && cal[1] > 0.7f && cal[1] < 1.35f && cal[2] > 0.7f && cal[2] < 1.35f) { good_count++; calibration[0] += cal[0]; calibration[1] += cal[1]; calibration[2] += cal[2]; } #if 0 /* useful for debugging */ hal.console->printf_P(PSTR("MagX: %d MagY: %d MagZ: %d\n"), (int)_mag_x, (int)_mag_y, (int)_mag_z); hal.console->printf_P(PSTR("CalX: %.2f CalY: %.2f CalZ: %.2f\n"), cal[0], cal[1], cal[2]); #endif } if (good_count >= 5) { calibration[0] = calibration[0] * gain_multiple / good_count; calibration[1] = calibration[1] * gain_multiple / good_count; calibration[2] = calibration[2] * gain_multiple / good_count; success = true; } else { /* best guess */ calibration[0] = 1.0; calibration[1] = 1.0; calibration[2] = 1.0; } // leave test mode if (!re_initialise()) { _i2c_sem->give(); return false; } _i2c_sem->give(); _initialised = true; // perform an initial read _healthy[0] = true; read(); return success; }
// Draws thick lines from triangles void DrawGLThickLines( int n, wxPoint points[],wxCoord xoffset, wxCoord yoffset, wxPen pen, bool b_hiqual ) { #ifdef ocpnUSE_GL if(n < 2) return; /* for dashed case, for now just draw thick lines */ wxDash *dashes; if( pen.GetDashes( &dashes ) ) { wxPoint p0 = points[0]; for( int i = 1; i < n; i++ ) { DrawGLThickLine( p0.x + xoffset, p0.y + yoffset, points[i].x + xoffset, points[i].y + yoffset, pen, b_hiqual ); p0 = points[i]; } return; } /* cull zero segments */ wxPoint *cpoints = new wxPoint[n]; cpoints[0] = points[0]; int c = 1; for( int i = 1; i < n; i++ ) { if(points[i].x != points[i-1].x || points[i].y != points[i-1].y) cpoints[c++] = points[i]; } /* nicer than than rendering each segment separately, this is because thick line segments drawn as rectangles which have different angles have rectangles which overlap and also leave a gap. This code properly calculates vertexes for adjoining segments */ float t1 = pen.GetWidth(); float x0 = cpoints[0].x, y0 = cpoints[0].y, x1 = cpoints[1].x, y1 = cpoints[1].y; float a0 = atan2f( y1 - y0, x1 - x0 ); // It is also possible to use triangle strip, (and triangle fan for endcap) // to reduce vertex count.. is it worth it? glBegin( GL_TRIANGLES ); float t2sina0 = t1 / 2 * sinf( a0 ); float t2cosa0 = t1 / 2 * cosf( a0 ); for( int i = 1; i < c; i++ ) { float x2, y2; float a1; if(i < c - 1) { x2 = cpoints[i + 1].x, y2 = cpoints[i + 1].y; a1 = atan2f( y2 - y1, x2 - x1 ); } else { x2 = x1, y2 = y1; a1 = a0; } float aa = (a0 + a1) / 2; float diff = fabsf(a0 - a1); if(diff > M_PI) diff -= 2 * (float)M_PI; float rad = t1 / 2 / wxMax(cosf(diff / 2), .4); float t2sina1 = rad * sinf( aa ); float t2cosa1 = rad * cosf( aa ); glVertex2f( x1 + t2sina1, y1 - t2cosa1 ); glVertex2f( x1 - t2sina1, y1 + t2cosa1 ); glVertex2f( x0 + t2sina0, y0 - t2cosa0 ); glVertex2f( x0 - t2sina0, y0 + t2cosa0 ); glVertex2f( x0 + t2sina0, y0 - t2cosa0 ); float dot = t2sina0 * t2sina1 + t2cosa0 * t2cosa1; if(dot > 0) glVertex2f( x1 - t2sina1, y1 + t2cosa1 ); else glVertex2f( x1 + t2sina1, y1 - t2cosa1 ); x0 = x1, x1 = x2; y0 = y1, y1 = y2; a0 = a1; t2sina0 = t2sina1, t2cosa0 = t2cosa1; } if(pen.GetCap() == wxCAP_ROUND) { DrawEndCap( x0, y0, t1, a0); DrawEndCap( x0, y0, t1, a0 + M_PI); } glEnd(); glPopAttrib(); delete [] cpoints; #endif }
void dt_view_image_expose( dt_view_image_over_t *image_over, uint32_t imgid, cairo_t *cr, int32_t width, int32_t height, int32_t zoom, int32_t px, int32_t py) { const double start = dt_get_wtime(); // some performance tuning stuff, for your pleasure. // on my machine with 7 image per row it seems grouping has the largest // impact from around 400ms -> 55ms per redraw. #define DRAW_THUMB 1 #define DRAW_COLORLABELS 1 #define DRAW_GROUPING 1 #define DRAW_SELECTED 1 #define DRAW_HISTORY 1 #if DRAW_THUMB == 1 // this function is not thread-safe (gui-thread only), so we // can safely allocate this leaking bit of memory to decompress thumbnails: static int first_time = 1; static uint8_t *scratchmem = NULL; if(first_time) { // scratchmem might still be NULL after this, if compression is off. scratchmem = dt_mipmap_cache_alloc_scratchmem(darktable.mipmap_cache); first_time = 0; } #endif cairo_save (cr); float bgcol = 0.4, fontcol = 0.425, bordercol = 0.1, outlinecol = 0.2; int selected = 0, altered = 0, imgsel = -1, is_grouped = 0; // this is a gui thread only thing. no mutex required: imgsel = darktable.control->global_settings.lib_image_mouse_over_id; #if DRAW_SELECTED == 1 /* clear and reset statements */ DT_DEBUG_SQLITE3_CLEAR_BINDINGS(darktable.view_manager->statements.is_selected); DT_DEBUG_SQLITE3_RESET(darktable.view_manager->statements.is_selected); /* bind imgid to prepared statments */ DT_DEBUG_SQLITE3_BIND_INT(darktable.view_manager->statements.is_selected, 1, imgid); /* lets check if imgid is selected */ if(sqlite3_step(darktable.view_manager->statements.is_selected) == SQLITE_ROW) selected = 1; #endif #if DRAW_HISTORY == 1 DT_DEBUG_SQLITE3_CLEAR_BINDINGS(darktable.view_manager->statements.have_history); DT_DEBUG_SQLITE3_RESET(darktable.view_manager->statements.have_history); DT_DEBUG_SQLITE3_BIND_INT(darktable.view_manager->statements.have_history, 1, imgid); /* lets check if imgid has history */ if(sqlite3_step(darktable.view_manager->statements.have_history) == SQLITE_ROW) altered = 1; #endif const dt_image_t *img = dt_image_cache_read_testget(darktable.image_cache, imgid); #if DRAW_GROUPING == 1 DT_DEBUG_SQLITE3_CLEAR_BINDINGS(darktable.view_manager->statements.get_grouped); DT_DEBUG_SQLITE3_RESET(darktable.view_manager->statements.get_grouped); DT_DEBUG_SQLITE3_BIND_INT(darktable.view_manager->statements.get_grouped, 1, imgid); DT_DEBUG_SQLITE3_BIND_INT(darktable.view_manager->statements.get_grouped, 2, imgid); /* lets check if imgid is in a group */ if(sqlite3_step(darktable.view_manager->statements.get_grouped) == SQLITE_ROW) is_grouped = 1; else if(img && darktable.gui->expanded_group_id == img->group_id) darktable.gui->expanded_group_id = -1; #endif if(selected == 1) { outlinecol = 0.4; bgcol = 0.6; fontcol = 0.5; } if(imgsel == imgid) { bgcol = 0.8; // mouse over fontcol = 0.7; outlinecol = 0.6; // if the user points at this image, we really want it: if(!img) img = dt_image_cache_read_get(darktable.image_cache, imgid); } float imgwd = 0.90f; if(zoom == 1) { imgwd = .97f; // cairo_set_antialias(cr, CAIRO_ANTIALIAS_NONE); } else { double x0 = 1, y0 = 1, rect_width = width-2, rect_height = height-2, radius = 5; double x1, y1, off, off1; x1=x0+rect_width; y1=y0+rect_height; off=radius*0.666; off1 = radius-off; cairo_move_to (cr, x0, y0 + radius); cairo_curve_to (cr, x0, y0+off1, x0+off1 , y0, x0 + radius, y0); cairo_line_to (cr, x1 - radius, y0); cairo_curve_to (cr, x1-off1, y0, x1, y0+off1, x1, y0 + radius); cairo_line_to (cr, x1 , y1 - radius); cairo_curve_to (cr, x1, y1-off1, x1-off1, y1, x1 - radius, y1); cairo_line_to (cr, x0 + radius, y1); cairo_curve_to (cr, x0+off1, y1, x0, y1-off1, x0, y1- radius); cairo_close_path (cr); cairo_set_source_rgb(cr, bgcol, bgcol, bgcol); cairo_fill_preserve(cr); cairo_set_line_width(cr, 0.005*width); cairo_set_source_rgb(cr, outlinecol, outlinecol, outlinecol); cairo_stroke(cr); if(img) { const char *ext = img->filename + strlen(img->filename); while(ext > img->filename && *ext != '.') ext--; ext++; cairo_set_source_rgb(cr, fontcol, fontcol, fontcol); cairo_select_font_face (cr, "sans-serif", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_BOLD); cairo_set_font_size (cr, .25*width); cairo_text_extents_t text_extends; cairo_text_extents (cr, ext, &text_extends); cairo_move_to (cr, .025*width - text_extends.x_bearing, .24*height); cairo_show_text (cr, ext); } } dt_mipmap_buffer_t buf; dt_mipmap_size_t mip = dt_mipmap_cache_get_matching_size( darktable.mipmap_cache, imgwd*width, imgwd*height); dt_mipmap_cache_read_get( darktable.mipmap_cache, &buf, imgid, mip, 0); #if DRAW_THUMB == 1 float scale = 1.0; // decompress image, if necessary. if compression is off, scratchmem will be == NULL, // so get the real pointer back: uint8_t *buf_decompressed = dt_mipmap_cache_decompress(&buf, scratchmem); cairo_surface_t *surface = NULL; if(buf.buf) { const int32_t stride = cairo_format_stride_for_width (CAIRO_FORMAT_RGB24, buf.width); surface = cairo_image_surface_create_for_data (buf_decompressed, CAIRO_FORMAT_RGB24, buf.width, buf.height, stride); if(zoom == 1) { scale = fminf( fminf(darktable.thumbnail_width, width) / (float)buf.width, fminf(darktable.thumbnail_height, height) / (float)buf.height ); } else scale = fminf(width*imgwd/(float)buf.width, height*imgwd/(float)buf.height); } // draw centered and fitted: cairo_save(cr); cairo_translate(cr, width/2.0, height/2.0f); cairo_scale(cr, scale, scale); if(buf.buf) { cairo_translate(cr, -.5f*buf.width, -.5f*buf.height); cairo_set_source_surface (cr, surface, 0, 0); // set filter no nearest: // in skull mode, we want to see big pixels. // in 1 iir mode for the right mip, we want to see exactly what the pipe gave us, 1:1 pixel for pixel. // in between, filtering just makes stuff go unsharp. if((buf.width <= 8 && buf.height <= 8) || fabsf(scale - 1.0f) < 0.01f) cairo_pattern_set_filter(cairo_get_source(cr), CAIRO_FILTER_NEAREST); cairo_rectangle(cr, 0, 0, buf.width, buf.height); cairo_fill(cr); cairo_surface_destroy (surface); cairo_rectangle(cr, 0, 0, buf.width, buf.height); } // border around image const float border = zoom == 1 ? 16/scale : 2/scale; cairo_set_source_rgb(cr, bordercol, bordercol, bordercol); if(buf.buf && selected) { cairo_set_line_width(cr, 1./scale); if(zoom == 1) { // draw shadow around border cairo_set_source_rgb(cr, 0.2, 0.2, 0.2); cairo_stroke(cr); // cairo_new_path(cr); cairo_set_fill_rule (cr, CAIRO_FILL_RULE_EVEN_ODD); float alpha = 1.0f; for(int k=0; k<16; k++) { cairo_rectangle(cr, 0, 0, buf.width, buf.height); cairo_new_sub_path(cr); cairo_rectangle(cr, -k/scale, -k/scale, buf.width+2.*k/scale, buf.height+2.*k/scale); cairo_set_source_rgba(cr, 0, 0, 0, alpha); alpha *= 0.6f; cairo_fill(cr); } } else { cairo_set_fill_rule (cr, CAIRO_FILL_RULE_EVEN_ODD); cairo_new_sub_path(cr); cairo_rectangle(cr, -border, -border, buf.width+2.*border, buf.height+2.*border); cairo_stroke_preserve(cr); cairo_set_source_rgb(cr, 1.0-bordercol, 1.0-bordercol, 1.0-bordercol); cairo_fill(cr); } } else if(buf.buf) { cairo_set_line_width(cr, 1); cairo_stroke(cr); } cairo_restore(cr); #endif if(buf.buf) dt_mipmap_cache_read_release(darktable.mipmap_cache, &buf); const float fscale = fminf(width, height); if(imgsel == imgid) { // draw mouseover hover effects, set event hook for mouse button down! *image_over = DT_VIEW_DESERT; cairo_set_line_width(cr, 1.5); cairo_set_source_rgb(cr, outlinecol, outlinecol, outlinecol); cairo_set_line_join (cr, CAIRO_LINE_JOIN_ROUND); float r1, r2; if(zoom != 1) { r1 = 0.05*width; r2 = 0.022*width; } else { r1 = 0.015*fscale; r2 = 0.007*fscale; } float x, y; if(zoom != 1) y = 0.90*height; else y = .12*fscale; gboolean image_is_rejected = (img && ((img->flags & 0x7) == 6)); if(img) for(int k=0; k<5; k++) { if(zoom != 1) x = (0.41+k*0.12)*width; else x = (.08+k*0.04)*fscale; if(!image_is_rejected) //if rejected: draw no stars { dt_view_star(cr, x, y, r1, r2); if((px - x)*(px - x) + (py - y)*(py - y) < r1*r1) { *image_over = DT_VIEW_STAR_1 + k; cairo_fill(cr); } else if((img->flags & 0x7) > k) { cairo_fill_preserve(cr); cairo_set_source_rgb(cr, 1.0-bordercol, 1.0-bordercol, 1.0-bordercol); cairo_stroke(cr); cairo_set_source_rgb(cr, outlinecol, outlinecol, outlinecol); } else cairo_stroke(cr); } } //Image rejected? if(zoom !=1) x = 0.11*width; else x = .04*fscale; if (image_is_rejected) cairo_set_source_rgb(cr, 1., 0., 0.); if((px - x)*(px - x) + (py - y)*(py - y) < r1*r1) { *image_over = DT_VIEW_REJECT; //mouse sensitive cairo_new_sub_path(cr); cairo_arc(cr, x, y, (r1+r2)*.5, 0, 2.0f*M_PI); cairo_stroke(cr); } if (image_is_rejected) cairo_set_line_width(cr, 2.5); //reject cross: cairo_move_to(cr, x-r2, y-r2); cairo_line_to(cr, x+r2, y+r2); cairo_move_to(cr, x+r2, y-r2); cairo_line_to(cr, x-r2, y+r2); cairo_close_path(cr); cairo_stroke(cr); cairo_set_source_rgb(cr, outlinecol, outlinecol, outlinecol); cairo_set_line_width(cr, 1.5); // image part of a group? if(is_grouped && darktable.gui && darktable.gui->grouping) { // draw grouping icon and border if the current group is expanded // align to the right, left of altered float s = (r1+r2)*.75; float _x, _y; if(zoom != 1) { _x = width*0.9 - s*2.5; _y = height*0.1 - s*.4; } else { _x = (.04+7*0.04-1.1*.04)*fscale; _y = y - (.17*.04)*fscale; } cairo_save(cr); if(img && (imgid != img->group_id)) cairo_set_source_rgb(cr, fontcol, fontcol, fontcol); dtgtk_cairo_paint_grouping(cr, _x, _y, s, s, 23); cairo_restore(cr); // mouse is over the grouping icon if(img && abs(px-_x-.5*s) <= .8*s && abs(py-_y-.5*s) <= .8*s) *image_over = DT_VIEW_GROUP; } // image altered? if(altered) { // align to right float s = (r1+r2)*.5; if(zoom != 1) { x = width*0.9; y = height*0.1; } else x = (.04+7*0.04)*fscale; dt_view_draw_altered(cr, x, y, s); //g_print("px = %d, x = %.4f, py = %d, y = %.4f\n", px, x, py, y); if(img && abs(px-x) <= 1.2*s && abs(py-y) <= 1.2*s) // mouse hovers over the altered-icon -> history tooltip! { darktable.gui->center_tooltip = 1; } } } // kill all paths, in case img was not loaded yet, or is blocked: cairo_new_path(cr); #if DRAW_COLORLABELS == 1 // TODO: make mouse sensitive, just as stars! // TODO: cache in image struct! { // color labels: const float x = zoom == 1 ? (0.07)*fscale : .21*width; const float y = zoom == 1 ? 0.17*fscale: 0.1*height; const float r = zoom == 1 ? 0.01*fscale : 0.03*width; /* clear and reset prepared statement */ DT_DEBUG_SQLITE3_CLEAR_BINDINGS(darktable.view_manager->statements.get_color); DT_DEBUG_SQLITE3_RESET(darktable.view_manager->statements.get_color); /* setup statement and iterate rows */ DT_DEBUG_SQLITE3_BIND_INT(darktable.view_manager->statements.get_color, 1, imgid); while(sqlite3_step(darktable.view_manager->statements.get_color) == SQLITE_ROW) { cairo_save(cr); int col = sqlite3_column_int(darktable.view_manager->statements.get_color, 0); // see src/dtgtk/paint.c dtgtk_cairo_paint_label(cr, x+(3*r*col)-5*r, y-r, r*2, r*2, col); cairo_restore(cr); } } #endif if(img && (zoom == 1)) { // some exif data cairo_set_source_rgb(cr, .7, .7, .7); cairo_select_font_face (cr, "sans-serif", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_BOLD); cairo_set_font_size (cr, .025*fscale); cairo_move_to (cr, .02*fscale, .04*fscale); // cairo_show_text(cr, img->filename); cairo_text_path(cr, img->filename); char exifline[50]; cairo_move_to (cr, .02*fscale, .08*fscale); dt_image_print_exif(img, exifline, 50); cairo_text_path(cr, exifline); cairo_fill_preserve(cr); cairo_set_line_width(cr, 1.0); cairo_set_source_rgb(cr, 0.3, 0.3, 0.3); cairo_stroke(cr); } if(img) dt_image_cache_read_release(darktable.image_cache, img); cairo_restore(cr); // if(zoom == 1) cairo_set_antialias(cr, CAIRO_ANTIALIAS_DEFAULT); const double end = dt_get_wtime(); dt_print(DT_DEBUG_PERF, "[lighttable] image expose took %0.04f sec\n", end-start); }
// *** Compare bool Vector2::Compare(Vector2 const &b) const { return ( (fabsf(x - b.x) < FLT_EPSILON) && (fabsf(y - b.y) < FLT_EPSILON) ); }
// updates the internals of a mapped particle void CMappedParticle::Update( float flTimeSinceLastDraw ) { flParticleTime += flTimeSinceLastDraw; // limit to at least 1 sort per second as 0 sorts can cause problems if (g_ParticleSorts->value == 0) g_ParticleSorts->value = 1; // update distance between particle and player g_ParticleSorts times a second float flTimeSinceLastSort = (gEngfuncs.GetClientTime() - sParticle.flLastSort); if(flTimeSinceLastSort == 0 || (((int)(1 / flTimeSinceLastSort)) < g_ParticleSorts->value)) { // update lightlevel everytime we sort // if (pSys->iParticleLightCheck == NO_CHECK) if (pSys->iParticleLightCheck == CHECK_EVERY_SORT) { // great thanks go to Sneaky_Bastard and randomnine for helping me alot to finding // this great method of obtaining lightlevel on the clientside // create temporary entity to get illumination from it alight_t lighting; cl_entity_t *LightEntity; vec3_t dir; // bogus data as valve calls it :) LightEntity = gEngfuncs.GetLocalPlayer(); if ( !LightEntity ) return; // move it in the particles location LightEntity->origin = sParticle.vPosition; // I have no idea what this does but if you don't do it -> crash lighting.plightvec = dir; IEngineStudio.StudioDynamicLight(LightEntity, &lighting ); IEngineStudio.StudioSetupLighting (&lighting); IEngineStudio.StudioEntityLight( &lighting ); sParticle.iRed = (int)(lighting.color[0] * lighting.shadelight ); sParticle.iGreen = (int)(lighting.color[1] * lighting.shadelight ); sParticle.iBlue = (int)(lighting.color[2] * lighting.shadelight ); } } // dont draw the particle if smoke is enabled and its close to the player if (pSys->bSmoke == true) { if (sParticle.flSquareDistanceToPlayer < (PARTICLE_THRESHOLD_START*PARTICLE_THRESHOLD_START)) bIgnoreParticle = true; else bIgnoreParticle = false; } if (pSys->iParticleAlign == NO_ALIGN) { vNormal.x += flTimeSinceLastDraw * (pSys->vRotationVel.x + vRotationVelVar.x); vNormal.y += flTimeSinceLastDraw * (pSys->vRotationVel.y + vRotationVelVar.y); vNormal.z += flTimeSinceLastDraw * (pSys->vRotationVel.z + vRotationVelVar.z); while (vNormal.x > 360) vNormal.x -= 360; while (vNormal.y > 360) vNormal.y -= 360; while (vNormal.z > 360) vNormal.z -= 360; } // if we're using a custom mode for displaying our texture if (pSys->iFPS != 0) { if (pSys->iAnimSpeed == CUSTOM) { if (pSys->iAnimBehaviour == LOOP) { while (flParticleTime > (1.0f / pSys->iFPS)) { iCurrentFrame++; flParticleTime -= (1.0f / pSys->iFPS); } if (iCurrentFrame > (pSys->iEndingFrame - 1)) iCurrentFrame = (pSys->iStartingFrame - 1); } else if (pSys->iAnimBehaviour == REVERSE_LOOP) { bool bCountUp = true; if (iCurrentFrame >= (pSys->iEndingFrame - 1)) bCountUp = false; if (iCurrentFrame <= (pSys->iStartingFrame - 1)) bCountUp = true; while (flParticleTime > (1.0f / pSys->iFPS)) { if (bCountUp == true) iCurrentFrame++; else iCurrentFrame--; flParticleTime -= (1.0f / pSys->iFPS); } } else { // once_through while (flParticleTime > (1.0f / pSys->iFPS)) { iCurrentFrame++; flParticleTime -= (1.0f / pSys->iFPS); } if (iCurrentFrame > (pSys->iEndingFrame - 1)) iCurrentFrame = (pSys->iEndingFrame - 1); } } } if (pSys->iAnimSpeed == START_FAST_END_SLOW) iCurrentFrame = pSys->iEndingFrame * (1 - exp(-3*sParticle.flAge)); if (pSys->iAnimSpeed == ANIMATE_OVER_LIFE && sParticle.flMaxAge) iCurrentFrame = (pSys->iEndingFrame) * (sParticle.flAge / sParticle.flMaxAge); if (flTimeSinceLastDraw < 0) // how the hell can the time between updates be less than nothing? flTimeSinceLastDraw = -flTimeSinceLastDraw; if (pSys->flVelocityDampening > 0.01) { sParticle.vVelocity.x *= ( 1 / ( 1 + fabsf(pSys->flVelocityDampening * flTimeSinceLastDraw * sParticle.vVelocity.x))); sParticle.vVelocity.y *= ( 1 / ( 1 + fabsf(pSys->flVelocityDampening * flTimeSinceLastDraw * sParticle.vVelocity.y))); sParticle.vVelocity.z *= ( 1 / ( 1 + fabsf(pSys->flVelocityDampening * flTimeSinceLastDraw * sParticle.vVelocity.z))); } sParticle.vVelocity.z -= pSys->flGravity * flTimeSinceLastDraw; if (pSys->bWindy) { sParticle.vVelocity.x += (sParticle.vWind.x * flTimeSinceLastDraw); sParticle.vVelocity.y += (sParticle.vWind.y * flTimeSinceLastDraw); sParticle.vVelocity.z += (sParticle.vWind.z * flTimeSinceLastDraw); } VectorMA( sParticle.vPosition, (60.0 * flTimeSinceLastDraw), sParticle.vVelocity, sParticle.vPosition ); sParticle.flSize += flTimeSinceLastDraw * sParticle.flGrowth; sParticle.flCurrentRotation += flTimeSinceLastDraw * sParticle.flRotation; while (sParticle.flCurrentRotation > 360) { sParticle.flCurrentRotation -= 360; } sParticle.flAge += flTimeSinceLastDraw; }
void VariableSizeBokehBlurOperation::executePixel(float output[4], int x, int y, void *data) { VariableSizeBokehBlurTileData *tileData = (VariableSizeBokehBlurTileData *)data; MemoryBuffer *inputProgramBuffer = tileData->color; MemoryBuffer *inputBokehBuffer = tileData->bokeh; MemoryBuffer *inputSizeBuffer = tileData->size; float *inputSizeFloatBuffer = inputSizeBuffer->getBuffer(); float *inputProgramFloatBuffer = inputProgramBuffer->getBuffer(); float readColor[4]; float bokeh[4]; float tempSize[4]; float multiplier_accum[4]; float color_accum[4]; const float max_dim = max(m_width, m_height); const float scalar = this->m_do_size_scale ? (max_dim / 100.0f) : 1.0f; int maxBlurScalar = tileData->maxBlurScalar; BLI_assert(inputBokehBuffer->getWidth() == COM_BLUR_BOKEH_PIXELS); BLI_assert(inputBokehBuffer->getHeight() == COM_BLUR_BOKEH_PIXELS); #ifdef COM_DEFOCUS_SEARCH float search[4]; this->m_inputSearchProgram->read(search, x / InverseSearchRadiusOperation::DIVIDER, y / InverseSearchRadiusOperation::DIVIDER, NULL); int minx = search[0]; int miny = search[1]; int maxx = search[2]; int maxy = search[3]; #else int minx = max(x - maxBlurScalar, 0); int miny = max(y - maxBlurScalar, 0); int maxx = min(x + maxBlurScalar, (int)m_width); int maxy = min(y + maxBlurScalar, (int)m_height); #endif { inputSizeBuffer->readNoCheck(tempSize, x, y); inputProgramBuffer->readNoCheck(readColor, x, y); copy_v4_v4(color_accum, readColor); copy_v4_fl(multiplier_accum, 1.0f); float size_center = tempSize[0] * scalar; const int addXStepValue = QualityStepHelper::getStep(); const int addYStepValue = addXStepValue; const int addXStepColor = addXStepValue * COM_NUM_CHANNELS_COLOR; if (size_center > this->m_threshold) { for (int ny = miny; ny < maxy; ny += addYStepValue) { float dy = ny - y; int offsetValueNy = ny * inputSizeBuffer->getWidth(); int offsetValueNxNy = offsetValueNy + (minx); int offsetColorNxNy = offsetValueNxNy * COM_NUM_CHANNELS_COLOR; for (int nx = minx; nx < maxx; nx += addXStepValue) { if (nx != x || ny != y) { float size = min(inputSizeFloatBuffer[offsetValueNxNy] * scalar, size_center); if (size > this->m_threshold) { float dx = nx - x; if (size > fabsf(dx) && size > fabsf(dy)) { float uv[2] = { (float)(COM_BLUR_BOKEH_PIXELS / 2) + (dx / size) * (float)((COM_BLUR_BOKEH_PIXELS / 2) - 1), (float)(COM_BLUR_BOKEH_PIXELS / 2) + (dy / size) * (float)((COM_BLUR_BOKEH_PIXELS / 2) - 1)}; inputBokehBuffer->read(bokeh, uv[0], uv[1]); madd_v4_v4v4(color_accum, bokeh, &inputProgramFloatBuffer[offsetColorNxNy]); add_v4_v4(multiplier_accum, bokeh); } } } offsetColorNxNy += addXStepColor; offsetValueNxNy += addXStepValue; } } } output[0] = color_accum[0] / multiplier_accum[0]; output[1] = color_accum[1] / multiplier_accum[1]; output[2] = color_accum[2] / multiplier_accum[2]; output[3] = color_accum[3] / multiplier_accum[3]; /* blend in out values over the threshold, otherwise we get sharp, ugly transitions */ if ((size_center > this->m_threshold) && (size_center < this->m_threshold * 2.0f)) { /* factor from 0-1 */ float fac = (size_center - this->m_threshold) / this->m_threshold; interp_v4_v4v4(output, readColor, output, fac); } } }
/** Handles steering for a player kart. */ void PlayerController::steer(float dt, int steer_val) { if(UserConfigParams::m_gamepad_debug) { Log::debug("PlayerController", "steering: steer_val %d ", steer_val); RaceGUIBase* gui_base = World::getWorld()->getRaceGUI(); gui_base->clearAllMessages(); gui_base->addMessage(StringUtils::insertValues(L"steer_val %i", steer_val), m_kart, 1.0f, video::SColor(255, 255, 0, 255), false); } if(stk_config->m_disable_steer_while_unskid && m_controls->m_skid==KartControl::SC_NONE && m_kart->getSkidding()->getVisualSkidRotation()!=0) { m_controls->m_steer = 0; } // Amount the steering is changed for digital devices. // If the steering is 'back to straight', a different steering // change speed is used. const float STEER_CHANGE = ( (steer_val<=0 && m_controls->m_steer<0) || (steer_val>=0 && m_controls->m_steer>0) ) ? dt/m_kart->getKartProperties()->getTimeResetSteer() : dt/m_kart->getTimeFullSteer(fabsf(m_controls->m_steer)); if (steer_val < 0) { // If we got analog values do not cumulate. if (steer_val > -32767) m_controls->m_steer = -steer_val/32767.0f; else m_controls->m_steer += STEER_CHANGE; } else if(steer_val > 0) { // If we got analog values do not cumulate. if (steer_val < 32767) m_controls->m_steer = -steer_val/32767.0f; else m_controls->m_steer -= STEER_CHANGE; } else { // no key is pressed if(m_controls->m_steer>0.0f) { m_controls->m_steer -= STEER_CHANGE; if(m_controls->m_steer<0.0f) m_controls->m_steer=0.0f; } else { // m_controls->m_steer<=0.0f; m_controls->m_steer += STEER_CHANGE; if(m_controls->m_steer>0.0f) m_controls->m_steer=0.0f; } // if m_controls->m_steer<=0.0f } // no key is pressed if(UserConfigParams::m_gamepad_debug) { Log::debug("PlayerController", " set to: %f\n", m_controls->m_steer); } m_controls->m_steer = std::min(1.0f, std::max(-1.0f, m_controls->m_steer)); } // steer
int main(int argc, char** argv) { int iter_max = 1000; const float pi = 2.0 * asinf(1.0f); const float tol = 1.0e-5f; int rank = 0; int size = 1; MPI_Init(&argc, &argv); MPI_Comm_rank(MPI_COMM_WORLD, &rank); MPI_Comm_size(MPI_COMM_WORLD, &size); memset(A, 0, N * M * sizeof(float)); memset(Aref, 0, N * M * sizeof(float)); // set boundary conditions for (int j = 0; j < N; j++) { float y0 = sinf( 2.0 * pi * j / (N-1)); A[j][0] = y0; A[j][M-1] = y0; Aref[j][0] = y0; Aref[j][M-1] = y0; } #if _OPENACC int ngpus=acc_get_num_devices(acc_device_nvidia); int devicenum=rank%ngpus; acc_set_device_num(devicenum,acc_device_nvidia); // Call acc_init after acc_set_device_num to avoid multiple contexts on device 0 in multi GPU systems acc_init(acc_device_nvidia); #endif /*_OPENACC*/ // Ensure correctness if N%size != 0 int chunk_size = ceil( (1.0*N)/size ); int jstart = rank * chunk_size; int jend = jstart + chunk_size; // Do not process boundaries jstart = max( jstart, 1 ); jend = min( jend, N - 1 ); if ( rank == 0) printf("Jacobi relaxation Calculation: %d x %d mesh\n", N, M); if ( rank == 0) printf("Calculate reference solution and time serial execution.\n"); StartTimer(); laplace2d_serial( rank, iter_max, tol ); double runtime_serial = GetTimer(); //Wait for all processes to ensure correct timing of the parallel version MPI_Barrier( MPI_COMM_WORLD ); if ( rank == 0) printf("Parallel execution.\n"); StartTimer(); int iter = 0; float error = 1.0f; #pragma acc data copy(A) create(Anew) while ( error > tol && iter < iter_max ) { error = 0.f; #pragma acc kernels for (int j = jstart; j < jend; j++) { for( int i = 1; i < M-1; i++ ) { Anew[j][i] = 0.25f * ( A[j][i+1] + A[j][i-1] + A[j-1][i] + A[j+1][i]); error = fmaxf( error, fabsf(Anew[j][i]-A[j][i])); } } float globalerror = 0.0f; MPI_Allreduce( &error, &globalerror, 1, MPI_FLOAT, MPI_MAX, MPI_COMM_WORLD ); error = globalerror; #pragma acc kernels for (int j = jstart; j < jend; j++) { for( int i = 1; i < M-1; i++ ) { A[j][i] = Anew[j][i]; } } //Periodic boundary conditions int top = (rank == 0) ? (size-1) : rank-1; int bottom = (rank == (size-1)) ? 0 : rank+1; #pragma acc host_data use_device( A ) { //1. Sent row jstart (first modified row) to top receive lower boundary (jend) from bottom MPI_Sendrecv( A[jstart], M, MPI_FLOAT, top , 0, A[jend], M, MPI_FLOAT, bottom, 0, MPI_COMM_WORLD, MPI_STATUS_IGNORE ); //2. Sent row (jend-1) (last modified row) to bottom receive upper boundary (jstart-1) from top MPI_Sendrecv( A[(jend-1)], M, MPI_FLOAT, bottom, 0, A[(jstart-1)], M, MPI_FLOAT, top , 0, MPI_COMM_WORLD, MPI_STATUS_IGNORE ); } if(rank == 0 && (iter % 100) == 0) printf("%5d, %0.6f\n", iter, error); iter++; } MPI_Barrier( MPI_COMM_WORLD ); double runtime = GetTimer(); if (check_results( rank, jstart, jend, tol ) && rank == 0) { printf( "Num GPUs: %d\n", size ); printf( "%dx%d: 1 GPU: %8.4f s, %d GPUs: %8.4f s, speedup: %8.2f, efficiency: %8.2f%\n", N,M, runtime_serial/ 1000.f, size, runtime/ 1000.f, runtime_serial/runtime, runtime_serial/(size*runtime)*100 ); } MPI_Finalize(); return 0; }
__global__ void FloatMathPrecise() { int iX; float fX, fY; acosf(1.0f); acoshf(1.0f); asinf(0.0f); asinhf(0.0f); atan2f(0.0f, 1.0f); atanf(0.0f); atanhf(0.0f); cbrtf(0.0f); fX = ceilf(0.0f); fX = copysignf(1.0f, -2.0f); cosf(0.0f); coshf(0.0f); cospif(0.0f); cyl_bessel_i0f(0.0f); cyl_bessel_i1f(0.0f); erfcf(0.0f); erfcinvf(2.0f); erfcxf(0.0f); erff(0.0f); erfinvf(1.0f); exp10f(0.0f); exp2f(0.0f); expf(0.0f); expm1f(0.0f); fX = fabsf(1.0f); fdimf(1.0f, 0.0f); fdividef(0.0f, 1.0f); fX = floorf(0.0f); fmaf(1.0f, 2.0f, 3.0f); fX = fmaxf(0.0f, 0.0f); fX = fminf(0.0f, 0.0f); fmodf(0.0f, 1.0f); frexpf(0.0f, &iX); hypotf(1.0f, 0.0f); ilogbf(1.0f); isfinite(0.0f); fX = isinf(0.0f); fX = isnan(0.0f); j0f(0.0f); j1f(0.0f); jnf(-1.0f, 1.0f); ldexpf(0.0f, 0); lgammaf(1.0f); llrintf(0.0f); llroundf(0.0f); log10f(1.0f); log1pf(-1.0f); log2f(1.0f); logbf(1.0f); logf(1.0f); lrintf(0.0f); lroundf(0.0f); modff(0.0f, &fX); fX = nanf("1"); fX = nearbyintf(0.0f); nextafterf(0.0f, 0.0f); norm3df(1.0f, 0.0f, 0.0f); norm4df(1.0f, 0.0f, 0.0f, 0.0f); normcdff(0.0f); normcdfinvf(1.0f); fX = 1.0f; normf(1, &fX); powf(1.0f, 0.0f); rcbrtf(1.0f); remainderf(2.0f, 1.0f); remquof(1.0f, 2.0f, &iX); rhypotf(0.0f, 1.0f); fY = rintf(1.0f); rnorm3df(0.0f, 0.0f, 1.0f); rnorm4df(0.0f, 0.0f, 0.0f, 1.0f); fX = 1.0f; rnormf(1, &fX); fY = roundf(0.0f); rsqrtf(1.0f); scalblnf(0.0f, 1); scalbnf(0.0f, 1); signbit(1.0f); sincosf(0.0f, &fX, &fY); sincospif(0.0f, &fX, &fY); sinf(0.0f); sinhf(0.0f); sinpif(0.0f); sqrtf(0.0f); tanf(0.0f); tanhf(0.0f); tgammaf(2.0f); fY = truncf(0.0f); y0f(1.0f); y1f(1.0f); ynf(1, 1.0f); }
// Detect if we are in flight or on ground void NavEKF2_core::detectFlight() { /* If we are a fly forward type vehicle (eg plane), then in-air status can be determined through a combination of speed and height criteria. Because of the differing certainty requirements of algorithms that need the in-flight / on-ground status we use two booleans where onGround indicates a high certainty we are not flying and inFlight indicates a high certainty we are flying. It is possible for both onGround and inFlight to be false if the status is uncertain, but they cannot both be true. If we are a plane as indicated by the assume_zero_sideslip() status, then different logic is used TODO - this logic should be moved out of the EKF and into the flight vehicle code. */ if (assume_zero_sideslip()) { // To be confident we are in the air we use a criteria which combines arm status, ground speed, airspeed and height change float gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y); bool highGndSpd = false; bool highAirSpd = false; bool largeHgtChange = false; // trigger at 8 m/s airspeed if (_ahrs->airspeed_sensor_enabled()) { const AP_Airspeed *airspeed = _ahrs->get_airspeed(); if (airspeed->get_airspeed() * airspeed->get_EAS2TAS() > 10.0f) { highAirSpd = true; } } // trigger at 10 m/s GPS velocity, but not if GPS is reporting bad velocity errors if (gndSpdSq > 100.0f && gpsSpdAccuracy < 1.0f) { highGndSpd = true; } // trigger if more than 10m away from initial height if (fabsf(hgtMea) > 10.0f) { largeHgtChange = true; } // Determine to a high certainty we are flying if (motorsArmed && highGndSpd && (highAirSpd || largeHgtChange)) { onGround = false; inFlight = true; } // if is possible we are in flight, set the time this condition was last detected if (motorsArmed && (highGndSpd || highAirSpd || largeHgtChange)) { airborneDetectTime_ms = imuSampleTime_ms; onGround = false; } // Determine to a high certainty we are not flying // after 5 seconds of not detecting a possible flight condition or we are disarmed, we transition to on-ground mode if(!motorsArmed || ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) { onGround = true; inFlight = false; } } else { // Non fly forward vehicle, so can only use height and motor arm status // If the motors are armed then we could be flying and if they are not armed then we are definitely not flying if (motorsArmed) { onGround = false; } else { inFlight = false; onGround = true; } if (!onGround) { // If height has increased since exiting on-ground, then we definitely are flying if ((stateStruct.position.z - posDownAtTakeoff) < -1.5f) { inFlight = true; } // If rangefinder has increased since exiting on-ground, then we definitely are flying if ((rangeDataNew.rng - rngAtStartOfFlight) > 0.5f) { inFlight = true; } // If more than 5 seconds since likely_flying was set // true, then set inFlight true if (_ahrs->get_time_flying_ms() > 5000) { inFlight = true; } } } // store current on-ground and in-air status for next time prevOnGround = onGround; prevInFlight = inFlight; // Store vehicle height and range prior to takeoff for use in post takeoff checks if (onGround) { // store vertical position at start of flight to use as a reference for ground relative checks posDownAtTakeoff = stateStruct.position.z; // store the range finder measurement which will be used as a reference to detect when we have taken off rngAtStartOfFlight = rangeDataNew.rng; // if the magnetic field states have been set, then continue to update the vertical position // quaternion and yaw innovation snapshots to use as a reference when we start to fly. if (magStateInitComplete) { posDownAtLastMagReset = stateStruct.position.z; quatAtLastMagReset = stateStruct.quat; yawInnovAtLastMagReset = innovYaw; } } }
void xnAudioSourcePush3D(xnAudioSource* source, float* ppos, float* pforward, float* pup, float* pvel) { float4 pos; memcpy(&pos, ppos, sizeof(float) * 3); float4 forward; memcpy(&forward, pforward, sizeof(float) * 3); float4 up; memcpy(&up, pup, sizeof(float) * 3); float4 vel; memcpy(&vel, pvel, sizeof(float) * 3); #ifdef __clang__ //resharper does not know about opencl vectors // To evaluate the Doppler effect we calculate the distance to the listener from one wave to the next one and divide it by the sound speed // we use 343m/s for the sound speed which correspond to the sound speed in the air. // we use 600Hz for the sound frequency which correspond to the middle of the human hearable sounds frequencies. auto dopplerShift = 1.0f; auto vecListEmit = pos - source->listener->pos; auto distListEmit = npLengthF4(vecListEmit); // avoid useless calculations. if (!(vel.x == 0 && vel.y == 0 && vel.z == 0 && source->listener->velocity.x == 0 && source->listener->velocity.y == 0 && source->listener->velocity.z == 0)) { auto vecListEmitNorm = vecListEmit; if (distListEmit > ZeroTolerance) { auto inv = 1.0f / distListEmit; vecListEmitNorm *= inv; } auto vecListEmitSpeed = vel - source->listener->velocity; auto speedDot = vecListEmitSpeed[0] * vecListEmitNorm[0] + vecListEmitSpeed[1] * vecListEmitNorm[1] + vecListEmitSpeed[2] * vecListEmitNorm[2]; if (speedDot < -SoundSpeed) // emitter and listener are getting closer more quickly than the speed of the sound. { dopplerShift = MaxValue; //positive infinity } else { auto timeSinceLastWaveArrived = 0.0f; // time elapsed since the previous wave arrived to the listener. auto lastWaveDistToListener = 0.0f; // the distance that the last wave still have to travel to arrive to the listener. const auto DistLastWave = SoundPeriod * SoundSpeed; // distance traveled by the previous wave. if (DistLastWave > distListEmit) timeSinceLastWaveArrived = (DistLastWave - distListEmit) / SoundSpeed; else lastWaveDistToListener = distListEmit - DistLastWave; auto nextVecListEmit = vecListEmit + SoundPeriod * vecListEmitSpeed; auto nextWaveDistToListener = sqrtf(nextVecListEmit[0] * nextVecListEmit[0] + nextVecListEmit[1] * nextVecListEmit[1] + nextVecListEmit[2] * nextVecListEmit[2]); auto timeBetweenTwoWaves = timeSinceLastWaveArrived + (nextWaveDistToListener - lastWaveDistToListener) / SoundSpeed; auto apparentFrequency = 1 / timeBetweenTwoWaves; dopplerShift = apparentFrequency / SoundFreq; } } source->doppler_pitch = dopplerShift; auto pitch = source->pitch * dopplerShift; pitch = pitch > 4.0f ? 4.0f : pitch < -4.0f ? -4.0f : pitch; (*source->playRate)->SetRate(source->playRate, SLpermille(pitch * 1000.0f)); // After an analysis of the XAudio2 left/right stereo balance with respect to 3D world position, // it could be found the volume repartition is symmetric to the Up/Down and Front/Back planes. // Moreover the left/right repartition seems to follow a third degree polynomial function: // Volume_left(a) = 2(c-1)*a^3 - 3(c-1)*a^2 + c*a , where c is a constant close to c = 1.45f and a is the angle normalized bwt [0,1] // Volume_right(a) = 1-Volume_left(a) // As for signal attenuation wrt distance the model follows a simple inverse square law function as explained in XAudio2 documentation // ( http://msdn.microsoft.com/en-us/library/windows/desktop/microsoft.directx_sdk.x3daudio.x3daudio_emitter(v=vs.85).aspx ) // Volume(d) = 1 , if d <= ScaleDistance where d is the distance to the listener // Volume(d) = ScaleDistance / d , if d >= ScaleDistance where d is the distance to the listener auto attenuationFactor = distListEmit <= 1.0f ? 1.0f : 1.0f / distListEmit; // 2. Left/Right balance. auto repartRight = 0.5f; float4 rightVec = npCrossProductF4(source->listener->forward, source->listener->up); float4 worldToList[4]; npMatrixIdentityF4(worldToList); worldToList[0].x = rightVec.x; worldToList[0].y = source->listener->forward.x; worldToList[0].z = source->listener->up.x; worldToList[1].x = rightVec.y; worldToList[1].y = source->listener->forward.y; worldToList[1].z = source->listener->up.y; worldToList[2].x = rightVec.z; worldToList[2].y = source->listener->forward.z; worldToList[2].z = source->listener->up.z; auto vecListEmitListBase = npTransformNormalF4(vecListEmit, worldToList); auto vecListEmitListBaseLen = npLengthF4(vecListEmitListBase); if(vecListEmitListBaseLen > 0.0f) { const auto c = 1.45f; auto absAlpha = fabsf(atan2f(vecListEmitListBase.y, vecListEmitListBase.x)); auto normAlpha = absAlpha / (E_PI / 2.0f); if (absAlpha > E_PI / 2.0f) normAlpha = 2.0f - normAlpha; repartRight = 0.5f * (2 * (c - 1) * normAlpha * normAlpha * normAlpha - 3 * (c - 1) * normAlpha * normAlpha * normAlpha + c * normAlpha); if (absAlpha > E_PI / 2.0f) repartRight = 1 - repartRight; } xnAudioSourceSetPan(source, repartRight - 0.5); source->localizationGain = attenuationFactor; xnAudioSourceSetGain(source, source->gain); #endif }