Esempio n. 1
0
static void
pdf_write_line_cap_appearance(fz_context *ctx, fz_buffer *buf, fz_rect *rect,
		float x, float y, float dx, float dy, float w, int ic, pdf_obj *cap)
{
	if (cap == PDF_NAME(Square))
	{
		float r = fz_max(2.5f, w * 2.5f);
		fz_append_printf(ctx, buf, "%g %g %g %g re\n", x-r, y-r, r*2, r*2);
		fz_append_string(ctx, buf, ic ? "b\n" : "s\n");
		include_cap(rect, x, y, r);
	}
	else if (cap == PDF_NAME(Circle))
	{
		float r = fz_max(2.5f, w * 2.5f);
		float m = r * CIRCLE_MAGIC;
		fz_append_printf(ctx, buf, "%g %g m\n", x, y+r);
		fz_append_printf(ctx, buf, "%g %g %g %g %g %g c\n", x+m, y+r, x+r, y+m, x+r, y);
		fz_append_printf(ctx, buf, "%g %g %g %g %g %g c\n", x+r, y-m, x+m, y-r, x, y-r);
		fz_append_printf(ctx, buf, "%g %g %g %g %g %g c\n", x-m, y-r, x-r, y-m, x-r, y);
		fz_append_printf(ctx, buf, "%g %g %g %g %g %g c\n", x-r, y+m, x-m, y+r, x, y+r);
		fz_append_string(ctx, buf, ic ? "b\n" : "s\n");
		include_cap(rect, x, y, r);
	}
	else if (cap == PDF_NAME(Diamond))
	{
		float r = fz_max(2.5f, w * 2.5f);
		fz_append_printf(ctx, buf, "%g %g m\n", x, y+r);
		fz_append_printf(ctx, buf, "%g %g l\n", x+r, y);
		fz_append_printf(ctx, buf, "%g %g l\n", x, y-r);
		fz_append_printf(ctx, buf, "%g %g l\n", x-r, y);
		fz_append_string(ctx, buf, ic ? "b\n" : "s\n");
		include_cap(rect, x, y, r);
	}
	else if (cap == PDF_NAME(OpenArrow))
	{
		pdf_write_arrow_appearance(ctx, buf, rect, x, y, dx, dy, w);
		fz_append_string(ctx, buf, "S\n");
	}
	else if (cap == PDF_NAME(ClosedArrow))
	{
		pdf_write_arrow_appearance(ctx, buf, rect, x, y, dx, dy, w);
		fz_append_string(ctx, buf, ic ? "b\n" : "s\n");
	}
	/* PDF 1.5 */
	else if (cap == PDF_NAME(Butt))
	{
		float r = fz_max(3, w * 3);
		fz_point a = { x-dy*r, y+dx*r };
		fz_point b = { x+dy*r, y-dx*r };
		fz_append_printf(ctx, buf, "%g %g m\n", a.x, a.y);
		fz_append_printf(ctx, buf, "%g %g l\n", b.x, b.y);
		fz_append_string(ctx, buf, "S\n");
		*rect = fz_include_point_in_rect(*rect, a);
		*rect = fz_include_point_in_rect(*rect, b);
	}
	/* PDF 1.6 */
	else if (cap == PDF_NAME(ROpenArrow))
	{
		pdf_write_arrow_appearance(ctx, buf, rect, x, y, -dx, -dy, w);
		fz_append_string(ctx, buf, "S\n");
	}
	else if (cap == PDF_NAME(RClosedArrow))
	{
		pdf_write_arrow_appearance(ctx, buf, rect, x, y, -dx, -dy, w);
		fz_append_string(ctx, buf, ic ? "b\n" : "s\n");
	}
	else if (cap == PDF_NAME(Slash))
	{
		float r = fz_max(5, w * 5);
		float angle = atan2f(dy, dx) - (30 * FZ_PI / 180);
		fz_point a, b, v;
		v = rotate_vector(angle, 0, r);
		a = fz_make_point(x + v.x, y + v.y);
		v = rotate_vector(angle, 0, -r);
		b = fz_make_point(x + v.x, y + v.y);
		fz_append_printf(ctx, buf, "%g %g m\n", a.x, a.y);
		fz_append_printf(ctx, buf, "%g %g l\n", b.x, b.y);
		fz_append_string(ctx, buf, "S\n");
		*rect = fz_include_point_in_rect(*rect, a);
		*rect = fz_include_point_in_rect(*rect, b);
	}
}
void MAIN_PROG (uint16_t time)
{
	static uint16_t ticker = 0;
	
	if ((uint16_t)(ticker+time)== GetTicker()) 
	{
		ticker = GetTicker();
		
		//Encoder section
		ENC_PROCESSING (&Left);
		ENC_PROCESSING (&Right);
	
		Linear.velo_raw = (Left.velo_fb - Right.velo_fb)/ 2.0f;
		Smooth_filter (Linear.velo_raw,Linear.velo_filted,0.1);
		PID_VELO_RUN.feedback = Linear.velo_filted[0];
		PID_VELO_HOLD.feedback = Linear.velo_filted[0];
		
		Linear.posi_raw = (Left.posi_fb - Right.posi_fb)/ 2.0f;
		Smooth_filter (Linear.posi_raw,Linear.posi_filted,0.1);
		PID_POSI.feedback = Linear.posi_filted[0];
		
		/*MPU 1*/
		/* Read all data */
		MPU6050_Status = TM_MPU6050_ReadAll(&MPU6050_Data0);
		
		//Pitch
		/* Complementary filter*/
		Pitch.raw = (0.98f * (Pitch.raw - MPU6050_Data0.Gyroscope_Y * dt)) + (0.02f * (atan2f(MPU6050_Data0.Accelerometer_X, MPU6050_Data0.Accelerometer_Z) * 180.0f/PI));
		Smooth_filter (Pitch.raw,Pitch.filted_theta,0.25);
		PID_TILT.feedback = Pitch.filted_theta[0] - Pitch.theta_offset;
		
		//Yaw
//		Yaw.raw = - MPU6050_Data0.Gyroscope_Z;
//		/* HighPass filter*/
//		Yaw.filted_theta[0] = HPF(Yaw.raw,10,f_s);
//		PID_TURN.feedback = Yaw.filted_theta[0] - Yaw.theta_offset;
		
	
		
		if ((PID_TILT.feedback < 45) && (PID_TILT.feedback > -45))
		{
			//Preprocessing
			if ((setspeed-PID_VELO_RUN.set_point) > 0)
			{
				PID_VELO_RUN.set_point += 0.5f;
			}
			
			if ((setspeed-PID_VELO_RUN.set_point) < 0)
			{
				PID_VELO_RUN.set_point -= 0.5f;
			}
			
			if (PID_VELO_RUN.set_point == 0)
			{
				//PID POSITION
				PID (&PID_POSI,0.34);
				PID_VELO_HOLD.set_point = PID_POSI.filted_U[0] * MAX_VELO;
				
				//PID VELOCITY
				PID (&PID_VELO_HOLD,0.34);
				PID_TILT.set_point = -PID_VELO_HOLD.filted_U[0] * MAX_TILT;
			}
			else
			{
				//PID VELOCITY
				PID (&PID_VELO_RUN,0.34);
				PID_TILT.set_point = -PID_VELO_RUN.filted_U[0] * MAX_TILT;
			}
			
			//PID TILTING	
			PID (&PID_TILT,0.36);
			
			//Turning	
			//PID (&PID_TURN,0.12);
			U_offset = -setturn*TURN_OFFSET;
			
			//FUSION 
			Left_motor.PWM  = PID_TILT.filted_U[0]  + U_offset;
			Right_motor.PWM = PID_TILT.filted_U[0]  - U_offset;
				
			//PWM section
			PWM (&Left_motor);
			PWM (&Right_motor);
		}
		else
		{			
			TIM_SetCompare1(TIM4,0);
			TIM_SetCompare2(TIM4,0);
			TIM_SetCompare3(TIM4,0);
			TIM_SetCompare4(TIM4,0);
			GPIO_ResetBits(GPIOD,GPIO_Pin_12);
			GPIO_ResetBits(GPIOD,GPIO_Pin_13);
			GPIO_ResetBits(GPIOD,GPIO_Pin_14);
			GPIO_ResetBits(GPIOD,GPIO_Pin_15);
			ResetPID (&PID_TILT);
			ResetPID (&PID_VELO_HOLD);
			ResetPID (&PID_VELO_RUN);
			ResetPID (&PID_POSI);
		}
	}
}
Esempio n. 3
0
// Draws a line between (x1,y1) - (x2,y2) with a start thickness of t1
void DrawGLThickLine( float x1, float y1, float x2, float y2, wxPen pen, bool b_hiqual )
{
#ifdef ocpnUSE_GL
    
    float angle = atan2f( y2 - y1, x2 - x1 );
    float t1 = pen.GetWidth();
    float t2sina1 = t1 / 2 * sinf( angle );
    float t2cosa1 = t1 / 2 * cosf( angle );

    glBegin( GL_TRIANGLES );

    //    n.b.  The dwxDash interpretation for GL only allows for 2 elements in the dash table.
    //    The first is assumed drawn, second is assumed space
    wxDash *dashes;
    int n_dashes = pen.GetDashes( &dashes );
    if( n_dashes ) {
        float lpix = sqrtf( powf( (float) (x1 - x2), 2) + powf( (float) (y1 - y2), 2) );
        float lrun = 0.;
        float xa = x1;
        float ya = y1;
        float ldraw = t1 * dashes[0];
        float lspace = t1 * dashes[1];

        while( lrun < lpix ) {
            //    Dash
            float xb = xa + ldraw * cosf( angle );
            float yb = ya + ldraw * sinf( angle );

            if( ( lrun + ldraw ) >= lpix )         // last segment is partial draw
            {
                xb = x2;
                yb = y2;
            }

            glVertex2f( xa + t2sina1, ya - t2cosa1 );
            glVertex2f( xb + t2sina1, yb - t2cosa1 );
            glVertex2f( xb - t2sina1, yb + t2cosa1 );

            glVertex2f( xb - t2sina1, yb + t2cosa1 );
            glVertex2f( xa - t2sina1, ya + t2cosa1 );
            glVertex2f( xa + t2sina1, ya - t2cosa1 );

            xa = xb;
            ya = yb;
            lrun += ldraw;

            //    Space
            xb = xa + lspace * cos( angle );
            yb = ya + lspace * sin( angle );

            xa = xb;
            ya = yb;
            lrun += lspace;
        }
    } else {
        glVertex2f( x1 + t2sina1, y1 - t2cosa1 );
        glVertex2f( x2 + t2sina1, y2 - t2cosa1 );
        glVertex2f( x2 - t2sina1, y2 + t2cosa1 );

        glVertex2f( x2 - t2sina1, y2 + t2cosa1 );
        glVertex2f( x1 - t2sina1, y1 + t2cosa1 );
        glVertex2f( x1 + t2sina1, y1 - t2cosa1 );

        /* wx draws a nice rounded end in dc mode, so replicate
           this for opengl mode, should this be done for the dashed mode case? */
        if(pen.GetCap() == wxCAP_ROUND) {
            DrawEndCap( x1, y1, t1, angle);
            DrawEndCap( x2, y2, t1, angle + M_PI);
        }

    }

    glEnd();
#endif    
}
Esempio n. 4
0
static BOOL ProcessData(void)
{
    // Loop through any new incoming packets
    while (OrionCommReceive(&PktIn))
    {
        // If this is a response to the request we just sent
        if (PktIn.ID == getOrionCamerasPacketID())
        {
            OrionCameras_t Cameras;

            // If the cameras packet decodes properly
            if (decodeOrionCamerasPacketStructure(&PktIn, &Cameras))
            {
                int i;

                // Print a header row to stdout
                printf(" Index  Type     Zoom  WFOV  NFOV\n");
                printf("----------------------------------\n");

                // Loop through each camera in the array
                for (i = 0; i < Cameras.NumCameras; i++)
                {
                    OrionCamSettings_t *pSettings = &Cameras.OrionCamSettings[i];
                    float ArraySize = pSettings->PixelPitch * pSettings->ArrayWidth;
                    float Zoom = 1.0f, Wfov, Nfov;
                    char TypeString[16];

                    // If this camera doesn't exist, skip it
                    if (pSettings->Type == CAMERA_TYPE_NONE)
                        continue;

                    // Build a type string based on the type enumeration
                    switch (pSettings->Type)
                    {
                    case CAMERA_TYPE_VISIBLE: strcpy(TypeString, "Visible"); break;
                    case CAMERA_TYPE_SWIR:    strcpy(TypeString, "SWIR");    break;
                    case CAMERA_TYPE_MWIR:    strcpy(TypeString, "MWIR");    break;
                    case CAMERA_TYPE_LWIR:    strcpy(TypeString, "LWIR");    break;
                    default:                  strcpy(TypeString, "Unknown"); break;
                    }

                    // Calculate max zoom ratio for use in OrionCameraState, avoiding (unlikely) divide by zero
                    if (pSettings->MinFocalLength > 0)
                        Zoom = pSettings->MaxFocalLength / pSettings->MinFocalLength;

                    // Compute wide and narrow horizontal FOV in radians
                    Wfov = atan2f(0.5f * ArraySize, pSettings->MinFocalLength) * 2.0f;
                    Nfov = atan2f(0.5f * ArraySize, pSettings->MaxFocalLength) * 2.0f;

                    // Print the index, type, max zoom, and min/max FOV in degrees for this camera
                    printf(" %5d  %-7s %5.1f %5.1f %5.1f\n", i, TypeString, Zoom, degreesf(Wfov), degreesf(Nfov));
                }

                // Packet decoded: Mission accomplished
                return TRUE;
            }
        }
    }

    // Haven't gotten the response we're looking for yet
    return FALSE;

}// ProcessData
Esempio n. 5
0
void makeStrokeForJoint(std::vector<poPoint> &stroke, poExtrudedLineSeg &seg1, poExtrudedLineSeg &seg2, poStrokeJoinProperty join, float stroke_width) {
	poPoint top, bottom;
	poPoint p1, p2, p3, p4, tmp;
	
	bool top_outside = combineExtrudedLineSegments(seg1, seg2, &top, &bottom);
	poPoint joint = (seg1.p4 + seg1.p3) / 2.f;
	
	switch(join) {
		case PO_STROKE_JOIN_MITRE:
			if(top_outside) {
				stroke.push_back(top);
				stroke.push_back(bottom);
			}
			else {
				stroke.push_back(bottom);
				stroke.push_back(top);
			}
			break;
			
		case PO_STROKE_JOIN_BEVEL:
			if(top_outside) {
				stroke.push_back(seg1.p3);
				stroke.push_back(bottom);
				stroke.push_back(seg2.p1);
				stroke.push_back(bottom);
			}
			else {
				stroke.push_back(top);
				stroke.push_back(seg1.p4);
				stroke.push_back(top);
				stroke.push_back(seg2.p2);
			}
			break;
			
		case PO_STROKE_JOIN_ROUND:
		{
			float halfW = stroke_width / 2.f;
			
			if(top_outside) {
				p1 = bottom;
				p2 = seg1.p3;
				p3 = joint;
				p4 = seg2.p1;
				
				stroke.push_back(p2);
				stroke.push_back(p1);
				
				float a1 = angleBetweenPoints(p2, p3, p4);
				
				poPoint diff = p2 - p3;
				float a2 = atan2f(diff.y, diff.x);
				
				float step = a1 / 9.f;
				float a = a2;
				
				for(int j=0; j<10; j++) {
					tmp.set(cosf(a)*halfW, sinf(a)*halfW, 0.f);
					stroke.push_back(tmp+p3);
					stroke.push_back(p3);
					a += step;
				}
				
				stroke.push_back(p4);
				stroke.push_back(p1);
			}
			else {
				p1 = top;
				p2 = seg1.p4;
				p3 = joint;
				p4 = seg2.p2;
				
				stroke.push_back(p1);
				stroke.push_back(p2);
				
				float a1 = angleBetweenPoints(p2, p3, p4);
				
				poPoint diff = p2 - p3;
				float a2 = atan2f(diff.y, diff.x);
				
				float step = a1 / 9.f;
				float a = a2;
				
				for(int j=0; j<10; j++) {
					tmp.set(cosf(a)*halfW, sinf(a)*halfW, 0.f);
					stroke.push_back(p3);
					stroke.push_back(tmp+p3);
					a += step;
				}
				
				stroke.push_back(p1);
				stroke.push_back(p4);
			}
			
			break;
		}
	}
}
Esempio n. 6
0
//------------------------------------------------------------------------------------------------
void BuyMenu::Update(float dt)
{
	int team = mPlayer->mTeam;
	if (mEngine->GetButtonClick(PSP_CTRL_CIRCLE)) {
		Disable();
		//mIsActive = false;
		return;
	}
	if (mEngine->GetButtonClick(PSP_CTRL_TRIANGLE) && mCategoryIndex != MAIN) {
		mCategoryIndex = MAIN;
	}

	if (mEngine->GetButtonClick(PSP_CTRL_CROSS)) {
		if (mSelectedIndex != -1) {
			if (mCategoryIndex == MAIN) {
				mCategoryIndex = mCategories[team][mCategoryIndex].buttons[mSelectedIndex].id;
				if (mIsOldStyle) {
					mSelectedIndex = 0;
				}
			}
			else {	
				mChoice = mCategories[team][mCategoryIndex].buttons[mSelectedIndex].id;
				mIsSelected = true;
				Disable();
			}
		}
		else {
			if (mCategoryIndex == MAIN) {
				mChoice = -1;
				mIsSelected = true;
				Disable();
			}
		}
	}

	if (!mIsOldStyle) {
		float aX = mEngine->GetAnalogX()-127.5f;
		float aY = mEngine->GetAnalogY()-127.5f;
		
		int size = mCategories[team][mCategoryIndex].buttons.size();
		if (aX >= 20 || aX <= -20 || aY >= 20 || aY <= -20) {
			angle = atan2f(aX,aY) + M_PI;
			mSelectedIndex = (int)(size*angle/(2*M_PI)+0.5f);
			if (mSelectedIndex == size) {
				mSelectedIndex = 0;
			}
		}
		else {
			mSelectedIndex = -1;
		}
	}
	else {
		int size = mCategories[team][mCategoryIndex].buttons.size();
		if (mEngine->GetButtonClick(PSP_CTRL_UP)) {
			mSelectedIndex--;
			if (mCategoryIndex == MAIN) {
				if (mSelectedIndex < -1) mSelectedIndex = size-1;
			}
			else {
				if (mSelectedIndex < 0) mSelectedIndex = size-1;
			}
		}
		else if (mEngine->GetButtonClick(PSP_CTRL_DOWN)) {
			mSelectedIndex++;
			if (mCategoryIndex == MAIN) {
				if (mSelectedIndex >= size) mSelectedIndex = -1;
			}
			else {
				if (mSelectedIndex >= size) mSelectedIndex = 0;
			}
		}
	}
}
Esempio n. 7
0
//----------------------------------------------------------------------------------
void ofxCvBlobFinder::draw(float x, float y, float w, float h) {
    
    // draw blobs
    //ofxCvContourFinder::draw(x, y, w, h);
    // scale blob
    float scalex = 0.0f;
    float scaley = 0.0f;
    
    if (_width != 0) {
        scalex = w / _width;
    }
    else {
        scalex = 1.0f;
    }
    
    if (_height != 0) {
        scaley = h / _height;
    }
    else {
        scaley = 1.0f;
    }
    
    
    ofSetPolyMode(OF_POLY_WINDING_NONZERO);
    // apply transformation
    glPushMatrix();
    glTranslatef(x, y, 0.0);
    glScalef(scalex, scaley, 0.0);
    
    
#define DRAW_BLOB_VECTOR(points) do{ \
ofBeginShape(); \
for(int i = 0; i < (points).size(); i++){ \
ofVertex((points[i]).x, (points[i]).y); \
} \
ofEndShape(true); \
} while(0)
    
    ofNoFill();
    for (int j = 0; j < blobz.size(); j++) {
        ofSetHexColor(0xFF0000);
        DRAW_BLOB_VECTOR( blobz[j].getPoints());
        
        ofSetHexColor(0x00FF00);
        DRAW_BLOB_VECTOR(blobz[j].getHullPoints());
        
        ofSetHexColor(0x0000FF);
        DRAW_BLOB_VECTOR( blobz[j].getApproxPoints());
        
        ofSetHexColor(0x00ffae);
        ofRectangle c = blobz[j].getBoundingBox();
        
        
        // draw bounding box
        ostringstream s;
        s << j << "Area = " << blobz[j].getArea();
        
        ofDrawBitmapString(s.str(), c.x, c.y + 50);
        ofRect(c.x, c.y, c.width, c.height);
        
        // get convexity defects
        vector<ofxCvConvexityDefect> cd = blobz[j].getConvexityDefects();
        ofSetHexColor(0x00effe);
        for(int i=0; i < cd.size(); i++){
            ofLine(cd[i].start.x, cd[i].start.y, cd[i].end.x, cd[i].end.y);
            ofCircle(cd[i].defect.x, cd[i].defect.y, 2);
            float angle = atan2f( ( (float) (cd[i].end.y - cd[i].start.y) ) , ( (float) (cd[i].end.x - cd[i].start.x)));
            float x = cd[i].defect.x - sinf(angle) * cd[i].length;
            float y = cd[i].defect.y + cosf(angle) * cd[i].length;
            ofSetHexColor(0xF0F0F0);
            ofLine(cd[i].defect.x, cd[i].defect.y, x, y);
        }
    }
    
    for (int k = 0; k < blobParams.size(); k++){
        ofDrawBitmapString("Blob:" + ofToString(blobParams[k].id), blobParams[k].position);
    }
    
    glPopMatrix();
}
/**
 * Attitude controller.
 * Input: 'vehicle_attitude_setpoint' topics (depending on mode)
 * Output: '_rates_sp' vector, '_thrust_sp'
 */
