public SOrientation Update(SOrientation o)
{
m_Yaw.UpdateAngle(o.Yaw);
m_Pitch.Update(o.Pitch);
m_Roll.Update(o.Roll);
return(Orientation);
}
public SOrientation Update(SOrientation or_acc_mag, Quaternion gyro_rotation_step, float filter)
{
Quaternion Rotation_AccMag = or_acc_mag.Rotation;
m_FusedRotation = Quaternion.Lerp(Rotation_AccMag, m_FusedRotation * gyro_rotation_step, filter);
return(SOrientation.FromRotation(m_FusedRotation));
}
public void Rotate(Quaternion q)
{
Quaternion Me = Rotation;
SOrientation Rotated = FromRotation(Me * q);
Yaw = Rotated.Yaw;
Pitch = Rotated.Pitch;
Roll = Rotated.Roll;
}
public SOrientation GetRawOrientation()
{
SOrientation R = new SOrientation
{
Yaw = m_Yaw.getRawValue(),
Pitch = m_Pitch.getRawValue(),
Roll = m_Roll.getRawValue(),
};
return(R);
}
public static SOrientation Lerp(SOrientation o1, SOrientation o2, float t)
{
SOrientation R = new SOrientation
{
Yaw = LerpCyclic(o1.Yaw, o2.Yaw, t, 360.0f),
Pitch = Lerp(o1.Pitch, o2.Pitch, t),
Roll = Lerp(o1.Roll, o2.Roll, t),
};
return(R);
}
public static SOrientation FromHeadingGravity(float heading, Vector3 gravity)
{
SOrientation R = new SOrientation
{
Yaw = heading,
Pitch = GetPitch(gravity),
Roll = GetRoll(gravity),
};
return(R);
}
public static SOrientation FromRotation(Quaternion q1)
{
Vector3 Euler = q1.eulerAngles;
float pitch = Euler.x; if (pitch > 180.0f)
{
pitch -= 360.0f;
}
float roll = Euler.z; if (roll > 180.0f)
{
roll -= 360.0f;
}
SOrientation R = new SOrientation
{
Yaw = Euler.y,
Pitch = pitch,
Roll = roll,
};
return(R);
}
public static SOrientation FromQuaternion1(Quaternion q1)
{
// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
float heading, attitude, bank;
float test = q1.x * q1.y + q1.z * q1.w;
if (test > 0.499)
{ // singularity at north pole
heading = 2.0f * Mathf.Atan2(q1.x, q1.w) * Mathf.Rad2Deg;
attitude = 90.0f;
bank = 0.0f;
}
else if (test < -0.499)
{ // singularity at south pole
heading = -2.0f * Mathf.Atan2(q1.x, q1.w) * Mathf.Rad2Deg;
attitude = -90.0f;
bank = 0.0f;
}
else
{
float sqx = q1.x * q1.x;
float sqy = q1.y * q1.y;
float sqz = q1.z * q1.z;
heading = Mathf.Rad2Deg * Mathf.Atan2(2.0f * q1.y * q1.w - 2.0f * q1.x * q1.z, 1.0f - 2.0f * sqy - 2.0f * sqz);
attitude = Mathf.Rad2Deg * Mathf.Asin(2.0f * test);
bank = Mathf.Rad2Deg * Mathf.Atan2(2.0f * q1.x * q1.w - 2.0f * q1.y * q1.z, 1.0f - 2.0f * sqx - 2.0f * sqz);
}
if (heading < 0.0f)
{
heading += 360.0f;
}
SOrientation R = new SOrientation
{
Yaw = heading,
Pitch = attitude,
Roll = bank,
};
return(R);
}
// Update is called once per frame
public void UpdateTask()
{
float DeltaTime = Time.deltaTime;
//m_Timer.Update(DeltaTime);
DeviceInput.Update();
Smoothed.SetTimeStep(DeltaTime);
//ClearString();
SOrientation AccMagOrientation = DeviceInput.AccMagOrientation;
Quaternion RotationChange = DeviceInput.GetRotationChange(DeltaTime);
SOrientation Orientation;
if (DeviceInput.HasGyro)
{
m_SensorFusion.Update(AccMagOrientation, RotationChange, 0.9f);
}
m_SmoothedOrientation.Update(AccMagOrientation);
if (m_UseSensorFusion)
{
Orientation = m_SensorFusion.Orientation;
}
else
{
Orientation = m_SmoothedOrientation.Orientation;
}
m_DebugText.PrintText(Orientation);
m_DebugText.NewLine();
float heading = Orientation.Yaw;
float pitch = Orientation.Pitch;
float tilt = Orientation.Roll;
int iFocus = -1;
for (int i = 0; i < 4; i++)
{
// TaskPanel
float Dist = Panels[i].UpdateAngles(heading, pitch);
if (!Panels[i].IsFinished())
{
if (m_Distance >= Dist && m_Distance >= Mathf.Abs(tilt))
{
iFocus = i;
if (0.6f < Panels[i].GetFocusTime())
{
TakePicture(i);
}
else
{
Panels[i].SetStatus(TaskPanel.ETaskStatus.TS_Focus, m_FlashingFocus);
}
}
else
{
Panels[i].SetStatus(TaskPanel.ETaskStatus.TS_Pending);
}
}
}
bool bAllFinished = IsAllFinished();
if (bAllFinished)
{
m_bTakingPointPhoto = true;
}
if (m_bTakingPointPhoto)
{
m_CompassStrip.SetActive(false);
m_ButtonTakePicture.SetActive(true);
}
else
{
m_CompassStrip.SetActive(true);
m_ButtonTakePicture.SetActive(false);
}
if (bAllFinished || m_bTakingPointPhoto)
{
StatusText.text = m_StringTakePhoto;
}
else if (iFocus >= 0)
{
StatusText.text = m_StringHoldStill;//"Halten Sie das Handy kurz ruhig" ;
if (iFocus >= 0 && m_SnapIfAccurate)
{
tilt = 0.0f;
float SnappedHeading = Panels[iFocus].Heading;
float SnappedPitch = Panels[iFocus].Pitch;
for (int i = 0; i < 4; i++)
{
Panels[i].UpdateAngles(SnappedHeading, SnappedPitch);
}
}
}
else if (Mathf.Abs(tilt) > 20.0f)
{
StatusText.text = m_StringLandscape;//"Halte das Handy im Landschaftsmodus" ;
}
else
{
StatusText.text = m_StringFocus;//"Bringe die roten Quadrate in den Rahmen um Fotos nach Norden, Osten, Süden und Westen zu machen";
}
m_DebugText.PrintText("iFocus", iFocus);
float RotZ = Div.getClamped(-2.0f * tilt, -35.0f, 35.0f);
transform.localRotation = Quaternion.Euler(0.0f, 0.0f, RotZ);
}
public void PrintText(SOrientation o, int numdigits = 3)
{
PrintText("Yaw", o.Yaw, numdigits);
PrintText("Pitch", o.Pitch, numdigits);
PrintText("Roll", o.Roll, numdigits);
}
public void PrintText(Quaternion q, int numdigits = 3)
{
PrintText(SOrientation.FromRotation(q));
}
