// Steer away from obstacles in the way. This method returns a zero vector if no correction is required.
// It should be high priority and the steering from other behaviors should blend into the remaining space.
// So if this returns a length 1.0f vector, avoiding the obstacle is most urgent and there is no room for other
// steering.
private Vector2 AvoidObstacles(Actor owner)
{
BipedControllerComponent bcc = owner.GetComponent<BipedControllerComponent>(ActorComponent.ComponentType.Control);
// Conditions where we do not want to use this steering force.
if (GetAngleFromVertical(bcc.Controller.Body.LinearVelocity) < MathHelper.PiOver4 || // We're probably falling...
!bcc.Controller.SupportFinder.HasSupport ||
!bcc.Controller.SupportFinder.HasTraction)
return Vector2.Zero;
// Sphere cast ahead along facing.
List<RayCastResult> obstacles = new List<RayCastResult>();
SphereShape probe = new SphereShape(bcc.Controller.BodyRadius * 1.1f);
RigidTransform probeStartPosition = new RigidTransform(bcc.Controller.Body.Position);
// Add a small constant to the probe length because we want a minimum amount of forward probing, even if we are not moving.
float probeLength = Math.Max(BepuVec3.Dot(bcc.Controller.Body.LinearVelocity, bcc.Controller.ViewDirection), 0.0f) + 1.0f;
BepuVec3 probeSweep = bcc.Controller.ViewDirection * probeLength;
ObstacleFilter filter = new ObstacleFilter(bcc.Controller.Body.CollisionInformation);
GameResources.ActorManager.SimSpace.ConvexCast(probe, ref probeStartPosition, ref probeSweep, filter.Test, obstacles);
RayCastDistanceComparer rcdc = new RayCastDistanceComparer();
obstacles.Sort(rcdc);
BEPUutilities.Vector3 cross = BEPUutilities.Vector3.Zero;
int obstacleIndex = 0;
do
{
if (obstacles.Count == obstacleIndex)
return Vector2.Zero;
cross = BEPUutilities.Vector3.Cross(bcc.Controller.ViewDirection, -obstacles[obstacleIndex++].HitData.Normal);
}
while (cross.X > 0.7f); // if cross.X > 0.7f, the obstacle is some kind of gentle ramp; ignore it.
// dot will typically be negative and magnitude indicates how directly ahead the obstacle is.
float dot = BEPUutilities.Vector3.Dot(bcc.Controller.ViewDirection, -obstacles[0].HitData.Normal);
if (dot >= 0.0f) // The obstacle won't hinder us if we touch it.
return Vector2.Zero;
// When cross.Y is positive, the object is generally to the right, so veer left (and vice versa).
float directionSign = cross.Y >= 0.0f ? -1.0f : 1.0f;
BEPUutilities.Vector2 result = BEPUutilities.Vector2.UnitX * directionSign * -dot;
// Also scale response by how close the obstacle is.
float distance = (obstacles[0].HitData.Location - bcc.Controller.Body.Position).Length();
result *= MathHelper.Clamp((1.0f - distance / probeLength), 0.0f, 1.0f); // / Math.Abs(dot);
// So far the result is in terms of 'velocity space'. Rotate it to align with the controller facing.
float velocityTheta = (float)(Math.Atan2(-probeSweep.X, -probeSweep.Z));
BEPUutilities.Matrix2x2 velocityWorld = SpaceUtils.Create2x2RotationMatrix(velocityTheta);
float facingTheta = (float)(Math.Atan2(-bcc.Controller.HorizontalViewDirection.X, -bcc.Controller.HorizontalViewDirection.Z));
BEPUutilities.Matrix2x2 facingWorldInv = SpaceUtils.Create2x2RotationMatrix(facingTheta);
facingWorldInv.Transpose(); // We want the transpose/inverse of the facing transform because we want to transform the movement into 'facing space'.
return BepuConverter.Convert(SpaceUtils.TransformVec2(SpaceUtils.TransformVec2(result, velocityWorld), facingWorldInv));
}
// Steer away from obstacles in the way. This method returns a zero vector if no correction is required.
// It should be high priority and the steering from other behaviors should blend into the remaining space.
// So if this returns a length 1.0f vector, avoiding the obstacle is most urgent and there is no room for other
// steering.
private Vector2 AvoidObstacles(Actor owner)
{
BipedControllerComponent bcc = owner.GetComponent <BipedControllerComponent>(ActorComponent.ComponentType.Control);
// Conditions where we do not want to use this steering force.
if (GetAngleFromVertical(bcc.Controller.Body.LinearVelocity) < MathHelper.PiOver4 || // We're probably falling...
!bcc.Controller.SupportFinder.HasSupport ||
!bcc.Controller.SupportFinder.HasTraction)
{
return(Vector2.Zero);
}
// Sphere cast ahead along facing.
List <RayCastResult> obstacles = new List <RayCastResult>();
SphereShape probe = new SphereShape(bcc.Controller.BodyRadius * 1.1f);
RigidTransform probeStartPosition = new RigidTransform(bcc.Controller.Body.Position);
// Add a small constant to the probe length because we want a minimum amount of forward probing, even if we are not moving.
float probeLength = Math.Max(BepuVec3.Dot(bcc.Controller.Body.LinearVelocity, bcc.Controller.ViewDirection), 0.0f) + 1.0f;
BepuVec3 probeSweep = bcc.Controller.ViewDirection * probeLength;
ObstacleFilter filter = new ObstacleFilter(bcc.Controller.Body.CollisionInformation);
GameResources.ActorManager.SimSpace.ConvexCast(probe, ref probeStartPosition, ref probeSweep, filter.Test, obstacles);
RayCastDistanceComparer rcdc = new RayCastDistanceComparer();
obstacles.Sort(rcdc);
BEPUutilities.Vector3 cross = BEPUutilities.Vector3.Zero;
int obstacleIndex = 0;
do
{
if (obstacles.Count == obstacleIndex)
{
return(Vector2.Zero);
}
cross = BEPUutilities.Vector3.Cross(bcc.Controller.ViewDirection, -obstacles[obstacleIndex++].HitData.Normal);
}while (cross.X > 0.7f); // if cross.X > 0.7f, the obstacle is some kind of gentle ramp; ignore it.
// dot will typically be negative and magnitude indicates how directly ahead the obstacle is.
float dot = BEPUutilities.Vector3.Dot(bcc.Controller.ViewDirection, -obstacles[0].HitData.Normal);
if (dot >= 0.0f) // The obstacle won't hinder us if we touch it.
{
return(Vector2.Zero);
}
// When cross.Y is positive, the object is generally to the right, so veer left (and vice versa).
float directionSign = cross.Y >= 0.0f ? -1.0f : 1.0f;
BEPUutilities.Vector2 result = BEPUutilities.Vector2.UnitX * directionSign * -dot;
// Also scale response by how close the obstacle is.
float distance = (obstacles[0].HitData.Location - bcc.Controller.Body.Position).Length();
result *= MathHelper.Clamp((1.0f - distance / probeLength), 0.0f, 1.0f); // / Math.Abs(dot);
// So far the result is in terms of 'velocity space'. Rotate it to align with the controller facing.
float velocityTheta = (float)(Math.Atan2(-probeSweep.X, -probeSweep.Z));
BEPUutilities.Matrix2x2 velocityWorld = SpaceUtils.Create2x2RotationMatrix(velocityTheta);
float facingTheta = (float)(Math.Atan2(-bcc.Controller.HorizontalViewDirection.X, -bcc.Controller.HorizontalViewDirection.Z));
BEPUutilities.Matrix2x2 facingWorldInv = SpaceUtils.Create2x2RotationMatrix(facingTheta);
facingWorldInv.Transpose(); // We want the transpose/inverse of the facing transform because we want to transform the movement into 'facing space'.
return(BepuConverter.Convert(SpaceUtils.TransformVec2(SpaceUtils.TransformVec2(result, velocityWorld), facingWorldInv)));
}