void
MulticopterAttitudeControl::control_attitude(float dt)
{
	vehicle_attitude_setpoint_poll();

	_thrust_sp = _v_att_sp.thrust;

	/* construct attitude setpoint rotation matrix */
	math::Matrix<3, 3> R_sp;
	R_sp.set(_v_att_sp.R_body);

	/* rotation matrix for current state */
	math::Matrix<3, 3> R;
	R.set(_v_att.R);

	/* all input data is ready, run controller itself */

	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
	math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
	math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));

	/* axis and sin(angle) of desired rotation */
	math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);

	/* calculate angle error */
	float e_R_z_sin = e_R.length();
	float e_R_z_cos = R_z * R_sp_z;

	/* calculate weight for yaw control */
	float yaw_w = R_sp(2, 2) * R_sp(2, 2);

	/* calculate rotation matrix after roll/pitch only rotation */
	math::Matrix<3, 3> R_rp;

	if (e_R_z_sin > 0.0f) {
		/* get axis-angle representation */
		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
		math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;

		e_R = e_R_z_axis * e_R_z_angle;

		/* cross product matrix for e_R_axis */
		math::Matrix<3, 3> e_R_cp;
		e_R_cp.zero();
		e_R_cp(0, 1) = -e_R_z_axis(2);
		e_R_cp(0, 2) = e_R_z_axis(1);
		e_R_cp(1, 0) = e_R_z_axis(2);
		e_R_cp(1, 2) = -e_R_z_axis(0);
		e_R_cp(2, 0) = -e_R_z_axis(1);
		e_R_cp(2, 1) = e_R_z_axis(0);

		/* rotation matrix for roll/pitch only rotation */
		R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));

	} else {
		/* zero roll/pitch rotation */
		R_rp = R;
	}

	/* R_rp and R_sp has the same Z axis, calculate yaw error */
	math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
	math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;

	if (e_R_z_cos < 0.0f) {
		/* for large thrust vector rotations use another rotation method:
		 * calculate angle and axis for R -> R_sp rotation directly */
		math::Quaternion q;
		q.from_dcm(R.transposed() * R_sp);
		math::Vector<3> e_R_d = q.imag();
		e_R_d.normalize();
		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));

		/* use fusion of Z axis based rotation and direct rotation */
		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
	}

	/* calculate angular rates setpoint */
	_rates_sp = _params.att_p.emult(e_R);

	/* limit rates */
	for (int i = 0; i < 3; i++) {
		_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
	}

	/* feed forward yaw setpoint rate */
	_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
}
Esempio n. 9
0
 /**
  * Get the direction from 1 point to another
  * Thanks to "Snarkbait" for this little code snippit
  * @return Angle of difference
  */
 float Vector2::GetDirection( float fromx, float fromy, float tox, float toy )
 {
     // arcus tangens, convert to degrees, add 450 and normalize to 360.
     return fmodf((atan2f(toy - fromy, tox - fromx) / (float)M_PI * 180.0f + 450.0f), 360.0f);
 }
