//===========================================================================
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;
}
