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); } } }
// 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 }
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
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; } } }
//------------------------------------------------------------------------------------------------ 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; } } } }
//---------------------------------------------------------------------------------- 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; }
/** * 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); }
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); } } }
// update L1 control for loitering void AP_L1_Control::update_loiter(const struct Location ¢er_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 } }
// 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) ; }
float cargf(fcomplex z) { return (atan2f(F_IM(z), F_RE(z))); }
TEST(math, atan2f) { ASSERT_FLOAT_EQ(0.0f, atan2f(0.0f, 0.0f)); }
// 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; }
/** 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 */ }
//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); }
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; } }
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; }
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()); }
/** 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
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; }
/** 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)); }
/*----------------------------------------------------------------------------- チートカメラ操作(スクショなど撮る際) -----------------------------------------------------------------------------*/ 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; } } }
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)); }