CGFloat
ccpToAngle(const CCPoint v)
{
	return atan2f(v.y, v.x);
}
Esempio n. 11
0
void FrenetFinder::ConvertPathToFrenets (Spline3D *pSpline, Matrix3 relativeTransform, Tab<Matrix3> & tFrenets,
						   int numSegs, bool align, float rotateAroundZ) {
	// Given a path, a sequence of points in 3-space, create transforms
	// for putting a cross-section around each of those points, loft-style.

	// bezShape is provided by user, tFrenets contains output, numSegs is one less than the number of transforms requested.

	// Strategy: The Z-axis is mapped along the path, and the X and Y axes 
	// are chosen in a well-defined manner to get an orthonormal basis.

	int i;

	if (numSegs < 1) return;
	tFrenets.SetCount (numSegs+1);

	int numIntervals = pSpline->Closed() ? numSegs+1 : numSegs;
	float denominator = float(numIntervals);
	Point3 xDir, yDir, zDir, location, tangent, axis;
	float position, sine, cosine, theta;
	Matrix3 rotation;

	// Find initial x,y directions:
	location = relativeTransform * pSpline->InterpCurve3D (0.0f);
	tangent = relativeTransform.VectorTransform (pSpline->TangentCurve3D (0.0f));
	zDir = tangent;

	Matrix3 inverseBasisOfSpline(1);
	if (align) {
		xDir = Point3(0.0f, 0.0f, 0.0f);
		yDir = xDir;
		mBasisFinder.BasisFromZDir (zDir, xDir, yDir);
		if (rotateAroundZ) {
			Matrix3 rotator(1);
			rotator.SetRotate (AngAxis (zDir, rotateAroundZ));
			xDir = xDir * rotator;
			yDir = yDir * rotator;
		}
		Matrix3 basisOfSpline(1);
		basisOfSpline.SetRow (0, xDir);
		basisOfSpline.SetRow (1, yDir);
		basisOfSpline.SetRow (2, tangent);
		basisOfSpline.SetTrans (location);
		inverseBasisOfSpline = Inverse (basisOfSpline);
	} else {
		inverseBasisOfSpline.SetRow (3, -location);
	}

	// Make relative transform take the spline from its own object space to our object space,
	// and from there into the space defined by its origin and initial direction:
	relativeTransform = relativeTransform * inverseBasisOfSpline;
	// (Note left-to-right evaluation order: Given matrices A,B, point x, x(AB) = (xA)B

	// The first transform is necessarily the identity:
	tFrenets[0].IdentityMatrix ();

	// Set up xDir, yDir, zDir to match our first-point basis:
	xDir = Point3 (1,0,0);
	yDir = Point3 (0,1,0);
	zDir = Point3 (0,0,1);

	for (i=1; i<=numIntervals; i++) {
		position = float(i) / denominator;
		location = relativeTransform * pSpline->InterpCurve3D (position);
		tangent = relativeTransform.VectorTransform (pSpline->TangentCurve3D (position));

		// This is the procedure we follow at each step in the path: find the
		// orthonormal basis with the right orientation, then compose with
		// the translation putting the origin at the path-point.

		// As we proceed along the path, we apply minimal rotations to
		// our original basis to keep the Z-axis tangent to the curve.
		// The X and Y axes follow in a natural manner.

		// xDir, yDir, zDir still have their values from last time...
		// Create a rotation matrix which maps the last zDir onto the current tangent:
		axis = zDir ^ tangent;	// gives axis, scaled by sine of angle.
		sine = FLength(axis);	// positive - keeps angle value in (0,PI) range.
		cosine = DotProd (zDir, tangent);	// Gives cosine of angle.
		theta = atan2f (sine, cosine);
		rotation.SetRotate (AngAxis (Normalize(axis), theta));
		Point3 testVector = rotation * zDir;
		xDir = Normalize (rotation * xDir);
		yDir = Normalize (rotation * yDir);
		zDir = tangent;

		if (i<=numSegs) {
			tFrenets[i].IdentityMatrix ();
			tFrenets[i].SetRow (0, xDir);
			tFrenets[i].SetRow (1, yDir);
			tFrenets[i].SetRow (2, tangent);
			tFrenets[i].SetTrans (location);
		}
	}
}
Esempio n. 12
0
// update L1 control for loitering
void AP_L1_Control::update_loiter(const struct Location &center_WP, float radius, int8_t loiter_direction)
{
	struct Location _current_loc;

    // scale loiter radius with square of EAS2TAS to allow us to stay
    // stable at high altitude
    radius *= sq(_ahrs.get_EAS2TAS());

	// Calculate guidance gains used by PD loop (used during circle tracking)
	float omega = (6.2832f / _L1_period);
	float Kx = omega * omega;
	float Kv = 2.0f * _L1_damping * omega;

	// Calculate L1 gain required for specified damping (used during waypoint capture)
	float K_L1 = 4.0f * _L1_damping * _L1_damping;

	//Get current position and velocity
    _ahrs.get_position(_current_loc);

	Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();

	//Calculate groundspeed
	float groundSpeed = max(_groundspeed_vector.length() , 1.0f);


	// update _target_bearing_cd
	_target_bearing_cd = get_bearing_cd(_current_loc, center_WP);


	// Calculate time varying control parameters
	// Calculate the L1 length required for specified period
	// 0.3183099 = 1/pi
	_L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;

	//Calculate the NE position of the aircraft relative to WP A
    Vector2f A_air = location_diff(center_WP, _current_loc);
	
    //Calculate the unit vector from WP A to aircraft
    Vector2f A_air_unit = A_air.normalized();

	//Calculate Nu to capture center_WP
	float xtrackVelCap = A_air_unit % _groundspeed_vector; // Velocity across line - perpendicular to radial inbound to WP
	float ltrackVelCap = - (_groundspeed_vector * A_air_unit); // Velocity along line - radial inbound to WP
	float Nu = atan2f(xtrackVelCap,ltrackVelCap);

    _prevent_indecision(Nu);
    _last_Nu = Nu;

	Nu = constrain_float(Nu, -M_PI_2, M_PI_2); //Limit Nu to +- Pi/2

	//Calculate lat accln demand to capture center_WP (use L1 guidance law)
	float latAccDemCap = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);
	
	//Calculate radial position and velocity errors
	float xtrackVelCirc = -ltrackVelCap; // Radial outbound velocity - reuse previous radial inbound velocity
	float xtrackErrCirc = A_air.length() - radius; // Radial distance from the loiter circle

	// keep crosstrack error for reporting
	_crosstrack_error = xtrackErrCirc;
	
	//Calculate PD control correction to circle waypoint_ahrs.roll
	float latAccDemCircPD = (xtrackErrCirc * Kx + xtrackVelCirc * Kv);
	
	//Calculate tangential velocity
	float velTangent = xtrackVelCap * float(loiter_direction);
	
    //Prevent PD demand from turning the wrong way by limiting the command when flying the wrong way
    if (ltrackVelCap < 0.0f && velTangent < 0.0f) {
        latAccDemCircPD =  max(latAccDemCircPD, 0.0f);
	}
	
	// Calculate centripetal acceleration demand
	float latAccDemCircCtr = velTangent * velTangent / max((0.5f * radius), (radius + xtrackErrCirc));

	//Sum PD control and centripetal acceleration to calculate lateral manoeuvre demand
	float latAccDemCirc = loiter_direction * (latAccDemCircPD + latAccDemCircCtr);
	
	// Perform switchover between 'capture' and 'circle' modes at the
	// point where the commands cross over to achieve a seamless transfer
	// Only fly 'capture' mode if outside the circle
	if (xtrackErrCirc > 0.0f && loiter_direction * latAccDemCap < loiter_direction * latAccDemCirc) {
		_latAccDem = latAccDemCap;
		_WPcircle = false;
		_bearing_error = Nu; // angle between demanded and achieved velocity vector, +ve to left of track
		_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point
	} else {
		_latAccDem = latAccDemCirc;
		_WPcircle = true;
		_bearing_error = 0.0f; // bearing error (radians), +ve to left of track
		_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians)from AC to L1 point
	}
}
Esempio n. 13
0
// update L1 control for waypoint navigation
// this function costs about 3.5 milliseconds on a AVR2560
void AP_L1_Control::update_waypoint(const struct Location &prev_WP, const struct Location &next_WP)
{

	struct Location _current_loc;
	float Nu;
	float xtrackVel;
	float ltrackVel;
	
	// Calculate L1 gain required for specified damping
	float K_L1 = 4.0f * _L1_damping * _L1_damping;

	// Get current position and velocity
    _ahrs.get_position(_current_loc);

	Vector2f _groundspeed_vector = _ahrs.groundspeed_vector();

	// update _target_bearing_cd
	_target_bearing_cd = get_bearing_cd(_current_loc, next_WP);
	
	//Calculate groundspeed
	float groundSpeed = _groundspeed_vector.length();
    if (groundSpeed < 0.1f) {
        // use a small ground speed vector in the right direction,
        // allowing us to use the compass heading at zero GPS velocity
        groundSpeed = 0.1f;
        _groundspeed_vector = Vector2f(cosf(_ahrs.yaw), sinf(_ahrs.yaw)) * groundSpeed;
    }

	// Calculate time varying control parameters
	// Calculate the L1 length required for specified period
	// 0.3183099 = 1/1/pipi
	_L1_dist = 0.3183099f * _L1_damping * _L1_period * groundSpeed;
	
	// Calculate the NE position of WP B relative to WP A
    Vector2f AB = location_diff(prev_WP, next_WP);
	
	// Check for AB zero length and track directly to the destination
	// if too small
	if (AB.length() < 1.0e-6f) {
		AB = location_diff(_current_loc, next_WP);
	}
	AB.normalize();

	// Calculate the NE position of the aircraft relative to WP A
    Vector2f A_air = location_diff(prev_WP, _current_loc);

	// calculate distance to target track, for reporting
	_crosstrack_error = AB % A_air;

	//Determine if the aircraft is behind a +-135 degree degree arc centred on WP A
	//and further than L1 distance from WP A. Then use WP A as the L1 reference point
		//Otherwise do normal L1 guidance
	float WP_A_dist = A_air.length();
	float alongTrackDist = A_air * AB;
	if (WP_A_dist > _L1_dist && alongTrackDist/max(WP_A_dist, 1.0f) < -0.7071f) 
    {
		//Calc Nu to fly To WP A
		Vector2f A_air_unit = (A_air).normalized(); // Unit vector from WP A to aircraft
		xtrackVel = _groundspeed_vector % (-A_air_unit); // Velocity across line
		ltrackVel = _groundspeed_vector * (-A_air_unit); // Velocity along line
		Nu = atan2f(xtrackVel,ltrackVel);

        _prevent_indecision(Nu);

		_nav_bearing = atan2f(-A_air_unit.y , -A_air_unit.x); // bearing (radians) from AC to L1 point

	} else { //Calc Nu to fly along AB line
			
		//Calculate Nu2 angle (angle of velocity vector relative to line connecting waypoints)
		xtrackVel = _groundspeed_vector % AB; // Velocity cross track
		ltrackVel = _groundspeed_vector * AB; // Velocity along track
		float Nu2 = atan2f(xtrackVel,ltrackVel);
		//Calculate Nu1 angle (Angle to L1 reference point)
		float xtrackErr = A_air % AB;
		float sine_Nu1 = xtrackErr/max(_L1_dist, 0.1f);
		//Limit sine of Nu1 to provide a controlled track capture angle of 45 deg
		sine_Nu1 = constrain_float(sine_Nu1, -0.7071f, 0.7071f);
		float Nu1 = asinf(sine_Nu1);
		Nu = Nu1 + Nu2;
		_nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point		
	}	

    _last_Nu = Nu;
			
	//Limit Nu to +-pi
	Nu = constrain_float(Nu, -1.5708f, +1.5708f);
	_latAccDem = K_L1 * groundSpeed * groundSpeed / _L1_dist * sinf(Nu);
	
	// Waypoint capture status is always false during waypoint following
	_WPcircle = false;
	
	_bearing_error = Nu; // bearing error angle (radians), +ve to left of track
}
/** @brief MATLAB Driver.
 **/
