private xna.Vector3 SingleRaycast(xna.Vector3 from, xna.Vector3 to)
{
var rayResult = PhysicsEngine.Raycast2D(
RaycastProperties.FromSingleRay(
TypeConversion.FromXNA(from),
TypeConversion.FromXNA(to)
)
);
RaycastResult result = null;
//
// !Warning! this code is relying on the Raycast operation being
// fundementally synchronous, wrapped in an async API.
//
if (rayResult.Test(out result))
{
if (result.ImpactPoints.Count == 1)
{
var impact = result.ImpactPoints[0].Position;
return(new xna.Vector3(
impact.X,
impact.Y,
impact.Z
));
}
}
return(to);
}
private void SingleRaycastAsync(xna.Vector3 from, xna.Vector3 to, int index, Port <OcclusionRay> resultPort)
{
var rayResult = PhysicsEngine.Raycast2D(
RaycastProperties.FromSingleRay(
TypeConversion.FromXNA(from),
TypeConversion.FromXNA(to)
)
);
Activate(
Arbiter.Receive(false, rayResult,
delegate(RaycastResult result)
{
xna.Vector3 hitPoint;
if (result.ImpactPoints.Count == 1)
{
var impact = result.ImpactPoints[0].Position;
hitPoint = new xna.Vector3(
impact.X,
impact.Y,
impact.Z
);
}
else
{
hitPoint = to;
}
float distance = (hitPoint - to).Length();
resultPort.Post(
new OcclusionRay
{
Index = index,
Point = from,
Impact = hitPoint,
Distance = distance,
Occluded = distance > OcclusionThreshold,
Run = 0
}
);
}
)
);
}
/// <summary>
/// Frame update
/// </summary>
/// <param name="update"></param>
public override void Update(FrameUpdate update)
{
if (_raycastProperties == null)
{
base.Update(update);
return;
}
_appTime = (float)update.ApplicationTime;
// assume pose of parent
if (Parent != null)
{
State.Pose = Parent.State.Pose;
}
_elapsedSinceLastScan += (float)update.ElapsedTime;
// only retrieve raycast results every SCAN_INTERVAL.
// For entities that are compute intenisve, you should consider giving them
// their own task queue so they dont flood a shared queue
if (_elapsedSinceLastScan > SCAN_INTERVAL)
{
_elapsedSinceLastScan = 0;
// the LRF looks towards the negative Z axis (towards the user), not the positive Z axis
// which is the default orientation. So we have to rotate its orientation by 180 degrees
_raycastProperties.OriginPose.Orientation = TypeConversion.FromXNA(
TypeConversion.ToXNA(State.Pose.Orientation) * xna.Quaternion.CreateFromAxisAngle(new xna.Vector3(0, 1, 0), (float)Math.PI));
// to calculate the position of the origin of the raycast, we must first rotate the LocalPose position
// of the raycast (an offset from the origin of the parent entity) by the orientation of the parent entity.
// The origin of the raycast is then this rotated offset added to the parent position.
xna.Matrix parentOrientation = xna.Matrix.CreateFromQuaternion(TypeConversion.ToXNA(State.Pose.Orientation));
xna.Vector3 localOffset = xna.Vector3.Transform(TypeConversion.ToXNA(_sonarBox.State.LocalPose.Position), parentOrientation);
_raycastProperties.OriginPose.Position = State.Pose.Position + TypeConversion.FromXNA(localOffset);
_raycastResultsPort = PhysicsEngine.Raycast2D(_raycastProperties);
_raycastResultsPort.Test(out _lastResults);
if (_serviceNotification != null && _lastResults != null)
{
_serviceNotification.Post(_lastResults);
}
}
base.Update(update);
}
