public void Equals()
{
Pose p1 = new Pose(new Vector3(1, 2, 3), Quaternion.CreateRotationY(0.3f));
Pose p2 = new Pose(new Vector3(1, 2, 3), Quaternion.CreateRotationY(0.3f));
Assert.AreEqual(p1, p2);
Assert.IsTrue(p1.Equals((object)p2));
Assert.IsTrue(p1.Equals(p2));
Assert.IsFalse(p1.Equals(p2.ToMatrix()));
}
public override bool Equals(IRosMessage ____other)
{
if (____other == null)
{
return(false);
}
bool ret = true;
gazebo_msgs.GetLinkProperties.Response other = (Messages.gazebo_msgs.GetLinkProperties.Response)____other;
ret &= com.Equals(other.com);
ret &= gravity_mode == other.gravity_mode;
ret &= mass == other.mass;
ret &= ixx == other.ixx;
ret &= ixy == other.ixy;
ret &= ixz == other.ixz;
ret &= iyy == other.iyy;
ret &= iyz == other.iyz;
ret &= izz == other.izz;
ret &= success == other.success;
ret &= status_message == other.status_message;
// for each SingleType st:
// ret &= {st.Name} == other.{st.Name};
return(ret);
}
/// <summary>
/// Raycast from `Vector2` screen point. Returns true if raycast collides with a plane.
/// </summary>
/// <param name="position">Position on screen to fire raycast from.</param>
/// <param name="raycastManager">AR Foundation raycast manager component.</param>
/// <param name="planeManager">AR Foundation plane manager component.</param>
/// <param name="pose">Unity pose object.</param>
/// <param name="plane">AR Foundation plane object.</param>
/// <returns>void</returns>
public static bool RaycastToPlane(Vector2 position, ARRaycastManager raycastManager,
ARPlaneManager planeManager, out Pose pose, out ARPlane plane)
{
var hits = new List <ARRaycastHit>();
pose = Pose.identity;
plane = null;
if (!raycastManager.Raycast(position, hits, TrackableType.Planes))
{
return(false);
}
var hit = hits[0];
pose = hit.pose;
foreach (var trackable in planeManager.trackables)
{
if (trackable.trackableId.Equals(hit.trackableId))
{
plane = trackable;
}
}
return(!pose.Equals(Pose.identity) && plane != null);
}
public bool Equals(ProviderTrackableData other) => trackableId.Equals(other.trackableId) && pose.Equals(other.pose) && trackingState == other.trackingState && nativePtr == other.nativePtr && anInt32 == other.anInt32 && aBool == other.aBool;
public void InterpolateHandPoses(float t)
{
if (!animator.isHuman)
{
return;
}
foreach (FieldInfo field in typeof(PoseManager).GetFields().Where(f => f.Name.StartsWith("openHand")))
{
string boneName = field.Name.Split('_')[1];
FieldInfo closed = typeof(PoseManager).GetField("closedHand_" + boneName);
HumanBodyBones bone = (HumanBodyBones)Enum.Parse(typeof(HumanBodyBones), boneName);
Pose openPose = (Pose)field.GetValue(this);
Pose closedPose = (Pose)closed.GetValue(this);
if (openPose.Equals(Pose.identity) || closedPose.Equals(Pose.identity))
{
return;
}
Transform boneTransform = animator.GetBoneTransform(bone);
boneTransform.localPosition = Vector3.Lerp(openPose.position, closedPose.position, t);
boneTransform.localRotation = Quaternion.Slerp(openPose.rotation, closedPose.rotation, t);
}
}
/// <summary>
/// Tests for equality.
/// </summary>
/// <param name="other">The other <see cref="XRHumanBodyJoint"/> to compare against.</param>
/// <returns>`True` if every field in <paramref name="other"/> is equal to this <see cref="XRHumanBodyJoint"/>, otherwise false.</returns>
public bool Equals(XRHumanBodyJoint other)
{
return(m_Index.Equals(other.m_Index) && m_ParentIndex.Equals(other.m_ParentIndex) &&
m_LocalScale.Equals(other.m_LocalScale) && m_LocalPose.Equals(other.m_LocalPose) &&
m_AnchorScale.Equals(other.m_AnchorScale) && m_AnchorPose.Equals(other.m_AnchorPose) &&
m_Tracked.Equals(other.m_Tracked));
}
/// <summary>
/// Tests for equality.
/// </summary>
/// <param name="other">The other <see cref="XRParticipant"/> to compare against.</param>
/// <returns><c>true</c> if <paramref name="other"/> is equal to this <see cref="XRParticipant"/>.</returns>
public bool Equals(XRParticipant other)
{
return
(m_TrackableId.Equals(other.m_TrackableId) &&
m_Pose.Equals(other.m_Pose) &&
(m_TrackingState == other.m_TrackingState) &&
(m_NativePtr == other.m_NativePtr));
}
public void MoveTo(Pose target)
{
if (_pose.Equals(target))
{
StopAndSetPose(target);
return;
}
TweenToInTime(target, _tweenTime);
}
public override bool Equals(IRosMessage ____other)
{
if (____other == null)
{
return(false);
}
bool ret = true;
quad_controller.SetPose.Request other = (Messages.quad_controller.SetPose.Request)____other;
ret &= pose.Equals(other.pose);
return(ret);
}
public void TestEquals()
{
foreach (var(p, q) in poseHelper.RandomPoses(100).Zip(poseHelper.RandomPoses(100), (x, y) => (x, y)))
{
double delta = SeqAbsDiff(p.TransformMatrix.ToRowMajorArray(), q.TransformMatrix.ToRowMajorArray());
if (delta > 1E-9)
{
Assert.False(q.Equals(p));
}
Pose r = p;
Assert.True(r.Equals(p));
}
}
private void ApplyBodyBonePose(HumanBodyBones bodyBone, Pose openPose, Pose closedPose, float fade)
{
Transform boneTransform = animator.GetBoneTransform(bodyBone);
if (!boneTransform)
{
return;
}
if (openPose.Equals(Pose.identity) || closedPose.Equals(Pose.identity))
{
return;
}
boneTransform.localPosition = Vector3.Lerp(openPose.position, closedPose.position, fade);
boneTransform.localRotation = Quaternion.Slerp(openPose.rotation, closedPose.rotation, fade);
}
public override bool Equals(IRosMessage ____other)
{
if (____other == null)
{
return(false);
}
bool ret = true;
gazebo_msgs.SpawnModel.Request other = (Messages.gazebo_msgs.SpawnModel.Request)____other;
ret &= model_name == other.model_name;
ret &= model_xml == other.model_xml;
ret &= robot_namespace == other.robot_namespace;
ret &= initial_pose.Equals(other.initial_pose);
ret &= reference_frame == other.reference_frame;
return(ret);
}
public override bool Equals(IRosMessage ____other)
{
if (____other == null)
{
return(false);
}
bool ret = true;
octomap_msgs.OctomapWithPose other = (Messages.octomap_msgs.OctomapWithPose)____other;
ret &= header.Equals(other.header);
ret &= origin.Equals(other.origin);
ret &= octomap.Equals(other.octomap);
// for each SingleType st:
// ret &= {st.Name} == other.{st.Name};
return(ret);
}
public override bool Equals(IRosMessage ____other)
{
if (____other == null)
{
return(false);
}
bool ret = true;
gazebo_msgs.SetJointTrajectory.Request other = (Messages.gazebo_msgs.SetJointTrajectory.Request)____other;
ret &= model_name == other.model_name;
ret &= joint_trajectory.Equals(other.joint_trajectory);
ret &= model_pose.Equals(other.model_pose);
ret &= set_model_pose == other.set_model_pose;
ret &= disable_physics_updates == other.disable_physics_updates;
return(ret);
}
public override bool Equals(IRosMessage ____other)
{
if (____other == null)
{
return(false);
}
bool ret = true;
gazebo_msgs.LinkState other = (Messages.gazebo_msgs.LinkState)____other;
ret &= link_name == other.link_name;
ret &= pose.Equals(other.pose);
ret &= twist.Equals(other.twist);
ret &= reference_frame == other.reference_frame;
// for each SingleType st:
// ret &= {st.Name} == other.{st.Name};
return(ret);
}
public override bool Equals(IRosMessage ____other)
{
if (____other == null)
{
return(false);
}
bool ret = true;
baxter_core_msgs.EndpointState other = (Messages.baxter_core_msgs.EndpointState)____other;
ret &= header.Equals(other.header);
ret &= pose.Equals(other.pose);
ret &= twist.Equals(other.twist);
ret &= wrench.Equals(other.wrench);
// for each SingleType st:
// ret &= {st.Name} == other.{st.Name};
return(ret);
}
public override bool Equals(IRosMessage ____other)
{
if (____other == null)
{
return(false);
}
bool ret = true;
nav_msgs.MapMetaData other = (Messages.nav_msgs.MapMetaData)____other;
ret &= map_load_time.data.Equals(other.map_load_time.data);
ret &= resolution == other.resolution;
ret &= width == other.width;
ret &= height == other.height;
ret &= origin.Equals(other.origin);
// for each SingleType st:
// ret &= {st.Name} == other.{st.Name};
return(ret);
}
public override bool Equals(IRosMessage ____other)
{
if (____other == null)
{
return(false);
}
bool ret = true;
gazebo_msgs.GetModelState.Response other = (Messages.gazebo_msgs.GetModelState.Response)____other;
ret &= header.Equals(other.header);
ret &= pose.Equals(other.pose);
ret &= twist.Equals(other.twist);
ret &= success == other.success;
ret &= status_message == other.status_message;
// for each SingleType st:
// ret &= {st.Name} == other.{st.Name};
return(ret);
}
public override bool Equals(IRosMessage ____other)
{
if (____other == null)
{
return(false);
}
bool ret = true;
gazebo_msgs.SetLinkProperties.Request other = (Messages.gazebo_msgs.SetLinkProperties.Request)____other;
ret &= link_name == other.link_name;
ret &= com.Equals(other.com);
ret &= gravity_mode == other.gravity_mode;
ret &= mass == other.mass;
ret &= ixx == other.ixx;
ret &= ixy == other.ixy;
ret &= ixz == other.ixz;
ret &= iyy == other.iyy;
ret &= iyz == other.iyz;
ret &= izz == other.izz;
return(ret);
}
public override bool Equals(IRosMessage ____other)
{
if (____other == null)
{
return(false);
}
bool ret = true;
object_recognition_msgs.Table other = (Messages.object_recognition_msgs.Table)____other;
ret &= header.Equals(other.header);
ret &= pose.Equals(other.pose);
if (convex_hull.Length != other.convex_hull.Length)
{
return(false);
}
for (int __i__ = 0; __i__ < convex_hull.Length; __i__++)
{
ret &= convex_hull[__i__].Equals(other.convex_hull[__i__]);
}
// for each SingleType st:
// ret &= {st.Name} == other.{st.Name};
return(ret);
}
public bool Equals(ARCoreFaceRegionData other)
{
return
((m_Region == other.m_Region) &&
m_Pose.Equals(other.m_Pose));
}
public void Equals()
{
Pose p1 = new Pose(new Vector3F(1, 2, 3), QuaternionF.CreateRotationY(0.3f));
Pose p2 = new Pose(new Vector3F(1, 2, 3), QuaternionF.CreateRotationY(0.3f));
Assert.AreEqual(p1, p2);
Assert.IsTrue(p1.Equals((object)p2));
Assert.IsTrue(p1.Equals(p2));
Assert.IsFalse(p1.Equals(p2.ToMatrix44F()));
}
public virtual void ReplanGlobal(Platform platform)
{
// create searchGrid data structure for the EpPathFinding
BaseGrid searchGrid = new StaticGrid(platform.Map.Rows, platform.Map.Columns);
List <Tuple <double, GridPos> > searchPoses = new List <Tuple <double, GridPos> > (); // possible search poses
for (int i = 0; i < platform.Map.Rows; i++)
{
for (int j = 0; j < platform.Map.Columns; j++)
{
if (platform.Map.MapMatrix[i, j] < platform.OccupiedThreshold)
{
searchGrid.SetWalkableAt(i, j, true);
}
if ((platform.Map.MapMatrix[i, j] >= platform.FreeThreshold) && (platform.Map.MapMatrix[i, j] <= platform.OccupiedThreshold))
{
RegionLimits limits = platform.Map.CalculateLimits(i, j, 1);
List <Pose> posesl = limits.GetPosesWithinLimits();
foreach (Pose p in posesl)
{
if (platform.Map.MapMatrix[p.X, p.Y] < platform.FreeThreshold)
{
double d = Math.Sqrt(Math.Pow(p.X - platform.Pose.X, 2) + Math.Pow(p.Y - platform.Pose.Y, 2));
searchPoses.Add(new Tuple <double, GridPos>(d, new GridPos(i, j)));
break;
}
}
}
}
}
// set unaccessable for those places where are platforms and their enviroment within 1 step radius
foreach (Platform plt in platform.ObservedPlatforms)
{
if (plt.Equals(platform))
{
continue;
}
RegionLimits limits = platform.Map.CalculateLimits(plt.Pose.X, plt.Pose.Y, 1);
List <Pose> posesl = limits.GetPosesWithinLimits();
foreach (Pose p in posesl)
{
searchGrid.SetWalkableAt(p.X, p.Y, false);
}
}
// bound the search to avoid large computation
// select the first closest 50 candidates based on L2 distance
int maxNumOfSearchPoses = 50;
searchPoses.Sort((t1, t2) => t1.Item1.CompareTo(t2.Item1));
if (searchPoses.Count > maxNumOfSearchPoses)
{
searchPoses.RemoveRange(maxNumOfSearchPoses, searchPoses.Count - maxNumOfSearchPoses);
}
// init search
GridPos startPos = new GridPos(platform.Pose.X, platform.Pose.Y);
GridPos endPos = new GridPos(20, 10);
JumpPointParam jpParam = new JumpPointParam(searchGrid, startPos, endPos, false, true, true);
// find the best path
double bestPathScore = Double.PositiveInfinity;
List <GridPos> bestPath = null;
foreach (Tuple <double, GridPos> p in searchPoses)
{
//jpParam.Reset(startPos, p);
jpParam.Reset(new GridPos(platform.Pose.X, platform.Pose.Y), p.Item2);
List <GridPos> resultPathList = JumpPointFinder.FindPath(jpParam);
if (resultPathList.Count > 2)
{
double score = 0;
for (int i = 1; i < resultPathList.Count; i++)
{
score += Math.Sqrt(Math.Pow(resultPathList[i].x - resultPathList[i - 1].x, 2) + Math.Pow(resultPathList[i].y - resultPathList[i - 1].y, 2));
}
if (score < bestPathScore)
{
bestPathScore = score;
bestPath = resultPathList;
bestFronterier = new Pose(resultPathList.Last().x, resultPathList.Last().y);
}
}
}
// convert the best path to command sequence
if ((bestPath != null) && (bestPath.Count > 2))
{
List <Pose> bestPathConv = new List <Pose>();
bestPathConv.Add(platform.Pose);
for (int i = 1; i < bestPath.Count; i++)
{
Pose prevPose = bestPathConv.Last();
Pose goalPose = new Pose(bestPath[i].x, bestPath[i].y);
int dxl = Math.Sign(goalPose.X - prevPose.X);
int dyl = Math.Sign(goalPose.Y - prevPose.Y);
while (!prevPose.Equals(goalPose)) // it's a bit dangerous here
{
Pose newPose = new Pose(prevPose.X + dxl, prevPose.Y + dyl);
prevPose = newPose;
bestPathConv.Add(newPose);
}
}
for (int i = bestPathConv.Count - 2; i > 0; i--)
{
int dx = bestPathConv[i + 1].X - bestPathConv[i].X;
int dy = bestPathConv[i + 1].Y - bestPathConv[i].Y;
double dalpha = Math.Atan2(dy, dx);
Pose newPose = new Pose(bestPathConv[i].X, bestPathConv[i].Y, dalpha);
commandSequence.Push(newPose);
}
}
}
public bool Equals(GazeToDisplayCoordinateMappingRecord other)
{
return(Gaze.Equals(other.Gaze) && HeadPose.Equals(other.HeadPose) && Face.Equals(other.Face) && Display.Equals(other.Display));
}
public void Next(Platform platform)
{
platform.Measure();
platform.Communicate();
// init breadCumbers stack if it is null (fix serialization)
if (trajectory == null)
{
trajectory = new Stack <Pose>();
}
// init commandSequence stack if it is null (fix serialization)
if (commandSequence == null)
{
commandSequence = new Stack <Pose>();
}
this.NextInit(platform);
// if robot is stopped then return
if (platform.IsStopped)
{
return;
}
// if the map is discovered we are done
if (platform.Map.IsDiscovered(platform))
{
platform.Stop();
return;
}
Pose nextPose = null;
if (!(platform.ControlPolicy is ReplayPolicy))
{
// get the next pose and decide whether a replan is needed
bool isReplan = false;
if ((commandSequence != null) && (commandSequence.Count > 0))
{
nextPose = commandSequence.Pop();
// ok, on the next step, there is another guy, plan the track again
if (platform.ObservedPlatforms.Find(pt => nextPose.Equals(pt.Pose)) != null)
{
isReplan = true;
}
// ok, next step would be a wall, let's plan the track again
if (platform.Map.MapMatrix[nextPose.X, nextPose.Y] >= platform.OccupiedThreshold)
{
isReplan = true;
}
// if the goal is not changed, then keep the track
// this is good, if needed time to plan the way back from an abonden area
if ((bestFronterier != null) && (platform.Map.MapMatrix[bestFronterier.X, bestFronterier.Y] != prevFronterierValue))
{
isReplan = true;
}
}
else
{
isReplan = true;
}
//isReplan = true;
// *** need to replan
if (isReplan)
{
// this is the search radius around the platform
// if it is -1, then look for the first fronterier and don't worry about the manuevers
int rad = (int)((double)platform.FieldOfViewRadius * 1.5);
ReplanLocal(platform, rad);
// get the next pose, if it is available
if (commandSequence.Count > 0)
{
nextPose = commandSequence.Pop();
solutionType = SolutionType.LocalPlanner;
}
if ((nextPose == null) || (commandSequence.Count == 0))
{
ReplanGlobal(platform);
if (commandSequence.Count > 0)
{
nextPose = commandSequence.Pop();
solutionType = SolutionType.GlobalPlanner;
}
}
}
}
else
{
nextPose = commandSequence.Pop();
}
// *** need to replan
if ((platform.ControlPolicy is ReplayPolicy) || ((nextPose != null) && (platform.Map.MapMatrix[nextPose.X, nextPose.Y] < platform.OccupiedThreshold) && ((platform.ObservedPlatforms.Find(pt => nextPose.Equals(pt.Pose)) == null))))
{
// maintain breadcumbers
prevPose = new Pose(nextPose.X, nextPose.Y, nextPose.Heading);
// do action
double dx = nextPose.X - platform.Pose.X;
double dy = nextPose.Y - platform.Pose.Y;
int goalAlpha = Utililty.ConvertAngleTo360(Math.Atan2(dy, dx) / Math.PI * 180);
// choose the angle that is closer to the target heading
int dalpha1 = Utililty.ConvertAngleTo360(goalAlpha - platform.Pose.Heading);
int dalpha2 = Utililty.ConvertAngleTo360((platform.Pose.Heading - goalAlpha) + 360.0);
double dalpha = Math.Abs(dalpha1) < Math.Abs(dalpha2) ? dalpha1 : -dalpha2;
if (dalpha == 0)
{
if (bestFronterier != null)
{
prevFronterierValue = platform.Map.MapMatrix[bestFronterier.X, bestFronterier.Y];
}
trajectory.Push(nextPose);
platform.Move((int)dx, (int)dy);
}
else // rotatation is needed, let's rotate
{
double rot = Math.Sign(dalpha) * 45;
platform.Rotate(rot);
commandSequence.Push(nextPose);
}
hasFeasablePath = true;
}
else
{
platform.SendLog("No feasible solution");
hasFeasablePath = false;
commandSequence.Clear();
}
}