void
mexFunction(int nout, mxArray *out[], 
            int nin, const mxArray *in[])
{
  int M,N,S=0,smin=0,K,num_levels=0 ;
  const int* dimensions ;
  const double* P_pt ;
  const double* G_pt ;
  float* descr_pt ;
  float* buffer_pt ;
  float sigma0 ;
  float magnif = 3.0f ; /* Spatial bin extension factor. */
  int NBP = 4 ;         /* Number of bins for one spatial direction (even). */
  int NBO = 8 ;         /* Number of bins for the ortientation. */
  int mode = NOSCALESPACE ;
  int buffer_size=0;

  enum {IN_G=0,IN_P,IN_SIGMA0,IN_S,IN_SMIN} ;
  enum {OUT_L=0} ;

  /* ------------------------------------------------------------------
  **                                                Check the arguments
  ** --------------------------------------------------------------- */ 
 
  if (nin < 3) {
    mexErrMsgTxt("At least three arguments are required") ;
  } else if (nout > 1) {
    mexErrMsgTxt("Too many output arguments.");
  }
		
  if( !uIsRealScalar(in[IN_SIGMA0]) ) {
    mexErrMsgTxt("SIGMA0 should be a real scalar") ;
  }
	
  if(!mxIsDouble(in[IN_G]) ||
     mxGetNumberOfDimensions(in[IN_G]) > 3) {
    mexErrMsgTxt("G should be a real matrix or 3-D array") ;
  }
  
  sigma0 = (float) *mxGetPr(in[IN_SIGMA0]) ;
  
  dimensions = mxGetDimensions(in[IN_G]) ;
  M = dimensions[0] ;
  N = dimensions[1] ;
  G_pt = mxGetPr(in[IN_G]) ;
  
  P_pt = mxGetPr(in[IN_P]) ;	
  K = mxGetN(in[IN_P]) ;
  
  if( !uIsRealMatrix(in[IN_P],-1,-1)) {
    mexErrMsgTxt("P should be a real matrix") ;
  }

  if ( mxGetM(in[IN_P])  == 4) {
    /* Standard (scale space) mode */ 
    mode = SCALESPACE ;
    num_levels = dimensions[2] ;
    
    if(nin < 5) {
      mexErrMsgTxt("Five arguments are required in standard mode") ;
    }
    
    if( !uIsRealScalar(in[IN_S]) ) {
      mexErrMsgTxt("S should be a real scalar") ;
    }
    
    if( !uIsRealScalar(in[IN_SMIN]) ) {
      mexErrMsgTxt("SMIN should be a real scalar") ;
    }
    
    if( !uIsRealMatrix(in[IN_P],4,-1)) {
      mexErrMsgTxt("When the e mode P should be a 4xK matrix.") ;
    }
    
    S = (int)(*mxGetPr(in[IN_S])) ;
    smin = (int)(*mxGetPr(in[IN_SMIN])) ;
    
  } else if (  mxGetM(in[IN_P])  == 3 ) {
    mode = NOSCALESPACE ;
    num_levels = 1 ;
    S      = 1 ;
    smin   = 0 ;
  } else {
    mexErrMsgTxt("P should be either a 3xK or a 4xK matrix.") ;
  }

  /* Parse the property-value pairs */
  {
    char str [80] ;
    int arg = (mode == SCALESPACE) ? IN_SMIN + 1 : IN_SIGMA0 + 1 ;

    while(arg < nin) {
      int k ;

      if( !uIsString(in[arg],-1) ) {
        mexErrMsgTxt("The first argument in a property-value pair"
                     " should be a string") ;
      }
      mxGetString(in[arg], str, 80) ;

#ifdef WINDOWS      
      for(k = 0 ; properties[k] && strcmpi(str, properties[k]) ; ++k) ;
#else
      for(k = 0 ; properties[k] && strcasecmp(str, properties[k]) ; ++k) ;
#endif

      switch (k) {
      case PROP_NBP:
        if( !uIsRealScalar(in[arg+1]) ) {
          mexErrMsgTxt("'NumSpatialBins' should be real scalar") ;
        }
        NBP = (int) *mxGetPr(in[arg+1]) ;
        if( NBP <= 0 || (NBP & 0x1) ) {
          mexErrMsgTxt("'NumSpatialBins' must be positive and even") ;
        }
        break ;

      case PROP_NBO:
        if( !uIsRealScalar(in[arg+1]) ) {
          mexErrMsgTxt("'NumOrientBins' should be a real scalar") ;
        }
        NBO = (int) *mxGetPr(in[arg+1]) ;
        if( NBO <= 0 ) {
          mexErrMsgTxt("'NumOrientlBins' must be positive") ;
        }
        break ;

      case PROP_MAGNIF:
        if( !uIsRealScalar(in[arg+1]) ) {
          mexErrMsgTxt("'Magnif' should be a real scalar") ;
        }
        magnif = (float) *mxGetPr(in[arg+1]) ;
        if( magnif <= 0 ) {
          mexErrMsgTxt("'Magnif' must be positive") ;
        }
        break ;

      case PROP_UNKNOWN:
        mexErrMsgTxt("Property unknown.") ;
        break ;
      }
      arg += 2 ;
    }
  }
  
  /* -----------------------------------------------------------------
   *                                   Pre-compute gradient and angles
   * -------------------------------------------------------------- */
  /* Alloc two buffers and make sure their size is multiple of 128 for
   * better alignment (used also by the Altivec code below.)
   */
  buffer_size = (M*N*num_levels + 0x7f) & (~ 0x7f) ;
  buffer_pt = (float*) mxMalloc( sizeof(float) * 2 * buffer_size ) ;
  descr_pt  = (float*) mxCalloc( NBP*NBP*NBO*K,  sizeof(float)  ) ;

  {
    /* Offsets to move in the scale space. */
    const int yo = 1 ;
    const int xo = M ;
    const int so = M*N ;
    int x,y,s ;

#define at(x,y) (*(pt + (x)*xo + (y)*yo))

    /* Compute the gradient */
    for(s = 0 ; s < num_levels ; ++s) {
      const double* pt = G_pt + s*so ;
      for(x = 1 ; x < N-1 ; ++x) {
        for(y = 1 ; y < M-1 ; ++y) {
          float Dx = 0.5 * ( at(x+1,y) - at(x-1,y) ) ;
          float Dy = 0.5 * ( at(x,y+1) - at(x,y-1) ) ;
          buffer_pt[(x*xo+y*yo+s*so) + 0          ] = Dx ;
          buffer_pt[(x*xo+y*yo+s*so) + buffer_size] = Dy ;
        }
      }
    }
    
    /* Compute angles and modules */
    {
      float* pt = buffer_pt ;
      int j = 0 ;
      while (j < N*M*num_levels) {

#if defined( MACOSX ) && defined( __ALTIVEC__ )
        if( ((unsigned int)pt & 0x7) == 0 && j+3 < N*M*num_levels ) {
          /* If aligned to 128 bit and there are at least 4 pixels left */
          float4 a, b, c, d ;
          a.vec = vec_ld(0,(vector float*)(pt              )) ;
          b.vec = vec_ld(0,(vector float*)(pt + buffer_size)) ;
          c.vec = vatan2f(b.vec,a.vec) ;
          a.x[0] = a.x[0]*a.x[0]+b.x[0]*b.x[0] ;
          a.x[1] = a.x[1]*a.x[1]+b.x[1]*b.x[1] ;
          a.x[2] = a.x[2]*a.x[2]+b.x[2]*b.x[2] ;
          a.x[3] = a.x[3]*a.x[3]+b.x[3]*b.x[3] ;
          d.vec = vsqrtf(a.vec) ;
          vec_st(c.vec,0,(vector float*)(pt + buffer_size)) ;
          vec_st(d.vec,0,(vector float*)(pt              )) ;
          j += 4 ;
          pt += 4 ;
        } else {
#endif
          float Dx = *(pt              ) ;
          float Dy = *(pt + buffer_size) ;
          *(pt              ) = sqrtf(Dx*Dx + Dy*Dy) ;
          *(pt + buffer_size) = atan2f(Dy, Dx) ;
          j += 1 ;
          pt += 1 ;
#if defined( MACOSX ) && defined( __ALTIVEC__ )
        }
#endif

      }
    }
  }

  /* -----------------------------------------------------------------
   *                                                        Do the job
   * -------------------------------------------------------------- */ 
  if(K > 0) {    
    int p ;

    /* Offsets to move in the buffer */
    const int yo = 1 ;
    const int xo = M ;
    const int so = M*N ;

    /* Offsets to move in the descriptor. */
    /* Use Lowe's convention. */
    const int binto = 1 ;
    const int binyo = NBO * NBP ;
    const int binxo = NBO ;
    const int bino  = NBO * NBP * NBP ;

    for(p = 0 ; p < K ; ++p, descr_pt += bino) {
      /* The SIFT descriptor is a  three dimensional histogram of the position
       * and orientation of the gradient.  There are NBP bins for each spatial
       * dimesions and NBO  bins for the orientation dimesion,  for a total of
       * NBP x NBP x NBO bins.
       *
       * The support  of each  spatial bin  has an extension  of SBP  = 3sigma
       * pixels, where sigma is the scale  of the keypoint.  Thus all the bins
       * together have a  support SBP x NBP pixels wide  . Since weighting and
       * interpolation of  pixel is used, another  half bin is  needed at both
       * ends of  the extension. Therefore, we  need a square window  of SBP x
       * (NBP + 1) pixels. Finally, since the patch can be arbitrarly rotated,
       * we need to consider  a window 2W += sqrt(2) x SBP  x (NBP + 1) pixels
       * wide.
       */      
      const float x = (float) *P_pt++ ;
      const float y = (float) *P_pt++ ;
      const float s = (float) (mode == SCALESPACE) ? (*P_pt++) : 0.0 ;
      const float theta0 = (float) *P_pt++ ;

      const float st0 = sinf(theta0) ;
      const float ct0 = cosf(theta0) ;
      const int xi = (int) floor(x+0.5) ; /* Round-off */
      const int yi = (int) floor(y+0.5) ;
      const int si = (int) floor(s+0.5) - smin ;
      const float sigma = sigma0 * powf(2, s / S) ;
      const float SBP = magnif * sigma ;
      const int W = (int) floor( sqrt(2.0) * SBP * (NBP + 1) / 2.0 + 0.5) ;      
      int bin ;
      int dxi ;
      int dyi ;
      const float* pt ;
      float* dpt ;

      /* Check that keypoints are within bounds . */

      if(xi < 0   || 
         xi > N-1 || 
         yi < 0   || 
         yi > M-1 ||
         ((mode==SCALESPACE) && 
          (si < 0   ||
           si > dimensions[2]-1) ) )
        continue ;

      /* Center the scale space and the descriptor on the current keypoint. 
       * Note that dpt is pointing to the bin of center (SBP/2,SBP/2,0).
       */
      pt  = buffer_pt + xi*xo + yi*yo + si*so ;
      dpt = descr_pt + (NBP/2) * binyo + (NBP/2) * binxo ;
     
#define atd(dbinx,dbiny,dbint) (*(dpt + (dbint)*binto + (dbiny)*binyo + (dbinx)*binxo))
      
      /*
       * Process each pixel in the window and in the (1,1)-(M-1,N-1)
       * rectangle.
       */
      for(dxi = max(-W, 1-xi) ; dxi <= min(+W, N-2-xi) ; ++dxi) {
        for(dyi = max(-W, 1-yi) ; dyi <= min(+W, M-2-yi) ; ++dyi) {

          /* Compute the gradient. */
          float mod   = *(pt + dxi*xo + dyi*yo + 0           ) ;
          float angle = *(pt + dxi*xo + dyi*yo + buffer_size ) ;
#ifdef LOWE_COMPATIBLE
          float theta = fast_mod(-angle + theta0) ;
#else
          float theta = fast_mod(angle - theta0) ;
#endif
          /* Get the fractional displacement. */
          float dx = ((float)(xi+dxi)) - x;
          float dy = ((float)(yi+dyi)) - y;

          /* Get the displacement normalized w.r.t. the keypoint orientation
           * and extension. */
          float nx = ( ct0 * dx + st0 * dy) / SBP ;
          float ny = (-st0 * dx + ct0 * dy) / SBP ; 
          float nt = NBO * theta / (2*M_PI) ;

          /* Get the gaussian weight of the sample. The gaussian window
           * has a standard deviation of NBP/2. Note that dx and dy are in
           * the normalized frame, so that -NBP/2 <= dx <= NBP/2. */
          const float wsigma = NBP/2 ;
          float win =  expf(-(nx*nx + ny*ny)/(2.0 * wsigma * wsigma)) ;

          /* The sample will be distributed in 8 adijacient bins. 
           * Now we get the ``lower-left'' bin. */
          int binx = fast_floor( nx - 0.5 ) ;
          int biny = fast_floor( ny - 0.5 ) ;
          int bint = fast_floor( nt ) ;
          float rbinx = nx - (binx+0.5) ;
          float rbiny = ny - (biny+0.5) ;
          float rbint = nt - bint ;
          int dbinx ;
          int dbiny ;
          int dbint ;

          /* Distribute the current sample into the 8 adijacient bins. */
          for(dbinx = 0 ; dbinx < 2 ; ++dbinx) {
            for(dbiny = 0 ; dbiny < 2 ; ++dbiny) {
              for(dbint = 0 ; dbint < 2 ; ++dbint) {
                
                if( binx+dbinx >= -(NBP/2) &&
                    binx+dbinx <   (NBP/2) &&
                    biny+dbiny >= -(NBP/2) &&
                    biny+dbiny <   (NBP/2) ) {
                  float weight = win 
                    * mod 
                    * fabsf(1 - dbinx - rbinx)
                    * fabsf(1 - dbiny - rbiny)
                    * fabsf(1 - dbint - rbint) ;

                  atd(binx+dbinx, biny+dbiny, (bint+dbint) % NBO) += weight ;
                }
              }            
            }
          }
        }  
      }

      {
        /* Normalize the histogram to L2 unit length. */        
        normalize_histogram(descr_pt, descr_pt + NBO*NBP*NBP) ;
        
        /* Truncate at 0.2. */
        for(bin = 0; bin < NBO*NBP*NBP ; ++bin) {
          if (descr_pt[bin] > 0.2) descr_pt[bin] = 0.2;
        }
        
        /* Normalize again. */
        normalize_histogram(descr_pt, descr_pt + NBO*NBP*NBP) ;
      }
    }
  }

  /* Restore pointer to the beginning of the descriptors. */
  descr_pt -= NBO*NBP*NBP*K ;

  {
    int k ;
    double* L_pt ;
    out[OUT_L] = mxCreateDoubleMatrix(NBP*NBP*NBO, K, mxREAL) ;
    L_pt = mxGetPr(out[OUT_L]) ;
    for(k = 0 ; k < NBP*NBP*NBO*K ; ++k) {
      L_pt[k] = descr_pt[k] ;
    }
  }

  mxFree(descr_pt) ;  
  mxFree(buffer_pt) ;
}
Esempio n. 15
0
float
cargf(fcomplex z) {
	return (atan2f(F_IM(z), F_RE(z)));
}
Esempio n. 16
0
TEST(math, atan2f) {
  ASSERT_FLOAT_EQ(0.0f, atan2f(0.0f, 0.0f));
}
Esempio n. 17
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)
{
    Vector3f velocity;
    uint32_t last_correction_time;

    // perform yaw drift correction if we have a new yaw reference
    // vector
    drift_correction_yaw();

    // rotate accelerometer values into the earth frame
    for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
        if (_ins.get_accel_health(i)) {
            _accel_ef[i] = _dcm_matrix * _ins.get_accel(i);
            // integrate the accel vector in the earth frame between GPS readings
            _ra_sum[i] += _accel_ef[i] * deltat;
        }
    }

    //update _accel_ef_blended
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
    if (_ins.get_accel_count() == 2 && _ins.get_accel_health(0) && _ins.get_accel_health(1)) {
        float imu1_weight_target = _active_accel_instance == 0 ? 1.0f : 0.0f;
        // slew _imu1_weight over one second
        _imu1_weight += constrain_float(imu1_weight_target-_imu1_weight, -deltat, deltat);
        _accel_ef_blended = _accel_ef[0] * _imu1_weight + _accel_ef[1] * (1.0f - _imu1_weight);
    } else {
        _accel_ef_blended = _accel_ef[_ins.get_primary_accel()];
    }
#else
    _accel_ef_blended = _accel_ef[_ins.get_primary_accel()];
