//=========================================================================== int CEyeTrack::ParseBuffer( int nFrames, void * pvSrcBuffer, double * pdTimeStamps, double * pdCoord ){ IScanFrameStruct * pFrame; int i; double dRawX, dRawY, dX, dY; pFrame = (IScanFrameStruct *) pvSrcBuffer; for( i = 0; i < nFrames; i++ ) { CalcTime( pFrame->iFrame, pdTimeStamps); pdTimeStamps++; dRawX = pFrame->iRawX;; dRawY = pFrame->iRawY; TransformCoordinates( dRawX, dRawY, &dX, &dY ); *pdCoord++ = dX; *pdCoord++ = dY; pFrame++; } return 0; }
/* ComputeNewPosition Assumes a uniform motion for each wheel over some period with no slippage. @param l Left meters @param r Right meters Left meters/right meters: If they are positive, the wheel is moving in the forward direction */ void PositionInformation::ComputeNewPosition(double l, double r) { double dx, dy, dtheta; dtheta = ( r - l ) / m_wheelBase; // dy = ((r+l)/(2.0 *dtheta)) * sin(dtheta); // dx = - ((r+l)/(2.0 * dtheta)) * (1.0 - cos(dtheta)) // // BUT // // you need to expand these out using a taylor series approximation // to avoid divide by zero errors. So we end up with a more // complicated expression that looks like this: // // dy = ((r+l)/2.0 ) * ( 1 - dtheta^2 / 3! + dtheta^4/5! ); // dx = - ((r+l)/2.0) * (dtheta/2! - dtheta^3/4! + dtheta^5/6!) // if (fabs(dtheta) > M_PI/36.0) { // anything more than five degrees: simple expression dy = ((r+l)/(2.0 * dtheta)) * sin(dtheta); dx = - ((r+l)/(2.0 * dtheta)) * (1.0 - cos(dtheta)); } else { // anything less than five degrees: more complex expression dy = ((r+l)/2.0 ) * ( 1.0 - (dtheta*dtheta)/ 6.0 + (dtheta*dtheta*dtheta*dtheta)/120.0); dx = - ((r+l)/2.0) * (dtheta/2.0 - (dtheta*dtheta*dtheta)/24.0 + (dtheta*dtheta*dtheta*dtheta*dtheta)/720.0); } TransformCoordinates( dx, dy, dtheta ); m_display_angle = m_theta * (180.0 / M_PI); m_gyro_angle = m_resources.gyro.GetAngle(); }
void VRPNTrackerInputDevice::Update() { if(InputDevice){ { FScopeLock ScopeLock(&CritSect); InputDevice->mainloop(); } for(auto &InputPair : TrackerMap) { TrackerInput &Input = InputPair.Value; if(Input.TrackerDataDirty) { // Before firing events, transform the tracker into the right coordinate space FVector NewPosition; FQuat NewRotation; TransformCoordinates(Input, NewPosition, NewRotation); FRotator NewRotator = NewRotation.Rotator(); FAnalogInputEvent AnalogInputEventX(Input.MotionXKey, FSlateApplication::Get().GetModifierKeys(), 0, 0, 0, 0, NewPosition.X); FSlateApplication::Get().ProcessAnalogInputEvent(AnalogInputEventX); FAnalogInputEvent AnalogInputEventY(Input.MotionYKey, FSlateApplication::Get().GetModifierKeys(), 0, 0, 0, 0, NewPosition.Y); FSlateApplication::Get().ProcessAnalogInputEvent(AnalogInputEventY); FAnalogInputEvent AnalogInputEventZ(Input.MotionZKey, FSlateApplication::Get().GetModifierKeys(), 0, 0, 0, 0, NewPosition.Z); FSlateApplication::Get().ProcessAnalogInputEvent(AnalogInputEventZ); FAnalogInputEvent AnalogInputEventRotX(Input.RotationYawKey, FSlateApplication::Get().GetModifierKeys(), 0, 0, 0, 0, NewRotator.Yaw); FSlateApplication::Get().ProcessAnalogInputEvent(AnalogInputEventRotX); FAnalogInputEvent AnalogInputEventRotY(Input.RotationPitchKey, FSlateApplication::Get().GetModifierKeys(), 0, 0, 0, 0, NewRotator.Pitch); FSlateApplication::Get().ProcessAnalogInputEvent(AnalogInputEventRotY); FAnalogInputEvent AnalogInputEventRotZ(Input.RotationRollKey, FSlateApplication::Get().GetModifierKeys(), 0, 0, 0, 0, NewRotator.Roll); FSlateApplication::Get().ProcessAnalogInputEvent(AnalogInputEventRotZ); Input.TrackerDataDirty = false; } } } }
//=========================================================================== void TTarget::CalcPosition() { TTargetAccess *ta = &TargetAccess[TargetAccess_Count - 1]; TImpuls aImpuls; TFloat aRo, aFi, aRo1, aRo2; TFloat aTempQ = 0, aTempE = 0; TFloat aSumQ = 0, aSumE = 0; // ----------------------------- aImpuls.E1 = 0; aImpuls.E2 = 0; aImpuls.Time = 0; // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // -------------- Расчет координат по методу №1 --------------- // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ for (int i = 0; i < ta->Impuls_Count; i ++) { Get_Ro1_Ro2(ta->Impuls[i], aRo1, aRo2); //Get_Ro_Fi(ta->Impuls[i], aRo, aFi); // ---------------------------------------------------------- fDirectionalCosines.cell[1][0] = -sin(aRo1) + ___Fixed_Far_Ravnosignal_DirectionalCosines.cell[1][0]; fDirectionalCosines.cell[2][0] = sin(aRo2) + ___Fixed_Far_Ravnosignal_DirectionalCosines.cell[2][0]; fDirectionalCosines.cell[0][0] = sqrt(1 - sqr(fDirectionalCosines.cell[1][0]) - sqr(fDirectionalCosines.cell[2][0])); /*fDirectionalCosines.cell[1][0] = sin(aRo*cos(aFi)) + ___Fixed_Far_Ravnosignal_DirectionalCosines.cell[1][0]; fDirectionalCosines.cell[2][0] = sin(aRo*sin(aFi)) + ___Fixed_Far_Ravnosignal_DirectionalCosines.cell[2][0]; fDirectionalCosines.cell[0][0] = sqrt(1 - sqr(fDirectionalCosines.cell[1][0]) - sqr(fDirectionalCosines.cell[2][0]));*/ // --------- Положение цели в системе координат ФАР --------- Calc_Angle(fDirectionalCosines, aTempQ, aTempE); aSumQ = aSumQ + aTempQ; aSumE = aSumE + aTempE; } // --------- Положение цели в системе координат ФАР --------- aSumQ = aSumQ / (TFloat) ta->Impuls_Count; aSumE = aSumE / (TFloat) ta->Impuls_Count; ta->Coord_M1.Far.Q = aSumQ; ta->Coord_M1.Far.E = aSumE; // --------- Положение цели в системе координат GEO --------- TransformCoordinates(tm_Fixed_Geo_Far, ta->Coord_M1.Far.Q, ta->Coord_M1.Far.E , ta->Coord_M1.Geo.Q, ta->Coord_M1.Geo.E); // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // -------------- Расчет координат по методу №2 "Опредиление координат цели по методу накопления амплитуд"--------------- // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ for (int i = 0; i < ta->Impuls_Count; i ++) { aImpuls.E1 += ta->Impuls[i].E1; aImpuls.E2 += ta->Impuls[i].E2; } aImpuls.E1 = aImpuls.E1 / (TFloat) ta->Impuls_Count; aImpuls.E2 = aImpuls.E2 / (TFloat) ta->Impuls_Count; // ----------------------------- Get_Ro1_Ro2(aImpuls, aRo1, aRo2); //Get_Ro_Fi(aImpuls, aRo, aFi); // ----------------------------- fDirectionalCosines.cell[1][0] = -sin(aRo1) + ___Fixed_Far_Ravnosignal_DirectionalCosines.cell[1][0]; fDirectionalCosines.cell[2][0] = sin(aRo2) + ___Fixed_Far_Ravnosignal_DirectionalCosines.cell[2][0]; fDirectionalCosines.cell[0][0] = sqrt(1 - sqr(fDirectionalCosines.cell[1][0]) - sqr(fDirectionalCosines.cell[2][0])); /*fDirectionalCosines.cell[1][0] = sin(aRo*cos(aFi)) + ___Fixed_Far_Ravnosignal_DirectionalCosines.cell[1][0]; fDirectionalCosines.cell[2][0] = sin(aRo*sin(aFi)) + ___Fixed_Far_Ravnosignal_DirectionalCosines.cell[2][0]; fDirectionalCosines.cell[0][0] = sqrt(1 - sqr(fDirectionalCosines.cell[1][0]) - sqr(fDirectionalCosines.cell[2][0]));*/ // --------- Положение цели в системе координат ФАР --------- Calc_Angle(fDirectionalCosines, ta->Coord_M2.Far.Q, ta->Coord_M2.Far.E); // --------- Положение цели в системе координат GEO --------- TransformCoordinates(tm_Fixed_Geo_Far, ta->Coord_M2.Far.Q, ta->Coord_M2.Far.E , ta->Coord_M2.Geo.Q, ta->Coord_M2.Geo.E); // --------- Расчет Таненых Тета Фи --------- // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! // Внимание тут наколка - TFloat aa, bb; aFar->Calc_DeltaQ_DeltaAlfa_Teta_Fi(ta->Ideal_Coord_Far.Q, ta->Ideal_Coord_Far.E, aa, bb); ta->Ideal_Coord_Far.Q = aa; ta->Ideal_Coord_Far.E = bb; aFar->Calc_DeltaQ_DeltaAlfa_Teta_Fi(ta->Coord_M1.Far.Q, ta->Coord_M1.Far.E, aa, bb); ta->Coord_M1.Far.Q = aa; ta->Coord_M1.Far.E = bb; aFar->Calc_DeltaQ_DeltaAlfa_Teta_Fi(ta->Coord_M2.Far.Q, ta->Coord_M2.Far.E, aa, bb); ta->Coord_M2.Far.Q = aa; ta->Coord_M2.Far.E = bb; // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! ta->Coord_M1.Far.Q -= ta->Ideal_Coord_Far.Q; ta->Coord_M1.Far.E -= ta->Ideal_Coord_Far.E; ta->Coord_M1.Geo.Q -= ta->Ideal_Coord_Geo.Q; ta->Coord_M1.Geo.E -= ta->Ideal_Coord_Geo.E; ta->Coord_M2.Far.Q -= ta->Ideal_Coord_Far.Q; ta->Coord_M2.Far.E -= ta->Ideal_Coord_Far.E; ta->Coord_M2.Geo.Q -= ta->Ideal_Coord_Geo.Q; ta->Coord_M2.Geo.E -= ta->Ideal_Coord_Geo.E; // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // -------- Расчет текущих ошибок ------------ if (TargetAccess_Count > 0) { // ------ Расчет средних значений ---- for (int i = 0; i < TargetAccess_Count; i++){ ta->Coord_M1.Far.fCurSumQ += TargetAccess[i].Coord_M1.Far.Q; ta->Coord_M1.Far.fCurSumE += TargetAccess[i].Coord_M1.Far.E; ta->Coord_M1.Geo.fCurSumQ += TargetAccess[i].Coord_M1.Geo.Q; ta->Coord_M1.Geo.fCurSumE += TargetAccess[i].Coord_M1.Geo.E; ta->Coord_M2.Far.fCurSumQ += TargetAccess[i].Coord_M2.Far.Q; ta->Coord_M2.Far.fCurSumE += TargetAccess[i].Coord_M2.Far.E; ta->Coord_M2.Geo.fCurSumQ += TargetAccess[i].Coord_M2.Geo.Q; ta->Coord_M2.Geo.fCurSumE += TargetAccess[i].Coord_M2.Geo.E; } ta->Coord_M1.Far.SredErrorQ = ta->Coord_M1.Far.fCurSumQ / (TFloat) TargetAccess_Count; ta->Coord_M1.Far.SredErrorE = ta->Coord_M1.Far.fCurSumE / (TFloat) TargetAccess_Count; ta->Coord_M1.Geo.SredErrorQ = ta->Coord_M1.Geo.fCurSumQ / (TFloat) TargetAccess_Count; ta->Coord_M1.Geo.SredErrorE = ta->Coord_M1.Geo.fCurSumE / (TFloat) TargetAccess_Count; ta->Coord_M2.Far.SredErrorQ = ta->Coord_M2.Far.fCurSumQ / (TFloat) TargetAccess_Count; ta->Coord_M2.Far.SredErrorE = ta->Coord_M2.Far.fCurSumE / (TFloat) TargetAccess_Count; ta->Coord_M2.Geo.SredErrorQ = ta->Coord_M2.Geo.fCurSumQ / (TFloat) TargetAccess_Count; ta->Coord_M2.Geo.SredErrorE = ta->Coord_M2.Geo.fCurSumE / (TFloat) TargetAccess_Count; // --- TFloat odds; for (int i = 0; i < TargetAccess_Count; i++){ odds = TargetAccess[i].Coord_M1.Far.Q - ta->Coord_M1.Far.SredErrorQ; ta->Coord_M1.Far.SKOErrorQ += odds*odds; odds = TargetAccess[i].Coord_M1.Far.E - ta->Coord_M1.Far.SredErrorE; ta->Coord_M1.Far.SKOErrorE += odds*odds; odds = TargetAccess[i].Coord_M1.Geo.Q - ta->Coord_M1.Geo.SredErrorQ; ta->Coord_M1.Geo.SKOErrorQ += odds*odds; odds = TargetAccess[i].Coord_M1.Geo.E - ta->Coord_M1.Geo.SredErrorE; ta->Coord_M1.Geo.SKOErrorE += odds*odds; odds = TargetAccess[i].Coord_M2.Far.Q - ta->Coord_M2.Far.SredErrorQ; ta->Coord_M2.Far.SKOErrorQ += odds*odds; odds = TargetAccess[i].Coord_M2.Far.E - ta->Coord_M2.Far.SredErrorE; ta->Coord_M2.Far.SKOErrorE += odds*odds; odds = TargetAccess[i].Coord_M2.Geo.Q - ta->Coord_M2.Geo.SredErrorQ; ta->Coord_M2.Geo.SKOErrorQ += odds*odds; odds = TargetAccess[i].Coord_M2.Geo.E - ta->Coord_M2.Geo.SredErrorE; ta->Coord_M2.Geo.SKOErrorE += odds*odds; } } ta->Coord_M1.Far.SKOErrorQ = sqrt(ta->Coord_M1.Far.SKOErrorQ / (TFloat) TargetAccess_Count); ta->Coord_M1.Far.SKOErrorE = sqrt(ta->Coord_M1.Far.SKOErrorE / (TFloat) TargetAccess_Count); ta->Coord_M1.Geo.SKOErrorQ = sqrt(ta->Coord_M1.Geo.SKOErrorQ / (TFloat) TargetAccess_Count); ta->Coord_M1.Geo.SKOErrorE = sqrt(ta->Coord_M1.Geo.SKOErrorE / (TFloat) TargetAccess_Count); ta->Coord_M2.Far.SKOErrorQ = sqrt(ta->Coord_M2.Far.SKOErrorQ / (TFloat) TargetAccess_Count); ta->Coord_M2.Far.SKOErrorE = sqrt(ta->Coord_M2.Far.SKOErrorE / (TFloat) TargetAccess_Count); ta->Coord_M2.Geo.SKOErrorQ = sqrt(ta->Coord_M2.Geo.SKOErrorQ / (TFloat) TargetAccess_Count); ta->Coord_M2.Geo.SKOErrorE = sqrt(ta->Coord_M2.Geo.SKOErrorE / (TFloat) TargetAccess_Count); // ---------- for (int i = 0; i < TargetAccess_Count; i++){ if (ta->Coord_M1.Far.MaxErrorQ < fabs(TargetAccess[i].Coord_M1.Far.Q)) ta->Coord_M1.Far.MaxErrorQ = fabs(TargetAccess[i].Coord_M1.Far.Q); if (ta->Coord_M1.Far.MaxErrorE < fabs(TargetAccess[i].Coord_M1.Far.E)) ta->Coord_M1.Far.MaxErrorE = fabs(TargetAccess[i].Coord_M1.Far.E); if (ta->Coord_M1.Geo.MaxErrorQ < fabs(TargetAccess[i].Coord_M1.Geo.Q)) ta->Coord_M1.Geo.MaxErrorQ = fabs(TargetAccess[i].Coord_M1.Geo.Q); if (ta->Coord_M1.Geo.MaxErrorE < fabs(TargetAccess[i].Coord_M1.Geo.E)) ta->Coord_M1.Geo.MaxErrorE = fabs(TargetAccess[i].Coord_M1.Geo.E); if (ta->Coord_M2.Far.MaxErrorQ < fabs(TargetAccess[i].Coord_M2.Far.Q)) ta->Coord_M2.Far.MaxErrorQ = fabs(TargetAccess[i].Coord_M2.Far.Q); if (ta->Coord_M2.Far.MaxErrorE < fabs(TargetAccess[i].Coord_M2.Far.E)) ta->Coord_M2.Far.MaxErrorE = fabs(TargetAccess[i].Coord_M2.Far.E); if (ta->Coord_M2.Geo.MaxErrorQ < fabs(TargetAccess[i].Coord_M2.Geo.Q)) ta->Coord_M2.Geo.MaxErrorQ = fabs(TargetAccess[i].Coord_M2.Geo.Q); if (ta->Coord_M2.Geo.MaxErrorE < fabs(TargetAccess[i].Coord_M2.Geo.E)) ta->Coord_M2.Geo.MaxErrorE = fabs(TargetAccess[i].Coord_M2.Geo.E); } }
bool VRPNTrackerInputDevice::GetControllerOrientationAndPosition(const int32 ControllerIndex, const EControllerHand DeviceHand, FRotator& OutOrientation, FVector& OutPosition) const { for(auto &InputPair : TrackerMap) { const TrackerInput &Tracker = InputPair.Value; if(Tracker.PlayerIndex == ControllerIndex && Tracker.Hand == DeviceHand) { if(InputDevice) { FScopeLock ScopeLock(&CritSect); InputDevice->mainloop(); } FVector NewPosition; FQuat NewRotation; TransformCoordinates(Tracker, NewPosition, NewRotation); OutOrientation = NewRotation.Rotator(); OutPosition = NewPosition; return true; } } return false; }