IEnumerator Rotate(CoordinateFrame unityFrame, CoordinateFrame frame2, Vector3 unityAngles)
{
yield return(new WaitForSeconds(1));
CoordinateFrameConverter conv = new CoordinateFrameConverter(unityFrame, frame2);
EulerAngles eu1 = new EulerAngles(unityAngles.z, unityAngles.x, unityAngles.y);
EulerAngles eu2 = conv.ConvertEulerAngles(eu1);
int steps = 40;
EulerAngles teu1 = new EulerAngles(0, 0, 0);
EulerAngles teu2 = new EulerAngles(0, 0, 0);
for (int e = 0; e < 3; e++)
{
for (int i = 0; i < steps; i++)
{
teu1[e] = teu1[e] + eu1[e] * 1.0f / steps;
teu2[e] = teu2[e] + eu2[e] * 1.0f / steps;
target1.localRotation = Conversions.ToQuaternion(unityFrame.RotationOrder, teu1);
target2.localRotation = Conversions.ToQuaternion(unityFrame.RotationOrder, conv.inverse.ConvertEulerAngles(teu2));
yield return(null);
}
}
}
/// <summary>
/// Read the message from the given <paramref name="reader"/>.
/// </summary>
/// <param name="reader">The reader to read from.</param>
/// <returns>True</returns>
public bool Read(BinaryReader reader)
{
bool ok = true;
TimeUnit = reader.ReadUInt64();
DefaultFrameTime = reader.ReadUInt32();
try
{
CoordinateFrame = (CoordinateFrame)reader.ReadByte();
}
catch (InvalidCastException)
{
ok = false;
}
fixed(byte *reserved = Reserved)
{
for (int i = 0; i < ReservedBytes; ++i)
{
reserved[i] = reader.ReadByte();
}
}
return(ok);
}
public void resetEvolution()
{
evolutionMaze = HardMaze.loadEnvironment("QDMaze.xml");
evolutionFrame = new CoordinateFrame(0.0f, evolutionMaze.AOIRectangle.Y, 1.1f, 0.0f);
optimaAgent = null;
optima_net = null;
optima_generation = -1;
evolutionSession = new Session(this.evolutionMaze, new FitnessHandler(evolutionMaze.compute_fitness), EventHandler, new InstinctActionHandler(HardMaze.createInstinctAction),
new OptimaGestureHandler(evolutionMaze.GetOptimaGestureHandler),
new TaskBeginHandler(evolutionMaze.TaskBeginHandler));
}
private bool TryGetTangoPose(CoordinateFrame frame, out Pose pose)
{
PoseData poseData;
bool result;
if (TangoInputTracking.TryGetPoseAtTime(out poseData, TangoDevice.baseCoordinateFrame, frame, 0.0) && poseData.statusCode == PoseStatus.Valid)
{
pose.position = poseData.position;
pose.rotation = poseData.rotation;
result = true;
}
else
{
pose = Pose.identity;
result = false;
}
return(result);
}
/// <summary>
/// Set <paramref name="transform"/> to reflect a rotation from <paramref name="frame"/> to
/// the Unity coordinate frame.
/// </summary>
/// <param name="transform">The transform to modify.</param>
/// <param name="frame">The source <see cref="CoordinateFrame"/>.</param>
public static void SetFrameRotation(Transform transform, CoordinateFrame frame)
{
Matrix4x4 m = ToUnityTransforms[(int)frame];
//Vector3 side = new Vector3();
Vector3 up = new Vector3();
Vector3 fwd = new Vector3();
Vector3 scale = Vector3.one;
SetFrameRotation(ref m, frame);
//side = m.GetColumn(0);
up = m.GetColumn(1);
fwd = m.GetColumn(2);
if ((int)frame < (int)CoordinateFrame.LeftHanded)
{
scale.x = -1.0f;
}
transform.localRotation = Quaternion.LookRotation(fwd, up);
transform.localScale = scale;
}
private static void InvokeTangoTrackingEvent(TangoInputTracking.TrackingStateEventType eventType, CoordinateFrame frame)
{
Action <CoordinateFrame> action;
if (eventType != TangoInputTracking.TrackingStateEventType.TrackingAcquired)
{
if (eventType != TangoInputTracking.TrackingStateEventType.TrackingLost)
{
throw new ArgumentException("TrackingEventHandler - Invalid EventType: " + eventType);
}
action = TangoInputTracking.trackingLost;
}
else
{
action = TangoInputTracking.trackingAcquired;
}
if (action != null)
{
action(frame);
}
}
internal static bool TryGetPoseAtTime(out PoseData pose, CoordinateFrame baseFrame, CoordinateFrame targetFrame,
double time, ScreenOrientation screenOrientation)
{
return(Internal_TryGetPoseAtTime(time, screenOrientation, baseFrame, targetFrame, out pose));
}
private static extern bool Internal_TryGetPoseAtTime(double time, ScreenOrientation screenOrientation, CoordinateFrame baseFrame, CoordinateFrame targetFrame, out PoseData pose);
internal static bool TryGetPoseAtTime(out PoseData pose, CoordinateFrame baseFrame, CoordinateFrame targetFrame, double time = 0.0)
{
return(TangoInputTracking.Internal_TryGetPoseAtTime(time, Screen.orientation, baseFrame, targetFrame, out pose));
}
/// <summary>
/// Default constructor
/// </summary>
public AgentCamera()
{
Frame = new CoordinateFrame(new Vector3(128f, 128f, 20f));
Far = 128f;
}
/// <summary>
/// Convert a vector from the Unity frame to the specified coordinate fame.
/// </summary>
/// <param name="v">The vector to convert.</param>
/// <param name="frame">The source coordinate frame.</param>
/// <returns>The converted vector.</returns>
/// <remarks>
/// The Unity frame is <see cref="CoordinateFrame.XZY"/>.
/// </remarks>
public static Vector3 UnityToRemote(Vector3 v, CoordinateFrame frame)
{
return(ToUnityTransforms[(int)frame].inverse.MultiplyPoint(v));
}
/// <summary>
/// Query the axis indices for a <see cref="CoordinateFrame"/>.
/// </summary>
/// <param name="frame"></param>
/// <param name="axis"></param>
/// <returns></returns>
/// <remarks>
/// The <paramref name="axis"/> value is interpreted as follows:
///
/// <list type="table">
/// <listheader><term>Axis</term><description>Meaning</description>
/// <item><term>0</term><description>Right/left axis</description>
/// <item><term>1</term><description>Forward</description>
/// <item><term>2</term><description>Up</description>
/// </list>
///
/// The return values indicate which XYZ the is mapped to the descriptions above. For example, querying the
/// AxisIndex with <paramref name="axis"/> set to 1 is a query for the Forward axis of the given
/// <paramref name="frame"/>. This will return a value in the range [0, 2] mapping to XYZ respectively.
/// </remarks>
public static int AxisIndex(CoordinateFrame frame, int axis)
{
return(_axisIndices[(int)frame, axis]);
}
// public static bool FlippedUpAxis(CoordinateFrame frame)
// {
// }
public static bool LeftHanded(CoordinateFrame frame)
{
return((int)frame >= (int)CoordinateFrame.LeftHanded);
}
/// <summary>
/// Convert a vector from the specified coordinate frame to the Unity frame.
/// </summary>
/// <param name="v">The vector to convert</param>
/// <param name="frame">The target coordinate frame.</param>
/// <returns>The converted vector.</returns>
/// <remarks>
/// The Unity frame is <see cref="CoordinateFrame.XZY"/>.
/// </remarks>
public static Vector3 RemoteToUnity(Vector3 v, CoordinateFrame frame)
{
// MultiplyPoint() or MultiplyVector() is irrelevant. We only have rotation.
return(ToUnityTransforms[(int)frame].MultiplyPoint(v));
}
/// <summary>
/// Set <paramref name="transform"/> to reflect a rotation from <paramref name="frame"/> to
/// the Unity coordinate frame.
/// </summary>
/// <param name="transform">The transform to modify.</param>
/// <param name="frame">The source <see cref="CoordinateFrame"/>.</param>
public static void SetFrameRotation(ref Matrix4x4 transform, CoordinateFrame frame)
{
transform = ToUnityTransforms[(int)frame];
}
private static void InvokeTangoTrackingEvent(TrackingStateEventType eventType, CoordinateFrame frame)
{
Action <CoordinateFrame> callback = null;
switch (eventType)
{
case TrackingStateEventType.TrackingAcquired:
callback = trackingAcquired;
break;
case TrackingStateEventType.TrackingLost:
callback = trackingLost;
break;
default:
throw new ArgumentException("TrackingEventHandler - Invalid EventType: " + eventType);
}
if (callback != null)
{
callback(frame);
}
}
/// <summary>
/// Set <paramref name="transform"/> to reflect a rotation to <paramref name="frame"/> from
/// the Unity coordinate frame.
/// </summary>
/// <param name="transform">The transform to modify.</param>
/// <param name="frame">The target <see cref="CoordinateFrame"/>.</param>
public static void SetFrameRotationInverse(ref Matrix4x4 transform, CoordinateFrame frame)
{
SetFrameRotation(ref transform, frame);
transform = transform.inverse;
}
private static void InvokeTangoTrackingEvent(TangoInputTracking.TrackingStateEventType eventType, CoordinateFrame frame)
{
/*
* Action<CoordinateFrame> action;
* if (eventType != TangoInputTracking.TrackingStateEventType.TrackingAcquired)
* {
* if (eventType != TangoInputTracking.TrackingStateEventType.TrackingLost)
* {
* throw new ArgumentException("TrackingEventHandler - Invalid EventType: " + eventType);
* }
* action = TangoInputTracking.trackingLost;
* }
* else
* {
* action = TangoInputTracking.trackingAcquired;
* }
* if (action != null)
* {
* action(frame);
* }
*/
}