#endif // HAL_CPU_CLASS >= HAL_CPU_CLASS_75

    // keep a sum of the deltat values, so we know how much time
    // we have integrated over
    _ra_deltat += deltat;

    if (!have_gps() || 
        _gps.status() < AP_GPS::GPS_OK_FIX_3D || 
        _gps.num_sats() < _gps_minsats) {
        // no GPS, or not a good lock. From experience we need at
        // least 6 satellites to get a really reliable velocity number
        // from the GPS.
        //
        // As a fallback we use the fixed wing acceleration correction
        // if we have an airspeed estimate (which we only have if
        // _fly_forward is set), otherwise no correction
        if (_ra_deltat < 0.2f) {
            // not enough time has accumulated
            return;
        }
        float airspeed;
        if (airspeed_sensor_enabled()) {
            airspeed = _airspeed->get_airspeed();
        } else {
            airspeed = _last_airspeed;
        }
        // use airspeed to estimate our ground velocity in
        // earth frame by subtracting the wind
        velocity = _dcm_matrix.colx() * airspeed;

        // add in wind estimate
        velocity += _wind;

        last_correction_time = hal.scheduler->millis();
        _have_gps_lock = false;
    } else {
        if (_gps.last_fix_time_ms() == _ra_sum_start) {
            // we don't have a new GPS fix - nothing more to do
            return;
        }
        velocity = _gps.velocity();
        last_correction_time = _gps.last_fix_time_ms();
        if (_have_gps_lock == false) {
            // if we didn't have GPS lock in the last drift
            // correction interval then set the velocities equal
            _last_velocity = velocity;
        }
        _have_gps_lock = true;

        // keep last airspeed estimate for dead-reckoning purposes
        Vector3f airspeed = velocity - _wind;
        airspeed.z = 0;
        _last_airspeed = airspeed.length();
    }

    if (have_gps()) {
        // use GPS for positioning with any fix, even a 2D fix
        _last_lat = _gps.location().lat;
        _last_lng = _gps.location().lng;
        _position_offset_north = 0;
        _position_offset_east = 0;

        // once we have a single GPS lock, we can update using
        // dead-reckoning from then on
        _have_position = true;
    } else {
        // update dead-reckoning position estimate
        _position_offset_north += velocity.x * _ra_deltat;
        _position_offset_east  += velocity.y * _ra_deltat;        
    }

    // see if this is our first time through - in which case we
    // just setup the start times and return
    if (_ra_sum_start == 0) {
        _ra_sum_start = last_correction_time;
        _last_velocity = velocity;
        return;
    }

    // equation 9: get the corrected acceleration vector in earth frame. Units
    // are m/s/s
    Vector3f GA_e;
    GA_e = Vector3f(0, 0, -1.0f);

    bool using_gps_corrections = false;
    float ra_scale = 1.0f/(_ra_deltat*GRAVITY_MSS);

    if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
        float v_scale = gps_gain.get() * ra_scale;
        Vector3f vdelta = (velocity - _last_velocity) * v_scale;
        GA_e += vdelta;
        GA_e.normalize();
        if (GA_e.is_inf()) {
            // wait for some non-zero acceleration information
            _last_failure_ms = hal.scheduler->millis();
            return;
        }
        using_gps_corrections = true;
    }

    // calculate the error term in earth frame.
    // we do this for each available accelerometer then pick the
    // accelerometer that leads to the smallest error term. This takes
    // advantage of the different sample rates on different
    // accelerometers to dramatically reduce the impact of aliasing
    // due to harmonics of vibrations that match closely the sampling
    // rate of our accelerometers. On the Pixhawk we have the LSM303D
    // running at 800Hz and the MPU6000 running at 1kHz, by combining
    // the two the effects of aliasing are greatly reduced.
    Vector3f error[INS_MAX_INSTANCES];
    float error_dirn[INS_MAX_INSTANCES];
    Vector3f GA_b[INS_MAX_INSTANCES];
    int8_t besti = -1;
    float best_error = 0;
    for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
        if (!_ins.get_accel_health(i)) {
            // only use healthy sensors
            continue;
        }
        _ra_sum[i] *= ra_scale;

        // get the delayed ra_sum to match the GPS lag
        if (using_gps_corrections) {
            GA_b[i] = ra_delayed(i, _ra_sum[i]);
        } else {
            GA_b[i] = _ra_sum[i];
        }
        if (GA_b[i].is_zero()) {
            // wait for some non-zero acceleration information
            continue;
        }
        GA_b[i].normalize();
        if (GA_b[i].is_inf()) {
            // wait for some non-zero acceleration information
            continue;
        }
        error[i] = GA_b[i] % GA_e;
        // Take dot product to catch case vectors are opposite sign and parallel
        error_dirn[i] = GA_b[i] * GA_e;
        float error_length = error[i].length();
        if (besti == -1 || error_length < best_error) {
            besti = i;
            best_error = error_length;
        }
        // Catch case where orientation is 180 degrees out
        if (error_dirn[besti] < 0.0f) {
            best_error = 1.0f;
        }

    }

    if (besti == -1) {
        // no healthy accelerometers!
        _last_failure_ms = hal.scheduler->millis();
        return;
    }

    _active_accel_instance = besti;

#define YAW_INDEPENDENT_DRIFT_CORRECTION 0
#if YAW_INDEPENDENT_DRIFT_CORRECTION
    // step 2 calculate earth_error_Z
    float earth_error_Z = error.z;

    // equation 10
    float tilt = pythagorous2(GA_e.x, GA_e.y);

    // equation 11
    float theta = atan2f(GA_b[besti].y, GA_b[besti].x);

    // equation 12
    Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z);

    // step 6
    error = GA_b[besti] % GA_e2;
    error.z = earth_error_Z;
