/// <summary>
/// Finds the nearest RigidBody that doesn't belong to the robot.
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="result"></param>
/// <returns></returns>
public object CastRay(ref Vector3 from, ref Vector3 to, VehicleRaycasterResult result)
{
AllHitsRayResultCallback rayCallback = new AllHitsRayResultCallback(from, to);
dynamicsWorld.RayTest(from, to, rayCallback);
int i = 0;
foreach (CollisionObject co in rayCallback.CollisionObjects)
{
BRigidBody brb = co.UserObject as BRigidBody;
if (brb != null)
{
if (!brb.gameObject.name.StartsWith("node_"))
{
result.HitPointInWorld = rayCallback.HitPointWorld[i];
result.HitNormalInWorld = rayCallback.HitNormalWorld[i];
result.HitNormalInWorld.Normalize();
result.DistFraction = rayCallback.HitFractions[i];
return(RigidBody.Upcast(co));
}
}
i++;
}
return(null);
}
//Step #2
public override float ReturnOutput()
{
//setting shortest distance of a collider to the maxRange, then if any colliders are closer to the sensor,
//their distanceToCollider value becomes the new shortest distance
float shortestDistance = maxRange;
//Raycasting begins
Ray ray = new Ray(gameObject.transform.position, transform.forward);
BulletSharp.Math.Vector3 fromUltra = ray.origin.ToBullet();
BulletSharp.Math.Vector3 toCollider = ray.GetPoint(maxRange).ToBullet();
Vector3 toColliderUnity = toCollider.ToUnity();
//Callback returns all hit point results in order to avoid non colliders interfere with the ray test
AllHitsRayResultCallback raysCallback = new AllHitsRayResultCallback(fromUltra, toCollider);
//Retrieves bullet physics world and does a ray test with the given coordinates and updates the callback object
BPhysicsWorld world = BPhysicsWorld.Get();
world.world.RayTest(fromUltra, toCollider, raysCallback);
//Gets the position of all hit points of the ray test
List <BulletSharp.Math.Vector3> colliderPositions = raysCallback.HitPointWorld;
BulletSharp.Math.Vector3 colliderPosition = BulletSharp.Math.Vector3.Zero;
float distanceToCollider = maxRange;
//Loop through all hit points and get the shortest distance, exclude the origin since it is also counted as a hit point
foreach (BulletSharp.Math.Vector3 pos in colliderPositions)
{
if ((pos - fromUltra).Length < distanceToCollider && !pos.Equals(BulletSharp.Math.Vector3.Zero))
{
distanceToCollider = (pos - fromUltra).Length;
colliderPosition = pos;
}
}
Debug.DrawLine(fromUltra.ToUnity(), colliderPosition.ToUnity(), Color.green, 5f);
if (distanceToCollider < shortestDistance)
{
shortestDistance = distanceToCollider;
}
//Will need when data sent to emulator
//if (shortestDistance == maxRange)
//{
// //read out to user that nothing within range was detected by ultrasonic sensor;
// Debug.Log("False");
//}
//else
//{
// //read out to user that first object detected was 'shortestDistance' away;
// Debug.Log("True");
//}
return(shortestDistance);
}
public override float ReturnOutput()
{
//Raycasting begins, draw a ray from emitter to the receiver
Ray ray = new Ray(Emitter.transform.position, Emitter.transform.forward);
BulletSharp.Math.Vector3 fromUltra = ray.origin.ToBullet();
BulletSharp.Math.Vector3 toCollider = ray.GetPoint(10).ToBullet();
Vector3 toColliderUnity = toCollider.ToUnity();
//Callback returns all hit point results in order to avoid non colliders interfere with the ray test
AllHitsRayResultCallback raysCallback = new AllHitsRayResultCallback(fromUltra, toCollider);
//Retrieves bullet physics world and does a ray test with the given coordinates and updates the callback object
BPhysicsWorld world = BPhysicsWorld.Get();
world.world.RayTest(fromUltra, toCollider, raysCallback);
List <BulletSharp.Math.Vector3> colliderPositions = raysCallback.HitPointWorld;
BulletSharp.Math.Vector3 colliderPosition = BulletSharp.Math.Vector3.Zero;
float distanceToCollider = 0;
//Set the initial distance as the distance between emitter and receiver
if (main != null && main.IsMetric)
{
distanceToCollider = sensorOffset;
}
else
{
distanceToCollider = Auxiliary.ToFeet(sensorOffset);
}
//Loop through all hitpoints (exclude the origin), if there is at least one hitpoint less than the distance between two sensors,
//something should block the beam between emitter and receiver
foreach (BulletSharp.Math.Vector3 pos in colliderPositions)
{
if ((pos - fromUltra).Length < distanceToCollider && !pos.Equals(BulletSharp.Math.Vector3.Zero))
{
distanceToCollider = (pos - fromUltra).Length;
colliderPosition = pos;
}
}
//Again if the line connects to the middle of the field nothing is blocking the beam
Debug.DrawLine(fromUltra.ToUnity(), colliderPosition.ToUnity(), Color.blue);
if (distanceToCollider < sensorOffset)
{
//Something is there
state = "Broken";
return(1);
}
else
{
//Nothing in between
state = "Unbroken";
return(0);
}
}
//Step #2
public override float ReturnOutput()
{
//Raycasting begins
Ray ray = new Ray(gameObject.transform.position, transform.forward);
BulletSharp.Math.Vector3 fromUltra = ray.origin.ToBullet();
BulletSharp.Math.Vector3 toCollider = ray.GetPoint(MaxRange).ToBullet();
Vector3 toColliderUnity = toCollider.ToUnity();
//Callback returns all hit point results in order to avoid non colliders interfere with the ray test
AllHitsRayResultCallback raysCallback = new AllHitsRayResultCallback(fromUltra, toCollider);
//Retrieves bullet physics world and does a ray test with the given coordinates and updates the callback object
BPhysicsWorld world = BPhysicsWorld.Get();
world.world.RayTest(fromUltra, toCollider, raysCallback);
//Gets the position of all hit points of the ray test
List <BulletSharp.Math.Vector3> colliderPositions = raysCallback.HitPointWorld;
BulletSharp.Math.Vector3 colliderPosition = BulletSharp.Math.Vector3.Zero;
float distanceToCollider = MaxRange;
if (main != null && main.IsMetric)
{
distanceToCollider = MaxRange;
foreach (BulletSharp.Math.Vector3 pos in colliderPositions)
{
if ((pos - fromUltra).Length < MaxRange && !pos.Equals(BulletSharp.Math.Vector3.Zero))
{
distanceToCollider = (pos - fromUltra).Length;
colliderPosition = pos;
}
}
}
else
{
distanceToCollider = Auxiliary.ToFeet(MaxRange);
foreach (BulletSharp.Math.Vector3 pos in colliderPositions)
{
if (Auxiliary.ToFeet((pos - fromUltra).Length) < distanceToCollider && !pos.Equals(BulletSharp.Math.Vector3.Zero))
{
distanceToCollider = Auxiliary.ToFeet((pos - fromUltra).Length);
colliderPosition = pos;
}
}
}
//Draw a line to view the ray action
//When the ray links to the middle of the field, it means the sensor is out of range
Debug.DrawLine(fromUltra.ToUnity(), colliderPosition.ToUnity(), Color.green, 5f);
return(distanceToCollider);
}
//Step #2
public override float ReturnOutput()
{
//Raycasting begins
Ray ray = new Ray(gameObject.transform.position, transform.forward);
BulletSharp.Math.Vector3 fromUltra = ray.origin.ToBullet();
BulletSharp.Math.Vector3 toCollider = ray.GetPoint(MaxRange).ToBullet();
Vector3 toColliderUnity = toCollider.ToUnity();
//Callback returns all hit point results in order to avoid non colliders interfere with the ray test
AllHitsRayResultCallback raysCallback = new AllHitsRayResultCallback(fromUltra, toCollider);
//Retrieves bullet physics world and does a ray test with the given coordinates and updates the callback object
BPhysicsWorld world = BPhysicsWorld.Get();
world.world.RayTest(fromUltra, toCollider, raysCallback);
//Gets the position of all hit points of the ray test
List <BulletSharp.Math.Vector3> colliderPositions = raysCallback.HitPointWorld;
BulletSharp.Math.Vector3 colliderPosition = BulletSharp.Math.Vector3.Zero;
float distanceToCollider = MaxRange;
//Loop through all hit points and get the shortest distance, exclude the origin since it is also counted as a hit point
foreach (BulletSharp.Math.Vector3 pos in colliderPositions)
{
if ((pos - fromUltra).Length <= MaxRange && (pos - fromUltra).Length < distanceToCollider && !pos.Equals(BulletSharp.Math.Vector3.Zero))
{
distanceToCollider = (pos - fromUltra).Length;
colliderPosition = pos;
}
}
//Draw a line to view the ray action
//When the ray links to the middle of the field, it means the sensor is out of range
Debug.DrawLine(fromUltra.ToUnity(), colliderPosition.ToUnity(), Color.green, 5f);
//setting shortest distance of a collider to the maxRange, then if any colliders are closer to the sensor,
//their distanceToCollider value becomes the new shortest distance
float shortestDistance = MaxRange;
if (!isMetric)
{
distanceToCollider = AuxFunctions.ToFeet(distanceToCollider);
}
//A check that might be useful in the future if use a bundle of rays instead of a single ray
if (distanceToCollider < shortestDistance)
{
shortestDistance = distanceToCollider;
}
return(shortestDistance);
}
/// Perform a ray-hit test with the specified collision model.
public override bool RayHit(ChVector from,
ChVector to,
ChCollisionModel model,
ref ChRayhitResult mresult,
CollisionFilterGroups filter_group,
CollisionFilterGroups filter_mask)
{
IndexedVector3 btfrom = new IndexedVector3((float)from.x, (float)from.y, (float)from.z);
IndexedVector3 btto = new IndexedVector3((float)to.x, (float)to.y, (float)to.z);
BulletXNA.BulletCollision.AllHitsRayResultCallback rayCallback = new AllHitsRayResultCallback(btfrom, btto);
rayCallback.m_collisionFilterGroup = filter_group;
rayCallback.m_collisionFilterMask = filter_mask;
this.bt_collision_world.rayTest(btfrom, btto, rayCallback);
// Find the closest hit result on the specified model (if any)
int hit = -1;
float fraction = 1;
for (int i = 0; i < rayCallback.m_collisionObjects.Count; ++i)
{
if (rayCallback.m_collisionObjects[i].GetUserPointer() == model && rayCallback.m_hitFractions[i] < fraction)
{
hit = i;
fraction = rayCallback.m_hitFractions[i];
}
}
// Ray does not hit specified model
if (hit == -1)
{
mresult.hit = false;
return(false);
}
// Return the closest hit on the specified model
mresult.hit = true;
mresult.hitModel = (ChCollisionModel)(rayCallback.m_collisionObjects[hit].GetUserPointer());
mresult.abs_hitPoint.Set(rayCallback.m_hitPointWorld[hit].X, rayCallback.m_hitPointWorld[hit].Y,
rayCallback.m_hitPointWorld[hit].Z);
mresult.abs_hitNormal.Set(rayCallback.m_hitNormalWorld[hit].X, rayCallback.m_hitNormalWorld[hit].Y,
rayCallback.m_hitNormalWorld[hit].Z);
mresult.abs_hitNormal.Normalize();
mresult.dist_factor = fraction;
mresult.abs_hitPoint = mresult.abs_hitPoint - mresult.abs_hitNormal * mresult.hitModel.GetEnvelope();
return(true);
}
public bool RayCastAll(Vector3 from, Vector3 to, int filterMask, int filterGroup, List <Vector3> contactPoints, List <Vector3> contactNormals)
{
bool hasHit = false;
AllHitsRayResultCallback callback = new AllHitsRayResultCallback(from, to);
callback.m_collisionFilterGroup = (CollisionFilterGroups)filterGroup;
callback.m_collisionFilterMask = (CollisionFilterGroups)filterMask;
hasHit = callback.HasHit();
if (hasHit)
{
int numHits = callback.m_hitNormalWorld.Count;
for (int i = 0; i < numHits; ++i)
{
contactPoints.Add(callback.m_hitPointWorld[i]);
contactNormals.Add(callback.m_hitNormalWorld[i]);
}
}
return(hasHit);
}
public override bool Raycast(GameSystem.GameCore.SerializableMath.Vector3 startPoint, GameSystem.GameCore.SerializableMath.Vector3 endPoint, out GameSystem.GameCore.SerializableMath.Vector3[] hitPoint, out CollisionProxy[] hitObject, int mask = -1)
{
// transfer serializable math position to bullet math position
BulletSharp.Math.Vector3 start = startPoint.ToBullet(), end = endPoint.ToBullet();
var callback = new AllHitsRayResultCallback(start, end);
// set group mask filter
callback.CollisionFilterMask = (short)mask;
world.RayTest(start, end, callback);
if (callback.HasHit)
{
hitPoint = callback.HitPointWorld.ToSerializableArray();
hitObject = callback.CollisionObjects.SelectToArray(colObj => (CollisionProxy)colObj.UserObject);
}
else
{
hitPoint = new GameSystem.GameCore.SerializableMath.Vector3[0];
hitObject = new CollisionProxy[0];
}
return(callback.HasHit);
}
public void Evaluate(int dummy)
{
if (this.FWorld.IsConnected)
{
fraction.Clear();
position.Clear();
normal.Clear();
body.Clear();
qidx.Clear();
//Ignore slice count for excluded bodies, as 0 is allowed (means we do a full search)
int spreadMax = SpreadUtils.SpreadMax(FWorld, FFrom, FTo);
this.FHit.SliceCount = spreadMax;
this.FHitCount.SliceCount = spreadMax;
for (int i = 0; i < spreadMax; i++)
{
Vector3 from = this.FFrom[i].ToBulletVector();
Vector3 to = this.FTo[i].ToBulletVector();
if (cb == null)
{
cb = new AllHitsRayResultCallback(from, to);
}
cb.HitFractions.Clear();
cb.HitNormalWorld.Clear();
cb.HitPointWorld.Clear();
cb.CollisionObjects.Clear();
this.FWorld[0].World.RayTest(from, to, cb);
if (cb.HasHit)
{
this.FHitCount[i] = cb.HitFractions.Count;
float minfrac = float.MaxValue;
RigidBody closest = null;
int minidx = 0;
for (int h = 0; h < cb.HitFractions.Count; h++)
{
RigidBody rb = (RigidBody)cb.CollisionObjects[h];
BodyCustomData bd = (BodyCustomData)rb.UserObject;
if (cb.HitFractions[h] < minfrac && !this.FExcludedBody.Contains(rb))
{
closest = rb;
minidx = h;
}
}
if (closest != null)
{
this.FHit[i] = true;
Vector3 diff = to - from;
Vector3 inter = from + diff * cb.HitFractions[minidx];
position.Add(inter.ToVVVVector());
fraction.Add(cb.HitFractions[minidx]);
normal.Add(cb.HitNormalWorld[minidx].ToVVVVector());
body.Add(closest);
qidx.Add(i);
}
else
{
this.FHit[i] = false;
}
}
else
{
this.FHit[i] = false;
this.FHitCount[i] = 0;
}
}
this.FHitFraction.SliceCount = fraction.Count;
this.FHitNormal.SliceCount = fraction.Count;
this.FHitPosition.SliceCount = fraction.Count;
this.FQueryIndex.SliceCount = fraction.Count;
this.FBody.SliceCount = fraction.Count;
for (int i = 0; i < fraction.Count; i++)
{
this.FHitFraction[i] = fraction[i];
this.FHitNormal[i] = normal[i];
this.FHitPosition[i] = position[i];
this.FBody[i] = body[i];
this.FQueryIndex[i] = qidx[i];
}
}
else
{
this.FHit.SliceCount = 0;
this.FHitFraction.SliceCount = 0;
this.FHitPosition.SliceCount = 0;
this.FHitNormal.SliceCount = 0;
this.FHitCount.SliceCount = 0;
this.FQueryIndex.SliceCount = 0;
}
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(AllHitsRayResultCallback obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
