File content as of revision 1:66d79e3ee3d1:

//INCLUDES
#include "Algorithm.h"

//R1Angle
float Algorithm::R1Angle(float th, float a)
{
    return -tan(PI/2-2*a+th)*(REFLECTOR1_DIST+REFLECTOR1_LENGTH*cos(a))+
    PANEL_HEIGHT-REFLECTOR1_HEIGHT-REFLECTOR1_LENGTH*sin(a);
}

//R2Angle
float Algorithm::R2Angle(float th, float a)
{
    return tan(PI/2-2*a+th)*(-REFLECTOR2_DIST-REFLECTOR2_LENGTH*cos(a))+
    PANEL_HEIGHT-REFLECTOR2_HEIGHT-REFLECTOR2_LENGTH*sin(a);
}

//calcAngle
float Algorithm::calcAngle(int n, float th)
{
    float err,ap,a,an,af,afa;
    //Initial conditions
    a = PI/4;
    an = -PI/4;
    
    th *= PI/180; //Convert to rad
    
    switch(n)
    {
        case 1:
        for(int i = 0;i < LOOP_LIMIT;i++)
        {
            ap = a - Algorithm::R1Angle(th,a)*(a-an)/(Algorithm::R1Angle(th,a)
                -Algorithm::R1Angle(th,an));
            an = a;
            a = ap;
            err = ((ap-an) > 0)?(ap-an):(an-ap);
            
            if(err <= ERR_LIMIT)
            {
                break;
            }
            af = ((ap+an)/2)*180/PI; //Convert to degrees
            af = (af <= REFLECTOR1_LOW)? REFLECTOR1_LOW:(af >= REFLECTOR1_HIGH)?
                REFLECTOR1_HIGH : af;
        }
        break;
        
        case 2:
        for(int i = 0;i < LOOP_LIMIT;i++)
        {
            ap = a - Algorithm::R2Angle(th,a)/((Algorithm::R2Angle(th,a)
                -Algorithm::R2Angle(th,an))/(a-an));
            an = a;
            a = ap;
            err = ((ap-an) > 0)?(ap-an):(an-ap);
            
            if(err <= ERR_LIMIT)
            {
                break;
            }
            af = ((ap+an)/2)*180/PI; //Convert to degrees
            af = (af <= REFLECTOR2_LOW)? REFLECTOR2_LOW:(af >= REFLECTOR2_HIGH)?
                REFLECTOR2_HIGH : af;
        }
        break;
    }
    
    
    return af; 
}