#endif // YAW_INDEPENDENT_DRIFT_CORRECTION

    // to reduce the impact of two competing yaw controllers, we
    // reduce the impact of the gps/accelerometers on yaw when we are
    // flat, but still allow for yaw correction using the
    // accelerometers at high roll angles as long as we have a GPS
    if (AP_AHRS_DCM::use_compass()) {
        if (have_gps() && gps_gain == 1.0f) {
            error[besti].z *= sinf(fabsf(roll));
        } else {
            error[besti].z = 0;
        }
    }

    // if ins is unhealthy then stop attitude drift correction and
    // hope the gyros are OK for a while. Just slowly reduce _omega_P
    // to prevent previous bad accels from throwing us off
    if (!_ins.healthy()) {
        error[besti].zero();
    } else {
        // convert the error term to body frame
        error[besti] = _dcm_matrix.mul_transpose(error[besti]);
    }

    if (error[besti].is_nan() || error[besti].is_inf()) {
        // don't allow bad values
        check_matrix();
        _last_failure_ms = hal.scheduler->millis();
        return;
    }

    _error_rp = 0.8f * _error_rp + 0.2f * best_error;

    // base the P gain on the spin rate
    float spin_rate = _omega.length();

    // sanity check _kp value
    if (_kp < AP_AHRS_RP_P_MIN) {
        _kp = AP_AHRS_RP_P_MIN;
    }

    // we now want to calculate _omega_P and _omega_I. The
    // _omega_P value is what drags us quickly to the
    // accelerometer reading.
    _omega_P = error[besti] * _P_gain(spin_rate) * _kp;
    if (use_fast_gains()) {
        _omega_P *= 8;
    }

    if (_flags.fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D && 
        _gps.ground_speed() < GPS_SPEED_MIN && 
        _ins.get_accel().x >= 7 &&
	    pitch_sensor > -3000 && pitch_sensor < 3000) {
            // assume we are in a launch acceleration, and reduce the
            // rp gain by 50% to reduce the impact of GPS lag on
            // takeoff attitude when using a catapult
            _omega_P *= 0.5f;
    }

    // accumulate some integrator error
    if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
        _omega_I_sum += error[besti] * _ki * _ra_deltat;
        _omega_I_sum_time += _ra_deltat;
    }

    if (_omega_I_sum_time >= 5) {
        // limit the rate of change of omega_I to the hardware
        // reported maximum gyro drift rate. This ensures that
        // short term errors don't cause a buildup of omega_I
        // beyond the physical limits of the device
        float change_limit = _gyro_drift_limit * _omega_I_sum_time;
        _omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit);
        _omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit);
        _omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit);
        _omega_I += _omega_I_sum;
        _omega_I_sum.zero();
        _omega_I_sum_time = 0;
    }

    // zero our accumulator ready for the next GPS step
    memset(&_ra_sum[0], 0, sizeof(_ra_sum));
    _ra_deltat = 0;
    _ra_sum_start = last_correction_time;

    // remember the velocity for next time
    _last_velocity = velocity;
}
Esempio n. 18
0
/** Navigation along a figure 8. The cross center is defined by the waypoint
    [target], the center of one of the circles is defined by [c1]. Altitude is
    given by [target].
    The navigation goes through 6 states: C1 (circle around [c1]), R1T, RT2
    (route from circle 1 to circle 2 over [target]), C2 and R2T, RT1.
    If necessary, the [c1] waypoint is moved in the direction of [target]
    to be not far than [2*radius].
*/
void nav_eight(uint8_t target, uint8_t c1, float radius)
{
  float aradius = fabs(radius);
  float alt = waypoints[target].a;
  waypoints[c1].a = alt;

  float target_c1_x = waypoints[c1].x - waypoints[target].x;
  float target_c1_y = waypoints[c1].y - waypoints[target].y;
  float d = sqrtf(target_c1_x * target_c1_x + target_c1_y * target_c1_y);
  d = Max(d, 1.); /* To prevent a division by zero */

  /* Unit vector from target to c1 */
  float u_x = target_c1_x / d;
  float u_y = target_c1_y / d;

  /* Move [c1] closer if needed */
  if (d > 2 * aradius) {
    d = 2 * aradius;
    waypoints[c1].x = waypoints[target].x + d * u_x;
    waypoints[c1].y = waypoints[target].y + d * u_y;
  }

  /* The other center */
  struct point c2 = {
    waypoints[target].x - d * u_x,
    waypoints[target].y - d * u_y,
    alt
  };

  struct point c1_in = {
    waypoints[c1].x + radius * -u_y,
    waypoints[c1].y + radius * u_x,
    alt
  };
  struct point c1_out = {
    waypoints[c1].x - radius * -u_y,
    waypoints[c1].y - radius * u_x,
    alt
  };

  struct point c2_in = {
    c2.x + radius * -u_y,
    c2.y + radius * u_x,
    alt
  };
  struct point c2_out = {
    c2.x - radius * -u_y,
    c2.y - radius * u_x,
    alt
  };

  float qdr_out = M_PI - atan2f(u_y, u_x);
  if (radius < 0) {
    qdr_out += M_PI;
  }

  switch (eight_status) {
    case C1 :
      NavCircleWaypoint(c1, radius);
      if (NavQdrCloseTo(DegOfRad(qdr_out) - 10)) {
        eight_status = R1T;
        InitStage();
      }
      return;

    case R1T:
      nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y);
      if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c1_out.x, c1_out.y, 0)) {
        eight_status = RT2;
        InitStage();
      }
      return;

    case RT2:
      nav_route_xy(c1_out.x, c1_out.y, c2_in.x, c2_in.y);
      if (nav_approaching_xy(c2_in.x, c2_in.y, c1_out.x, c1_out.y, CARROT)) {
        eight_status = C2;
        InitStage();
      }
      return;

    case C2 :
      nav_circle_XY(c2.x, c2.y, -radius);
      if (NavQdrCloseTo(DegOfRad(qdr_out) + 10)) {
        eight_status = R2T;
        InitStage();
      }
      return;

    case R2T:
      nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
      if (nav_approaching_xy(waypoints[target].x, waypoints[target].y, c2_out.x, c2_out.y, 0)) {
        eight_status = RT1;
        InitStage();
      }
      return;

    case RT1:
      nav_route_xy(c2_out.x, c2_out.y, c1_in.x, c1_in.y);
      if (nav_approaching_xy(c1_in.x, c1_in.y, c2_out.x, c2_out.y, CARROT)) {
        eight_status = C1;
        InitStage();
      }
      return;

    default:/* Should not occur !!! Doing nothing */
      return;
  } /* switch */
}
Esempio n. 19
0
//void processLanes(CvSeq *lines, IplImage* edges, IplImage *temp_frame)
static void processLanes(CvSeq *lines, IplImage* edges, IplImage *temp_frame, IplImage *org_frame)
{
  /* classify lines to left/right side */
  std::vector<Lane> left, right;

  for (int i=0; i<lines->total; i++)
    {
      CvPoint *line = (CvPoint *)cvGetSeqElem(lines, i);
      int dx = line[1].x - line[0].x;
      int dy = line[1].y - line[0].y;
      float angle = atan2f(dy, dx) * 180/CV_PI;

      if (fabs(angle) <= LINE_REJECT_DEGREES) // reject near horizontal lines
        {
          continue;
        }

      /* assume that vanishing point is close to the image horizontal center
         calculate line parameters: y = kx + b; */
      dx = (dx == 0) ? 1 : dx;  // prevent DIV/0!
      float k = dy/(float)dx;
      float b = line[0].y - k*line[0].x;

      /* assign lane's side based by its midpoint position */
      int midx = (line[0].x + line[1].x) / 2;
      if (midx < temp_frame->width/2)
        {
          left.push_back(Lane(line[0], line[1], angle, k, b));
        }
      else if (midx > temp_frame->width/2)
        {
          right.push_back(Lane(line[0], line[1], angle, k, b));
        }
    }

  /* show Hough lines */
  int org_offset = temp_frame->height;
  for (std::size_t i = 0; i < right.size(); ++i)
    {
      CvPoint org_p0 = right[i].p0;
      org_p0.y += org_offset;
      CvPoint org_p1 = right[i].p1;
      org_p1.y += org_offset;
#ifdef USE_POSIX_SHARED_MEMORY
#ifdef SHOW_DETAIL
      cvLine(temp_frame, right[i].p0, right[i].p1, BLUE, 2);
#endif
      cvLine(org_frame, org_p0, org_p1, BLUE, 2);
#endif
    }
  for (std::size_t i = 0; i < left.size(); ++i)
    {
      CvPoint org_p0 = left[i].p0;
      org_p0.y += org_offset;
      CvPoint org_p1 = left[i].p1;
      org_p1.y += org_offset;
#ifdef USE_POSIX_SHARED_MEMORY
#ifdef SHOW_DETAIL
      cvLine(temp_frame, left[i].p0, left[i].p1, RED, 2);
#endif
      cvLine(org_frame, org_p0, org_p1, RED, 2);
#endif
    }

  processSide(left, edges, false);
  processSide(right, edges, true);

  /* show computed lanes */
  int x = temp_frame->width * 0.55f;
  int x2 = temp_frame->width;
#if defined(USE_POSIX_SHARED_MEMORY)
#ifdef SHOW_DETAIL
  cvLine(temp_frame, cvPoint(x, laneR.k.get()*x + laneR.b.get()),
         cvPoint(x2, laneR.k.get()*x2 + laneR.b.get()), PURPLE, 2);
#endif

  cvLine(org_frame, cvPoint(x, laneR.k.get()*x + laneR.b.get() + org_offset),
         cvPoint(x2, laneR.k.get()*x2 + laneR.b.get() + org_offset), PURPLE, 2);
#else
  lane_detector::ImageLaneObjects lane_msg;
  lane_msg.lane_r_x1 = x;
  lane_msg.lane_r_y1 = laneR.k.get()*x + laneR.b.get() + org_offset;
  lane_msg.lane_r_x2 = x2;
  lane_msg.lane_r_y2 = laneR.k.get()*x2 + laneR.b.get() + org_offset;
#endif

  x = temp_frame->width * 0;
  x2 = temp_frame->width * 0.45f;
#if defined(USE_POSIX_SHARED_MEMORY)
#ifdef SHOW_DETAIL
  cvLine(temp_frame, cvPoint(x, laneL.k.get()*x + laneL.b.get()),
         cvPoint(x2, laneL.k.get()*x2 + laneL.b.get()), PURPLE, 2);
#endif

  cvLine(org_frame, cvPoint(x, laneL.k.get()*x + laneL.b.get() + org_offset),
         cvPoint(x2, laneL.k.get()*x2 + laneL.b.get() + org_offset), PURPLE, 2);
#else
  lane_msg.lane_l_x1 = x;
  lane_msg.lane_l_y1 = laneL.k.get()*x + laneL.b.get() + org_offset;
  lane_msg.lane_l_x2 = x2;
  lane_msg.lane_l_y2 = laneL.k.get()*x2 + laneL.b.get() + org_offset;

  image_lane_objects.publish(lane_msg);
#endif
  // cvLine(org_frame, cvPoint(lane_msg.lane_l_x1, lane_msg.lane_l_y1), cvPoint(lane_msg.lane_l_x2, lane_msg.lane_l_y2), RED, 5);
  // cvLine(org_frame, cvPoint(lane_msg.lane_r_x1, lane_msg.lane_r_y1), cvPoint(lane_msg.lane_r_x2, lane_msg.lane_r_y2), RED, 5);
}
Esempio n. 20
0
void nav_oval(uint8_t p1, uint8_t p2, float radius)
{
  radius = - radius; /* Historical error ? */

  float alt = waypoints[p1].a;
  waypoints[p2].a = alt;

  float p2_p1_x = waypoints[p1].x - waypoints[p2].x;
  float p2_p1_y = waypoints[p1].y - waypoints[p2].y;
  float d = sqrtf(p2_p1_x * p2_p1_x + p2_p1_y * p2_p1_y);

  /* Unit vector from p1 to p2 */
  float u_x = p2_p1_x / d;
  float u_y = p2_p1_y / d;

  /* The half circle centers and the other leg */
  struct point p1_center = { waypoints[p1].x + radius * -u_y,
           waypoints[p1].y + radius * u_x,
           alt
  };
  struct point p1_out = { waypoints[p1].x + 2 * radius * -u_y,
           waypoints[p1].y + 2 * radius * u_x,
           alt
  };

  struct point p2_in = { waypoints[p2].x + 2 * radius * -u_y,
           waypoints[p2].y + 2 * radius * u_x,
           alt
  };
  struct point p2_center = { waypoints[p2].x + radius * -u_y,
           waypoints[p2].y + radius * u_x,
           alt
  };

  float qdr_out_2 = M_PI - atan2f(u_y, u_x);
  float qdr_out_1 = qdr_out_2 + M_PI;
  if (radius < 0) {
    qdr_out_2 += M_PI;
    qdr_out_1 += M_PI;
  }
  float qdr_anticipation = (radius > 0 ? -15 : 15);

  switch (oval_status) {
    case OC1 :
      nav_circle_XY(p1_center.x, p1_center.y, -radius);
      if (NavQdrCloseTo(DegOfRad(qdr_out_1) - qdr_anticipation)) {
        oval_status = OR12;
        InitStage();
        LINE_START_FUNCTION;
      }
      return;

    case OR12:
      nav_route_xy(p1_out.x, p1_out.y, p2_in.x, p2_in.y);
      if (nav_approaching_xy(p2_in.x, p2_in.y, p1_out.x, p1_out.y, CARROT)) {
        oval_status = OC2;
        nav_oval_count++;
        InitStage();
        LINE_STOP_FUNCTION;
      }
      return;

    case OC2 :
      nav_circle_XY(p2_center.x, p2_center.y, -radius);
      if (NavQdrCloseTo(DegOfRad(qdr_out_2) - qdr_anticipation)) {
        oval_status = OR21;
        InitStage();
        LINE_START_FUNCTION;
      }
      return;

    case OR21:
      nav_route_xy(waypoints[p2].x, waypoints[p2].y, waypoints[p1].x, waypoints[p1].y);
      if (nav_approaching_xy(waypoints[p1].x, waypoints[p1].y, waypoints[p2].x, waypoints[p2].y, CARROT)) {
        oval_status = OC1;
        InitStage();
        LINE_STOP_FUNCTION;
      }
      return;

    default: /* Should not occur !!! Doing nothing */
      return;
  }
}
Esempio n. 21
0
extern "C" int main(int argc, char **argv)
{
#if _WIN32
	// hack to redirect output to text files
	freopen("stdout.txt", "w", stdout);
	freopen("stderr.txt", "w", stderr);
#endif

	clientConsole = new narf::ClientConsole();
	narf::console = clientConsole;

	narf::console->println("Version: " + std::to_string(VERSION_MAJOR) + "." + std::to_string(VERSION_MINOR) + std::string(VERSION_RELEASE) + "+" VERSION_REV);

	if (enet_initialize() != 0) {
		narf::console->println("Error initializing ENet");
		return 1;
	}

	narf::cmd::cmds["set"] = cmdSet;
	narf::cmd::cmds["quit"] = cmdQuit;
	narf::cmd::cmds["connect"] = cmdConnect;
	narf::cmd::cmds["disconnect"] = cmdDisconnect;
	narf::cmd::cmds["save"] = cmdSave;
	narf::cmd::cmds["saveconfig"] = cmdSaveConfig;
	narf::cmd::cmds["load"] = cmdLoad;
	narf::cmd::cmds["stats"] = cmdStats;
	narf::cmd::cmds["about"] = cmdAbout;
	narf::cmd::cmds["music"] = cmdMusic;

	narf::MemoryFile iniMem;
	auto configFile = narf::util::appendPath(narf::util::userConfigDir("narfblock"), "client.ini");
	narf::console->println("Attempting to open user config file: " + configFile);
	if (!iniMem.read(configFile)) {
		configFile = narf::util::appendPath(narf::util::dataDir(), "client.ini");
		narf::console->println("Could not load user config file; falling back to local config file: " + configFile);
		if (!iniMem.read(configFile)) {
			narf::console->println("Could not load local config file; falling back to compile-time defaults");
		}
	}

	if (iniMem.size) {
		config.load(iniMem.data, iniMem.size);
	}

	config.updateSignal += configEvent;

	client = enet_host_create(nullptr, 1, narf::net::MAX_CHANNELS, 0, 0);
	if (!client) {
		fatalError("Could not create ENet client");
		return 1;
	}

	if (SDL_Init(SDL_INIT_TIMER | SDL_INIT_VIDEO | SDL_INIT_AUDIO) < 0) {
		fatalError("SDL_Init() failed: " + std::string(SDL_GetError()));
		SDL_Quit();
		return 1;
	}

	musicMutex = SDL_CreateMutex();
	config.initBool("audio.enabled", true);

	SDL_DisplayMode mode;
	// TODO: iterate over monitors?
	if (SDL_GetDesktopDisplayMode(0, &mode)) {
		fatalError("SDL_GetDesktopDisplayMode failed: " + std::string(SDL_GetError()));
		SDL_Quit();
		return 1;
	}

	auto w = mode.w;
	auto h = mode.h;

	narf::console->println("Current video mode is " + std::to_string(w) + "x" + std::to_string(h));

	// TODO: convert these to be modifiable at runtime and use config.init*
	bool fullscreen = config.getBool("video.fullscreen", true);
	float width_cfg = config.getFloat("video.width", 0.6f);
	float height_cfg = config.getFloat("video.height", 0.6f);
	if (!fullscreen) {
		if (width_cfg > 1) {
			w = (int32_t)width_cfg;
		} else {
			w = (int32_t)((float)w * width_cfg);
		}
		if (height_cfg > 1) {
			h = (int32_t)height_cfg;
		} else {
			h = (int32_t)((float)h * height_cfg);
		}
		narf::console->println("Setting video to windowed " + std::to_string(w) + "x" + std::to_string(h));
	} else {
		narf::console->println("Setting video to fullscreen");
	}

	if (!initVideo(w, h, fullscreen)) {
		fatalError("Error: could not set OpenGL video mode " + std::to_string(w) + "x" + std::to_string(h));
		SDL_Quit();
		return 1;
	}

	clientConsole->setGLContext(display);

	config.initBool("video.vsync", false);

	newWorld();

	playerEID = world->entityManager.newEntity();
	{
		narf::EntityRef player(world->entityManager, playerEID);

		// initial player position
		player->position = narf::Vector3f(15.0f, 10.0f, 3.0f * 16.0f);
		player->prevPosition = player->position;
	}

	// initialize camera to look at origin
	cam.orientation.yaw = atan2f(cam.position.y, cam.position.x);
	cam.orientation.pitch = 0.0f;

	bouncyBlockEID = world->entityManager.newEntity();
	{
		narf::EntityRef bouncyBlock(world->entityManager, bouncyBlockEID);
		bouncyBlock->position = narf::Vector3f(10.0f, 10.0f, 21.0f);
		bouncyBlock->prevPosition = bouncyBlock->position;
		bouncyBlock->bouncy = true;
		bouncyBlock->model = true;
	}

	if (!init_textures()) {
		fatalError("init_textures() failed");
		return 1;
	}

	renderer = new narf::Renderer(world, *display, tilesTex);

	config.initInt32("video.renderDistance", 5);

	fpsTextBuffer = new narf::font::TextBuffer(*display, nullptr);
	blockInfoBuffer = new narf::font::TextBuffer(*display, nullptr);
	entityInfoBuffer = new narf::font::TextBuffer(*display, nullptr);
	locationBuffer = new narf::font::TextBuffer(*display, nullptr);

	config.initString("video.hudFont", "DroidSansMono");
	config.initInt32("video.hudFontSize", 30);
	if (!fpsTextBuffer->getFont()) {
		fatalError("Error: could not load HUD font");
		return 1;
	}

	config.initString("video.consoleFont", "DroidSansMono");
	config.initInt32("video.consoleFontSize", 18);
	if (!clientConsole->getFont()) {
		fatalError("Error: could not load Console font");
		return 1;
	}

	config.initString("video.consoleCursorShape", "default");

	config.initBool("video.stereo.enabled", false);
	config.initBool("video.stereo.cross", true);
	config.initFloat("video.stereo.separation", 0.1f);

	SDL_SetRelativeMouseMode(SDL_TRUE);

	config.initDouble("misc.tickRate", 60);
	config.initDouble("misc.maxFrameTime", 0.25);

	narf::console->println("Unicode test\xE2\x80\xBC pi: \xCF\x80 (2-byte sequence), square root: \xE2\x88\x9A (3-byte sequence), U+2070E: \xF0\xA0\x9C\x8E (4-byte sequence)");

	gameLoop = new ClientGameLoop(config.getDouble("misc.maxFrameTime"), config.getDouble("misc.tickRate"));
	gameLoop->run();

	if (connectState == ConnectState::Connected) {
		enet_peer_disconnect(server, (uint32_t)narf::net::DisconnectType::UserQuit);
	} else if (connectState == ConnectState::Connecting) {
		enet_peer_reset(server);
	}

	enet_host_flush(client);
	enet_host_destroy(client);
	enet_deinitialize();
	SDL_Quit();
	return 0;
}
Esempio n. 22
0
int data_fusion(mpudata_t *mpu)
{
	quaternion_t dmpQuat;
	vector3d_t dmpEuler;
	quaternion_t magQuat;
	quaternion_t unfusedQuat;
	float deltaDMPYaw;
	float deltaMagYaw;
	float newMagYaw;
	float newYaw;
	
	dmpQuat[QUAT_W] = (float)mpu->rawQuat[QUAT_W];
	dmpQuat[QUAT_X] = (float)mpu->rawQuat[QUAT_X];
	dmpQuat[QUAT_Y] = (float)mpu->rawQuat[QUAT_Y];
	dmpQuat[QUAT_Z] = (float)mpu->rawQuat[QUAT_Z];

	quaternionNormalize(dmpQuat);	
	quaternionToEuler(dmpQuat, dmpEuler);

	mpu->fusedEuler[VEC3_X] = dmpEuler[VEC3_X];
	mpu->fusedEuler[VEC3_Y] = -dmpEuler[VEC3_Y];
	mpu->fusedEuler[VEC3_Z] = 0;

	eulerToQuaternion(mpu->fusedEuler, unfusedQuat);

	deltaDMPYaw = -dmpEuler[VEC3_Z] + mpu->lastDMPYaw;
	mpu->lastDMPYaw = dmpEuler[VEC3_Z];

	magQuat[QUAT_W] = 0;
	magQuat[QUAT_X] = mpu->calibratedMag[VEC3_X];
  	magQuat[QUAT_Y] = mpu->calibratedMag[VEC3_Y];
  	magQuat[QUAT_Z] = mpu->calibratedMag[VEC3_Z];

	tilt_compensate(magQuat, unfusedQuat);

	newMagYaw = -atan2f(magQuat[QUAT_Y], magQuat[QUAT_X]);

	if (newMagYaw != newMagYaw) {
		printf("newMagYaw NAN\n");
		return -1;
	}

	if (newMagYaw < 0.0f)
		newMagYaw = TWO_PI + newMagYaw;

	newYaw = mpu->lastYaw + deltaDMPYaw;

	if (newYaw > TWO_PI)
		newYaw -= TWO_PI;
	else if (newYaw < 0.0f)
		newYaw += TWO_PI;
	 
	deltaMagYaw = newMagYaw - newYaw;
	
	if (deltaMagYaw >= (float)M_PI)
		deltaMagYaw -= TWO_PI;
	else if (deltaMagYaw < -(float)M_PI)
		deltaMagYaw += TWO_PI;

	if (yaw_mixing_factor > 0)
		newYaw += deltaMagYaw / yaw_mixing_factor;

	if (newYaw > TWO_PI)
		newYaw -= TWO_PI;
	else if (newYaw < 0.0f)
		newYaw += TWO_PI;

	mpu->lastYaw = newYaw;

	if (newYaw > (float)M_PI)
		newYaw -= TWO_PI;

	mpu->fusedEuler[VEC3_Z] = newYaw;

	eulerToQuaternion(mpu->fusedEuler, mpu->fusedQuat);

	return 0;
}
static inline float angle_precise(const complex32_t t) {
	return atan2f(t.imag(), t.real());
}
Esempio n. 24
0
/** Returns information on the parameters needed to hit a target kart moving
 *  at constant velocity and direction for a given speed in the XZ-plane.
 *  \param origin Location of the kart shooting the item.
 *  \param target_kart Which kart to target.
 *  \param item_xz_speed Speed of the item projected in XZ plane.
 *  \param gravity The gravity used for this item.
 *  \param forw_offset How far ahead of the kart the item is shot (so that
 *         the item does not originate inside of the shooting kart.
 *  \param fire_angle Returns the angle to fire the item at.
 *  \param up_velocity Returns the upwards velocity to use for the item.
 */
