void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.LaserRangeFinderEntity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
CreateDefaultState();
physics.RaycastProperties raycastProperties = new physics.RaycastProperties();
raycastProperties.StartAngle = -_state.AngularRange / 2.0f;
raycastProperties.EndAngle = _state.AngularRange / 2.0f;
raycastProperties.AngleIncrement = (float)_state.AngularResolution;
raycastProperties.Range = LASER_RANGE;
raycastProperties.OriginPose = new Pose();
_entity.RaycastProperties = raycastProperties;
try
{
_entity.Register(_raycastResults);
}
catch (Exception ex)
{
LogError(ex);
}
// attach handler to raycast results port
Activate(Arbiter.Receive(true, _raycastResults, RaycastResultsHandler));
}
/// <summary>
/// Initialize the IR Distance sensor
/// </summary>
/// <param name="device"></param>
/// <param name="physicsEngine"></param>
public override void Initialize(xnagrfx.GraphicsDevice device, simcommon.PhysicsEngine physicsEngine)
{
try
{
if (Parent == null)
{
throw new Exception("This entity must be a child of another entity.");
}
// make sure that we take at least 2 samples in each direction
if (Samples < 2f)
{
Samples = 2f;
}
_raycastProperties = new simcommon.RaycastProperties();
_raycastProperties.StartAngle = -DispersionConeAngle / 2.0f;
_raycastProperties.EndAngle = DispersionConeAngle / 2.0f;
_raycastProperties.AngleIncrement = DispersionConeAngle / (Samples - 1f);
_raycastProperties.Range = MaximumRange;
_raycastProperties.OriginPose = new Pose();
// set flag so rendering engine renders us last
Flags |= simengine.VisualEntityProperties.UsesAlphaBlending;
base.Initialize(device, physicsEngine);
// set up for rendering impact points
simcommon.HeightFieldShapeProperties hf = new simcommon.HeightFieldShapeProperties("height field", 2, 0.02f, 2, 0.02f, 0, 0, 1, 1);
hf.HeightSamples = new simcommon.HeightFieldSample[hf.RowCount * hf.ColumnCount];
for (int i = 0; i < hf.HeightSamples.Length; i++)
{
hf.HeightSamples[i] = new simcommon.HeightFieldSample();
}
_particlePlane = new simcommon.Shape(hf);
_particlePlane.State.Name = "laser impact plane";
// The mesh is used to render the ray impact points rather than the sensor geometry.
int index = Meshes.Count;
Meshes.Add(simengine.SimulationEngine.ResourceCache.CreateMesh(device, _particlePlane.State));
Meshes[0].Textures[0] = simengine.SimulationEngine.ResourceCache.CreateTextureFromFile(device, "particle.bmp");
// we have a custom effect, with an additional global parameter. Get handle to it here
if (Effect != null)
{
_timeAttenuationHandle = Effect.GetParameter("timeAttenuation");
}
}
catch (Exception ex)
{
// clean up
if (PhysicsEntity != null)
{
PhysicsEngine.DeleteEntity(PhysicsEntity);
}
HasBeenInitialized = false;
InitError = ex.ToString();
}
}