Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
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;
}
Exemplo n.º 4
0
// 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];
}
Exemplo n.º 5
0
__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;
}
Exemplo n.º 6
0
// 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);
    }
}
Exemplo n.º 7
0
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);
}
Exemplo n.º 8
0
//------------------------------------------------------------------------------
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;
}
Exemplo n.º 9
0
	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;
	}
Exemplo n.º 10
0
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_ */
Exemplo n.º 11
0
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();
	}
}
Exemplo n.º 14
0
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;
}
Exemplo n.º 15
0
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);
}
Exemplo n.º 16
0
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);
}
Exemplo n.º 17
0
// 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++;
}
Exemplo n.º 18
0
// 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;
}
Exemplo n.º 19
0
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;
}
Exemplo n.º 20
0
// 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;
}
Exemplo n.º 21
0
// 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    
 }
Exemplo n.º 22
0
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);
}
Exemplo n.º 23
0
// *** Compare
bool Vector2::Compare(Vector2 const &b) const
{
	return ( (fabsf(x - b.x) < FLT_EPSILON) &&
			 (fabsf(y - b.y) < FLT_EPSILON) );
}
Exemplo n.º 24
0
// 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);
		}
	}

}
Exemplo n.º 26
0
/** 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;
}
Exemplo n.º 28
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);
}
Exemplo n.º 29
0
// 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;
        }
    }

}
Exemplo n.º 30
0
		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
		}