void Flyable::getLinearKartItemIntersection (const Vec3 &origin,
                                             const AbstractKart *target_kart,
                                             float item_XZ_speed,
                                             float gravity, float forw_offset,
                                             float *fire_angle,
                                             float *up_velocity)
{
    // Transform the target into the firing kart's frame of reference
    btTransform inv_trans = m_owner->getTrans().inverse();
    
    Vec3 relative_target_kart_loc = inv_trans(target_kart->getXYZ());
    
    // Find the direction target is moving in
    btTransform trans = target_kart->getTrans();
    Vec3 target_direction(trans.getBasis().getColumn(2));

    // Now rotate it to the firing kart's frame of reference
    btQuaternion inv_rotate = inv_trans.getRotation();
    target_direction = 
        target_direction.rotate(inv_rotate.getAxis(), inv_rotate.getAngle());
    
    // Now we try to find the angle to aim at to hit the target. 
    // Warning : Funky math stuff going on below. To understand, see answer by 
    // Jeffrey Hantin here : 
    // http://stackoverflow.com/questions/2248876/2d-game-fire-at-a-moving-target-by-predicting-intersection-of-projectile-and-u
    
    float target_x_speed = target_direction.getX()*target_kart->getSpeed();
    float target_z_speed = target_direction.getZ()*target_kart->getSpeed();
    float target_y_speed = target_direction.getY()*target_kart->getSpeed();

    float a = (target_x_speed*target_x_speed) + (target_z_speed*target_z_speed) -
                (item_XZ_speed*item_XZ_speed);
    float b = 2 * (target_x_speed * (relative_target_kart_loc.getX())
                    + target_z_speed * (relative_target_kart_loc.getZ()));
    float c = relative_target_kart_loc.getX()*relative_target_kart_loc.getX()
                + relative_target_kart_loc.getZ()*relative_target_kart_loc.getZ();
    
    float discriminant = b*b - 4 * a*c;
    if (discriminant < 0) discriminant = 0;

    float t1 = (-b + sqrt(discriminant)) / (2 * a);
    float t2 = (-b - sqrt(discriminant)) / (2 * a);
    float time;
    if (t1 >= 0 && t1<t2) time = t1;
    else time = t2;

    //createPhysics offset
    time -= forw_offset / item_XZ_speed;

    float aimX = time*target_x_speed + relative_target_kart_loc.getX();
    float aimZ = time*target_z_speed + relative_target_kart_loc.getZ();

    assert(time!=0);
    float angle = atan2f(aimX, aimZ);
    
    *fire_angle = angle;

    // Now find the up_velocity. This is an application of newton's equation.
    *up_velocity = (0.5f * time * gravity) + (relative_target_kart_loc.getY() / time)
                 + ( target_y_speed);
}   // getLinearKartItemIntersection
Esempio n. 25
0
bool CICP::CalculateOptimalTransformation(const Vec3d *pSourcePoints, const Vec3d *pTargetPoints, int nPoints, Mat3d &rotation, Vec3d &translation)
{
	if (nPoints < 2)
	{
		printf("error: CICP::CalculateOptimalTransformation needs at least two point pairs");
		Math3d::SetMat(rotation, Math3d::unit_mat);
		Math3d::SetVec(translation, Math3d::zero_vec);
		return false;
	}

	// The solution is based on
	// Berthold K. P. Horn (1987),
	// "Closed-form solution of absolute orientation using unit quaternions,"
	// Journal of the Optical Society of America A, pp. 629-642
	// Original python implementation by David G. Gobbi.
	
	// find the centroid of each set
	Vec3d source_centroid = { 0.0f, 0.0f, 0.0f };
	Vec3d target_centroid = { 0.0f, 0.0f, 0.0f };
	
	int i;
  
	for (i = 0; i < nPoints; i++)
	{
		Math3d::AddToVec(source_centroid, pSourcePoints[i]);
		Math3d::AddToVec(target_centroid, pTargetPoints[i]);
	}

	Math3d::MulVecScalar(source_centroid, 1.0f / nPoints, source_centroid);
	Math3d::MulVecScalar(target_centroid, 1.0f / nPoints, target_centroid);
  
	// build the 3x3 matrix M
	Mat3d M = { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
	
	for (i = 0; i < nPoints; i++)
	{
		Vec3d a, b;
		Mat3d matrix;

		Math3d::SubtractVecVec(pSourcePoints[i], source_centroid, a);
		Math3d::SubtractVecVec(pTargetPoints[i], target_centroid, b);
		    
		// accumulate the products a * b^T into the matrix M
		Math3d::MulVecTransposedVec(a, b, matrix);
		Math3d::AddToMat(M, matrix);
	}

	// build the 4x4 matrix N
	double N0[4], N1[4], N2[4], N3[4];
	double *N[4] = { N0, N1, N2, N3 };

	for (i = 0; i < 4; i++)
	{
		// fill N with zeros
		N[i][0] = 0.0f;
		N[i][1] = 0.0f;
		N[i][2] = 0.0f;
		N[i][3] = 0.0f;
	}

	// on-diagonal elements
	N[0][0] =  M.r1 + M.r5 + M.r9;
	N[1][1] =  M.r1 - M.r5 - M.r9;
	N[2][2] = -M.r1 + M.r5 - M.r9;
	N[3][3] = -M.r1 - M.r5 + M.r9;

	// off-diagonal elements
	N[0][1] = N[1][0] = M.r6 - M.r8;
	N[0][2] = N[2][0] = M.r7 - M.r3;
	N[0][3] = N[3][0] = M.r2 - M.r4;

	N[1][2] = N[2][1] = M.r2 + M.r4;
	N[1][3] = N[3][1] = M.r7 + M.r3;
	N[2][3] = N[3][2] = M.r6 + M.r8;

	double eigenvalues[4];
	double eigenvectors0[4], eigenvectors1[4], eigenvectors2[4], eigenvectors3[4];
	double *eigenvectors[4] = { eigenvectors0, eigenvectors1, eigenvectors2, eigenvectors3 };

	// eigen-decompose N (is symmetric)
	Jacobi4(N, eigenvalues, eigenvectors);

	// the eigenvector with the largest eigenvalue is the quaternion we want
	// (they are sorted in decreasing order for us by JacobiN)
	float w, x, y, z;

	// first: if points are collinear, choose the quaternion that 
	// results in the smallest rotation.
	if (eigenvalues[0] == eigenvalues[1] || nPoints == 2)
	{
		Vec3d s0, t0, s1, t1;
		Math3d::SetVec(s0, pSourcePoints[0]);
		Math3d::SetVec(t0, pTargetPoints[0]);
		Math3d::SetVec(s1, pSourcePoints[1]);
		Math3d::SetVec(t1, pTargetPoints[1]);

		Vec3d ds, dt;
		Math3d::SubtractVecVec(s1, s0, ds);
		Math3d::SubtractVecVec(t1, t0, dt);
		Math3d::NormalizeVec(ds);
		Math3d::NormalizeVec(dt);
  
		// take dot & cross product
		w = ds.x * dt.x + ds.y * dt.y + ds.z * dt.z;
		x = ds.y * dt.z - ds.z * dt.y;
		y = ds.z * dt.x - ds.x * dt.z;
		z = ds.x * dt.y - ds.y * dt.x;

		float r = sqrtf(x * x + y * y + z * z);
		const float theta = atan2f(r, w);

		// construct quaternion
		w = cosf(0.5f * theta);
  
		if (r != 0)
		{
			r = sinf(0.5f * theta) / r;
			x = x * r;
			y = y * r;
			z = z * r;
		}
		else // rotation by 180 degrees: special case
		{
			// rotate around a vector perpendicular to ds
			double ds_[3] = { ds.x, ds.y, ds.z };
			double dt_[3];
						
			Perpendiculars(ds_, dt_, 0, 0);
			
			r = sinf(0.5f * theta);
			x = float(dt_[0] * r);
			y = float(dt_[1] * r);
			z = float(dt_[2] * r);
		}
	}
	else
	{
		// points are not collinear
		w = (float) eigenvectors[0][0];
		x = (float) eigenvectors[1][0];
		y = (float) eigenvectors[2][0];
		z = (float) eigenvectors[3][0];
	}

	// convert quaternion to a rotation matrix
	const float ww = w * w;
	const float wx = w * x;
	const float wy = w * y;
	const float wz = w * z;

	const float xx = x * x;
	const float yy = y * y;
	const float zz = z * z;

	const float xy = x * y;
	const float xz = x * z;
	const float yz = y * z;

	rotation.r1 = ww + xx - yy - zz; 
	rotation.r4 = 2.0f * (wz + xy);
	rotation.r7 = 2.0f * (-wy + xz);

	rotation.r2 = 2.0f * (-wz + xy);  
	rotation.r5 = ww - xx + yy - zz;
	rotation.r8 = 2.0f * (wx + yz);

	rotation.r3 = 2.0f * (wy + xz);
	rotation.r6 = 2.0f * (-wx + yz);
	rotation.r9 = ww - xx - yy + zz;

	// the translation is given by the difference of the transformed
	// source centroid and the target centroid
	Vec3d temp;
	Math3d::MulMatVec(rotation, source_centroid, temp);
	Math3d::SubtractVecVec(target_centroid, temp, translation);
	
	return true;
}
Esempio n. 26
0
/** Updates this flyable. It calls Moveable::update. If this function returns
 *  true, the flyable will be deleted by the projectile manager.
 *  \param dt Time step size.
 *  \returns True if this object can be deleted.
 */
bool Flyable::updateAndDelete(int ticks)
{
    if (m_undo_creation)
        return false;

    if (hasAnimation())
    {
        if (!RewindManager::get()->isRewinding())
        {
            m_animation->update(ticks);
            Moveable::update(ticks);
        }
        return false;
    }   // if animation

    m_ticks_since_thrown += ticks;
    if(m_max_lifespan > -1 && m_ticks_since_thrown > m_max_lifespan)
        hit(NULL);

    if(m_has_hit_something) return true;

    //Vec3 xyz=getBody()->getWorldTransform().getOrigin();
    const Vec3 &xyz=getXYZ();
    // Check if the flyable is outside of the track. If so, explode it.
    const Vec3 *min, *max;
    Track::getCurrentTrack()->getAABB(&min, &max);

    // I have seen that the bullet AABB can be slightly different from the
    // one computed here - I assume due to minor floating point errors
    // (e.g. 308.25842 instead of 308.25845). To avoid a crash with a bullet
    // assertion (see bug 3058932) I add an epsilon here - but admittedly
    // that does not really explain the bullet crash, since bullet tests
    // against its own AABB, and should therefore not cause the assertion.
    // But since we couldn't reproduce the problem, and the epsilon used
    // here does not hurt, I'll leave it in.
    float eps = 0.1f;
    assert(!std::isnan(xyz.getX()));
    assert(!std::isnan(xyz.getY()));
    assert(!std::isnan(xyz.getZ()));
    if(xyz[0]<(*min)[0]+eps || xyz[2]<(*min)[2]+eps || xyz[1]<(*min)[1]+eps ||
       xyz[0]>(*max)[0]-eps || xyz[2]>(*max)[2]-eps || xyz[1]>(*max)[1]-eps   )
    {
        hit(NULL);    // flyable out of track boundary
        return true;
    }

    if (m_do_terrain_info)
    {
        Vec3 towards = getBody()->getGravity();
        towards.normalize();
        // Add the position offset so that the flyable can adjust its position
        // (usually to do the raycast from a slightly higher position to avoid
        // problems finding the terrain in steep uphill sections).
        // Towards is a unit vector. so we can multiply -towards to offset the
        // position by one unit.
        TerrainInfo::update(xyz + m_position_offset*(-towards), towards);

        // Make flyable anti-gravity when the it's projected on such surface
        const Material* m = TerrainInfo::getMaterial();
        if (m && m->hasGravity())
        {
            getBody()->setGravity(TerrainInfo::getNormal() * -70.0f);
        }
        else
        {
            getBody()->setGravity(Vec3(0, 1, 0) * -70.0f);
        }
    }

    if(m_adjust_up_velocity)
    {
        float hat = (xyz - getHitPoint()).length();

        // Use the Height Above Terrain to set the Z velocity.
        // HAT is clamped by min/max height. This might be somewhat
        // unphysical, but feels right in the game.

        float delta = m_average_height - std::max(std::min(hat, m_max_height),
                                                  m_min_height);
        Vec3 v = getVelocity();
        assert(!std::isnan(v.getX()));
        assert(!std::isnan(v.getX()));
        assert(!std::isnan(v.getX()));
        float heading = atan2f(v.getX(), v.getZ());
        assert(!std::isnan(heading));
        float pitch   = getTerrainPitch(heading);
        float vel_up = m_force_updown*(delta);
        if (hat < m_max_height) // take into account pitch of surface
            vel_up += v.length_2d()*tanf(pitch);
        assert(!std::isnan(vel_up));
        v.setY(vel_up);
        setVelocity(v);
    }   // if m_adjust_up_velocity

    Moveable::update(ticks);

    return false;
}   // updateAndDelete
	float getAngle() {
		return ofRadToDeg(atan2f(position.y, position.x));
	}
Esempio n. 28
0
/*-----------------------------------------------------------------------------
	チートカメラ操作(スクショなど撮る際)
-----------------------------------------------------------------------------*/
void CCamera::OperateCameraMouse(void)
{
	// キーボード取得
	CInputKeyboard* pInputKeyboard = CManager::GetInputKeyboard();

	// マウス取得
	CInputMouse* pInputMouse = CManager::GetInputMouse();
	float fValueX, fValueY, fValueZ;
	fValueX = (float)pInputMouse->GetAxisX();
	fValueY = (float)pInputMouse->GetAxisY();
	fValueZ = (float)pInputMouse->GetAxisZ();

	// マウス左押下ドラッグにより視点の移動
	if(pInputMouse->GetLeftPress())
	{
		// マウス右押下時ドラッグにより視点の旋回
		if(pInputMouse->GetRightPress())
		{
			// 左旋回
			if(fValueX < 0)
			{
				m_rot.y += D3DX_PI * fValueX * CAMERA_MOUSE_ROTATE;
			
				// PI以上または-PI以上ならば角度を正規化
				if(m_rot.y < -D3DX_PI)
				{
					m_rot.y += D3DX_PI * 2.0f;
				}
				else if(m_rot.y > D3DX_PI)
				{
					m_rot.y -= D3DX_PI * 2.0f;
				}

				m_posEye.x = m_posLook.x - sinf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
				m_posEye.z = m_posLook.z - cosf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
			}
			// 右旋回
			else if(fValueX > 0)
			{
				m_rot.y += D3DX_PI * fValueX * CAMERA_MOUSE_ROTATE;
			
				// PI以上または-PI以上ならば角度を正規化
				if(m_rot.y < -D3DX_PI)
				{
					m_rot.y += D3DX_PI * 2.0f;
				}
				else if(m_rot.y > D3DX_PI)
				{
					m_rot.y -= D3DX_PI * 2.0f;
				}

				m_posEye.x = m_posLook.x - sinf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
				m_posEye.z = m_posLook.z - cosf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
			}
			// 上旋回
			if(fValueY < 0)
			{
				m_rot.x -= D3DX_PI * fValueY * CAMERA_MOUSE_ROTATE;
			
				if(m_rot.x > (D3DX_PI/2.0f - D3DX_PI*0.02f))
				{
					m_rot.x = (D3DX_PI/2.0f - D3DX_PI*0.02f);
				}

				m_posEye.y = m_posLook.y - sinf(m_rot.x)
							* m_fDistanceEyeLookatAxisXYZ;

				m_fDistanceEyeLookatAxisXZ = cosf(m_rot.x)
											* m_fDistanceEyeLookatAxisXYZ;
				m_posEye.x = m_posLook.x - sinf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
				m_posEye.z = m_posLook.z - cosf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
			}
			// 下旋回
			else if(fValueY > 0)
			{
				m_rot.x -= D3DX_PI * fValueY * CAMERA_MOUSE_ROTATE;
			
				if(m_rot.x < (-D3DX_PI/2.0f + D3DX_PI*0.02f))
				{
					m_rot.x = (-D3DX_PI/2.0f + D3DX_PI*0.02f);
				}

				m_posEye.y = m_posLook.y - sinf(m_rot.x)
							* m_fDistanceEyeLookatAxisXYZ;

				m_fDistanceEyeLookatAxisXZ = cosf(m_rot.x)
											* m_fDistanceEyeLookatAxisXYZ;
				m_posEye.x = m_posLook.x - sinf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
				m_posEye.z = m_posLook.z - cosf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
			}
		}
		else
		{
			if(fValueX != 0 || fValueY != 0)
			{
				float angle = atan2f(fValueX, fValueY);
				float length = sqrtf(fValueX * fValueX + fValueY * fValueY)
								* CAMERA_MOUSE_DRAG;

				angle -= m_rot.y;
				if(angle < -D3DX_PI) angle += D3DX_PI * 2.0f;

				m_posLook.x -= sinf(angle) * length;
				m_posLook.z += cosf(angle) * length;

				m_posEye.x = m_posLook.x - sinf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
				m_posEye.z = m_posLook.z - cosf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
			}
		}
	}
	// マウス右押下時ドラッグにより注視点旋回
	else if(pInputMouse->GetRightPress())
	{
		// 左旋回
		if(fValueX < 0)
		{
			m_rot.y += D3DX_PI * fValueX * CAMERA_MOUSE_ROTATE;
			
			// PI以上または-PI以上ならば角度を正規化
			if(m_rot.y < -D3DX_PI)
			{
				m_rot.y += D3DX_PI * 2.0f;
			}
			else if(m_rot.y > D3DX_PI)
			{
				m_rot.y -= D3DX_PI * 2.0f;
			}

			m_posLook.x = m_posEye.x + sinf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
			m_posLook.z = m_posEye.z + cosf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
		}
		// 右旋回
		else if(fValueX > 0)
		{
			m_rot.y += D3DX_PI * fValueX * CAMERA_MOUSE_ROTATE;
			
			// PI以上または-PI以上ならば角度を正規化
			if(m_rot.y < -D3DX_PI)
			{
				m_rot.y += D3DX_PI * 2.0f;
			}
			else if(m_rot.y > D3DX_PI)
			{
				m_rot.y -= D3DX_PI * 2.0f;
			}

			m_posLook.x = m_posEye.x + sinf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
			m_posLook.z = m_posEye.z + cosf(m_rot.y)
							* m_fDistanceEyeLookatAxisXZ;
		}
	}
}
Esempio n. 29
0
void ocpnDC::DrawLine( wxCoord x1, wxCoord y1, wxCoord x2, wxCoord y2, bool b_hiqual )
{
    if( dc )
        dc->DrawLine( x1, y1, x2, y2 );
#ifdef ocpnUSE_GL
    else if( ConfigurePen() ) {
        bool b_draw_thick = false;

        float pen_width = wxMax(g_GLMinSymbolLineWidth, m_pen.GetWidth());

        //      Enable anti-aliased lines, at best quality
        if( b_hiqual ) {
            SetGLStipple();

#ifndef __WXQT__
            glEnable( GL_BLEND );
            glEnable( GL_LINE_SMOOTH );
#endif            

            if( pen_width > 1.0 ) {
                GLint parms[2];
                glGetIntegerv( GL_SMOOTH_LINE_WIDTH_RANGE, &parms[0] );
                if( pen_width > parms[1] ) b_draw_thick = true;
                else
                    glLineWidth( pen_width );
            } else
                glLineWidth( pen_width );
        } else {            
            if( pen_width > 1 ) {
                GLint parms[2];
                glGetIntegerv( GL_ALIASED_LINE_WIDTH_RANGE, &parms[0] );
                if( pen_width > parms[1] ) b_draw_thick = true;
                    else
                        glLineWidth( pen_width );
            } else
                glLineWidth( pen_width );
        }
        
        if( b_draw_thick ) DrawGLThickLine( x1, y1, x2, y2, m_pen, b_hiqual );
        else {
            wxDash *dashes;
            int n_dashes = m_pen.GetDashes( &dashes );
            if( n_dashes ) {
                float angle = atan2f( (float) ( y2 - y1 ), (float) ( x2 - x1 ) );
                float cosa = cosf( angle );
                float sina = sinf( angle );
                float t1 = m_pen.GetWidth();
                    
                float lpix = sqrtf( powf(x1 - x2, 2) + powf(y1 - y2, 2) );
                float lrun = 0.;
                float xa = x1;
                float ya = y1;
                float ldraw = t1 * dashes[0];
                float lspace = t1 * dashes[1];

                ldraw = wxMax(ldraw, 4.0);
                lspace = wxMax(lspace, 4.0);
                lpix = wxMin(lpix, 2000.0);
                
                glBegin( GL_LINES );
                while( lrun < lpix ) {
                    //    Dash
                    float xb = xa + ldraw * cosa;
                    float yb = ya + ldraw * sina;

                    if( ( lrun + ldraw ) >= lpix )         // last segment is partial draw
                    {
                        xb = x2;
                        yb = y2;
                        }

                        glVertex2f( xa, ya );
                        glVertex2f( xb, yb );

                        xa = xa + ( lspace + ldraw ) * cosa;
                        ya = ya + ( lspace + ldraw ) * sina;
                        lrun += lspace + ldraw;

                    }
                glEnd();
            } else                    // not dashed
            {
                glBegin( GL_LINES );
                glVertex2i( x1, y1 );
                glVertex2i( x2, y2 );
                glEnd();
            }
        }

        glDisable( GL_LINE_STIPPLE );

        if( b_hiqual ) {
            glDisable( GL_LINE_SMOOTH );
            glDisable( GL_BLEND );
        }
    }
#endif    
}
void Player::SetCalculatePlayerPosition()
{
	float speedXDeltaTime = m_speed * (float)GLOBAL::GetInstance().GetDeltaTime();
	// Check collision between player and static boxes

	OBB playerOBB;
	playerOBB.m_radius = -1.0f;
	if (GetBoundingBoxes().size() > 0)
	{
		playerOBB = GetBoundingBoxes()[0];
	}
	std::vector<OBB> collidingBoxes = CollisionManager::GetInstance()->CalculateLocalPlayerCollisionWithStaticBoxes(playerOBB, m_speed, m_direction);

	for (unsigned int i = 0; i < collidingBoxes.size(); i++)
	{
		if (m_direction.x == 1 || m_direction.x == -1 || m_direction.z == 1 || m_direction.z == -1)
		{
			Sphere playerSphere = Sphere(m_position, m_playerSphere.m_radius - 0.1f);
			playerSphere.m_position.x = m_position.x;
			playerSphere.m_position.z = m_position.z - 1.0f * speedXDeltaTime;
			bool down = CollisionManager::GetInstance()->CheckCollisionWithAllStaticObjects(playerSphere);
			playerSphere.m_position.x = m_position.x;
			playerSphere.m_position.z = m_position.z + 1.0f * speedXDeltaTime;
			bool up = CollisionManager::GetInstance()->CheckCollisionWithAllStaticObjects(playerSphere);
			playerSphere.m_position.x = m_position.x + 1.0f * speedXDeltaTime;
			playerSphere.m_position.z = m_position.z;
			bool right = CollisionManager::GetInstance()->CheckCollisionWithAllStaticObjects(playerSphere);
			playerSphere.m_position.x = m_position.x - 1.0f * speedXDeltaTime;
			playerSphere.m_position.z = m_position.z;
			bool left = CollisionManager::GetInstance()->CheckCollisionWithAllStaticObjects(playerSphere);

			if ((down && m_direction.z == -1) || (up && m_direction.z == 1) || (right && m_direction.x == 1) || (left && m_direction.x == -1))
			{
				SetDirection(DirectX::XMFLOAT3(0.0f, 0.0f, 0.0f));
			}
		}
		else if (collidingBoxes.size() > 1)
		{

			if (CheckSidesIfMultipleCollisions() == true)
			{
				SetDirection(DirectX::XMFLOAT3(0.0f, 0.0f, 0.0f));
				//SetPosition(DirectX::XMFLOAT3(m_position.x - m_direction.x * (float)GLOBAL::GetInstance().GetDeltaTime(), m_position.y, m_position.z - m_direction.z * (float)GLOBAL::GetInstance().GetDeltaTime()));
			}
			else
			{
				CalculatePlayerCubeCollision(collidingBoxes[i]);
			}
		}
		else
		{
			CalculatePlayerCubeCollision(collidingBoxes[i]);
		}
	}

	std::vector<Sphere> collidingSpheres = CollisionManager::GetInstance()->CalculateLocalPlayerCollisionWithStaticSpheres(m_playerSphere, m_speed, m_direction);
	// Check collision between player and static spheres
	for (unsigned int i = 0; i < collidingSpheres.size(); i++)
	{
		float r = collidingSpheres[i].m_radius;
		float deltaZ = m_position.z - collidingSpheres[i].m_position.z;
		float deltaX = m_position.x - collidingSpheres[i].m_position.x;
		float angle = atan2f(deltaZ, deltaX);

		float circleX = cosf(angle) * r;
		float circleY = sinf(angle) * r;

		float dz = collidingSpheres[i].m_position.z - m_position.z;
		float dx = collidingSpheres[i].m_position.x - m_position.x;
		float angle1 = atan2(dz, dx);
		float angle2 = atan2(m_direction.z, m_direction.x);
		float offset = angle1 - angle2;
		

		// Special cases ftw. Dont ask!
		if (angle1 < 0 && angle2 < 0)
		{
			offset *= -1;
		}		
		if (angle2 >= 0 && angle1 < 0)
		{
			offset *= -1;
		}
		if (angle2 >= DirectX::XM_PIDIV2 && angle1 <= -DirectX::XM_PIDIV2)
		{
			offset *= -1;
		}
		if (angle2 <= -DirectX::XM_PIDIV2 && angle1 >= DirectX::XM_PIDIV2)
		{
			offset *= -1;
		}

		// Circle equation:
		// circleX * X + circleY * Y = Radius * Radius
		// Bryt ut så att y blir ensam
		// Y = (Radius * Radius - circleX * X) / circleY		
		float yValue = (r * r - circleX * (circleX + offset)) / circleY;
		
		DirectX::XMFLOAT3 dir = DirectX::XMFLOAT3((circleX + offset) - circleX, 0, yValue - circleY);
		// Normalize
		float length = sqrt(dir.x * dir.x + dir.z * dir.z);
		dir.x = dir.x / length;
		dir.z = dir.z / length;
		SetDirection(dir);
	}
	if (collidingSpheres.size() > 1 || collidingSpheres.size() >= 1 && collidingBoxes.size() >= 1)
	{
		SetDirection(DirectX::XMFLOAT3(0.0f, 0.0f, 0.0f));
	}

	SendPosition(DirectX::XMFLOAT3(m_position.x + m_direction.x * speedXDeltaTime, m_position.y + m_direction.y * speedXDeltaTime, m_position.z + m_direction.z * speedXDeltaTime));
}