com.badlogic.gdx.ai.steer.SteeringAcceleration Java Examples

public CollisionAvoidanceSteererBase(final SteerableBody steerableBody) {
	super(steerableBody);

	this.proximity = new RadiusProximity<Vector3>(steerableBody, GameScreen.screen.engine.characters, steerableBody.getBoundingRadius() * 1.8f);
	this.collisionAvoidanceSB = new CollisionAvoidance<Vector3>(steerableBody, proximity) {
		@Override
		protected SteeringAcceleration<Vector3> calculateRealSteering(SteeringAcceleration<Vector3> steering) {
			super.calculateRealSteering(steering);
			steering.linear.y = 0; // remove any vertical acceleration
			return steering;
		}
	};

	this.prioritySteering = new PrioritySteering<Vector3>(steerableBody, 0.001f) //
		.add(collisionAvoidanceSB);
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> blendedSteering) {
	// Clear the output to start with
	blendedSteering.setZero();

	// Go through all the behaviors
	int len = list.size;
	for (int i = 0; i < len; i++) {
		BehaviorAndWeight<T> bw = list.get(i);

		// Calculate the behavior's steering
		bw.behavior.calculateSteering(steering);

		// Scale and add the steering to the accumulator
		blendedSteering.mulAdd(steering, bw.weight);
	}

	Limiter actualLimiter = getActualLimiter();

	// Crop the result
	blendedSteering.linear.limit(actualLimiter.getMaxLinearAcceleration());
	if (blendedSteering.angular > actualLimiter.getMaxAngularAcceleration())
		blendedSteering.angular = actualLimiter.getMaxAngularAcceleration();

	return blendedSteering;
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	// First we need to figure out where the two agents are going to be at
	// time T in the future. This is approximated by determining the time
	// taken by the owner to reach the desired point between the 2 agents
	// at the current time at the max speed. This desired point P is given by
	// P = posA + interpositionRatio * (posB - posA)
	internalTargetPosition.set(agentB.getPosition()).sub(agentA.getPosition()).scl(interpositionRatio)
		.add(agentA.getPosition());

	float timeToTargetPosition = owner.getPosition().dst(internalTargetPosition) / getActualLimiter().getMaxLinearSpeed();

	// Now we have the time, we assume that agent A and agent B will continue on a
	// straight trajectory and extrapolate to get their future positions.
	// Note that here we are reusing steering.linear vector as agentA future position
	// and targetPosition as agentB future position.
	steering.linear.set(agentA.getPosition()).mulAdd(agentA.getLinearVelocity(), timeToTargetPosition);
	internalTargetPosition.set(agentB.getPosition()).mulAdd(agentB.getLinearVelocity(), timeToTargetPosition);

	// Calculate the target position between these predicted positions
	internalTargetPosition.sub(steering.linear).scl(interpositionRatio).add(steering.linear);

	// Finally delegate to Arrive
	return arrive(steering, internalTargetPosition);
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	// We'll need epsilon squared later.
	float epsilonSquared = epsilon * epsilon;

	// Go through the behaviors until one has a large enough acceleration
	int n = behaviors.size;
	selectedBehaviorIndex = -1;
	for (int i = 0; i < n; i++) {
		selectedBehaviorIndex = i;

		SteeringBehavior<T> behavior = behaviors.get(i);

		// Calculate the behavior's steering
		behavior.calculateSteering(steering);

		// If we're above the threshold return the current steering
		if (steering.calculateSquareMagnitude() > epsilonSquared) return steering;
	}

	// If we get here, it means that no behavior had a large enough acceleration,
	// so return the small acceleration from the final behavior or zero if there are
	// no behaviors in the list.
	return n > 0 ? steering : steering.setZero();
}
 

private void applySteering (SteeringAcceleration<Vector2> steering, float time) {
	// Update position and linear velocity. Velocity is trimmed to maximum speed
	position.mulAdd(linearVelocity, time);
	linearVelocity.mulAdd(steering.linear, time).limit(getMaxLinearSpeed());

	// Update orientation and angular velocity
	if (independentFacing) {
		setRotation(getRotation() + (angularVelocity * time) * MathUtils.radiansToDegrees);
		angularVelocity += steering.angular * time;
	} else {
		// If we haven't got any velocity, then we can do nothing.
		if (!linearVelocity.isZero(getZeroLinearSpeedThreshold())) {
			float newOrientation = vectorToAngle(linearVelocity);
			angularVelocity = (newOrientation - getRotation() * MathUtils.degreesToRadians) * time; // this is superfluous if independentFacing is always true
			setRotation(newOrientation * MathUtils.radiansToDegrees);
		}
	}
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	steering.setZero();

	averageVelocity = steering.linear;

	int neighborCount = proximity.findNeighbors(this);

	if (neighborCount > 0) {
		// Average the accumulated velocities
		averageVelocity.scl(1f / neighborCount);

		// Match the average velocity.
		// Notice that steering.linear and averageVelocity are the same vector here.
		averageVelocity.sub(owner.getLinearVelocity()).limit(getActualLimiter().getMaxLinearAcceleration());
	}

	return steering;
}
 

public WanderSteerer(final SteerableBody steerableBody) {
	super(steerableBody);

	this.wanderSB = new Wander<Vector3>(steerableBody) {
		@Override
		protected SteeringAcceleration<Vector3> calculateRealSteering(SteeringAcceleration<Vector3> steering) {
			super.calculateRealSteering(steering);
			steering.linear.y = 0; // remove any vertical acceleration
			return steering;
		}
	};
	this.wanderSB.setWanderOffset(8) //
		.setWanderOrientation(0) //
		.setWanderRadius(0.5f) //
		.setWanderRate(MathUtils.PI2 * 4);

	this.prioritySteering.add(wanderSB);
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	// Check for a zero direction, and return no steering if so
	if (owner.getLinearVelocity().isZero(getActualLimiter().getZeroLinearSpeedThreshold())) return steering.setZero();

	// Calculate the orientation based on the velocity of the owner
	float orientation = owner.vectorToAngle(owner.getLinearVelocity());

	// Delegate to ReachOrientation
	return reachOrientation(steering, orientation);
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	// Initialize member variables used by the callback
	this.distance2ToClosest = Float.POSITIVE_INFINITY;
	this.toObstacle = steering.linear;

	// Find neighbors (the obstacles) using this behavior as callback
	int neighborsCount = proximity.findNeighbors(this);

	// If no suitable obstacles found return no steering otherwise use Arrive on the hiding spot
	return neighborsCount == 0 ? steering.setZero() : arrive(steering, bestHidingSpot);
}
 

/** Produces a steering that tries to align the owner to the target orientation. This method is called by subclasses that want
 * to align to a certain orientation.
 * @param steering the steering to be calculated.
 * @param targetOrientation the target orientation you want to align to.
 * @return the calculated steering for chaining. */
protected SteeringAcceleration<T> reachOrientation (SteeringAcceleration<T> steering, float targetOrientation) {
	// Get the rotation direction to the target wrapped to the range [-PI, PI]
	float rotation = ArithmeticUtils.wrapAngleAroundZero(targetOrientation - owner.getOrientation());

	// Absolute rotation
	float rotationSize = rotation < 0f ? -rotation : rotation;

	// Check if we are there, return no steering
	if (rotationSize <= alignTolerance) return steering.setZero();

	Limiter actualLimiter = getActualLimiter();

	// Use maximum rotation
	float targetRotation = actualLimiter.getMaxAngularSpeed();

	// If we are inside the slow down radius, then calculate a scaled rotation
	if (rotationSize <= decelerationRadius) targetRotation *= rotationSize / decelerationRadius;

	// The final target rotation combines
	// speed (already in the variable) and direction
	targetRotation *= rotation / rotationSize;

	// Acceleration tries to get to the target rotation
	steering.angular = (targetRotation - owner.getAngularVelocity()) / timeToTarget;

	// Check if the absolute acceleration is too great
	float angularAcceleration = steering.angular < 0f ? -steering.angular : steering.angular;
	if (angularAcceleration > actualLimiter.getMaxAngularAcceleration())
		steering.angular *= actualLimiter.getMaxAngularAcceleration() / angularAcceleration;

	// No linear acceleration
	steering.linear.setZero();

	// Output the steering
	return steering;
}
 

/**
 * Applies the linear component of the steering behaviour. As for the angular component,
 * the orientation of the model and the body is set to follow the direction of motion (non independent facing).
 *
 * @param steering the steering acceleration to apply
 * @param deltaTime the time between this frame and the previous one
 */
protected void applySteering(SteeringAcceleration<Vector3> steering, float deltaTime) {
	// Update linear velocity trimming it to maximum speed
	linearVelocity.set(body.getLinearVelocity().mulAdd(steering.linear, deltaTime).limit(getMaxLinearSpeed()));
	body.setLinearVelocity(linearVelocity);

	// Failed attempt to clear angular velocity possibly due to collision
	// Actually, this issue has been fixed by setting the angular factor
	// to Vector3.Zero in SteerableBody constructor
	//body.setAngularVelocity(Vector3.Zero);
	
	// Maybe we should do this even if applySteering is not invoked
	// since the entity might move because of other bodies that are pushing it 
	updateSteerableData(GameScreen.screen.engine.getScene());

	// Calculate the target orientation of the model based on the direction of motion
	// Note that the entity might twitch or jitter slightly when it finds itself in a situation with  
	// conflicting responses from different behaviors. If you need to mitigate this scenario you can decouple
	// the heading from the velocity vector and average its value over the last few frames, for instance 5.
	// This smoothed heading vector will be used to work out model's orientation.
	if (!linearVelocity.isZero(getZeroLinearSpeedThreshold())) {
		position = getPosition();
		targetOrientationVector.set(linearVelocity.x, 0, -linearVelocity.z).nor();
		modelTransform.setToLookAt(targetOrientationVector, Constants.V3_UP).setTranslation(position);
		body.setWorldTransform(modelTransform);

		targetOrientation.setFromMatrix(true, tmpMatrix.setToLookAt(targetOrientationVector, Constants.V3_UP));

		// Set current orientation of model, setting orientation of body causes problems when applying force.
		currentOrientation.slerp(targetOrientation, 10 * deltaTime);
		modelTransform.setFromEulerAngles(
				currentOrientation.getYaw(),
				currentOrientation.getPitch(),
				currentOrientation.getRoll()).setTranslation(position);
	}

	body.activate();
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	T targetPosition = target.getPosition();

	// Get the square distance to the evader (the target)
	float squareDistance = steering.linear.set(targetPosition).sub(owner.getPosition()).len2();

	// Work out our current square speed
	float squareSpeed = owner.getLinearVelocity().len2();

	float predictionTime = maxPredictionTime;

	if (squareSpeed > 0) {
		// Calculate prediction time if speed is not too small to give a reasonable value
		float squarePredictionTime = squareDistance / squareSpeed;
		if (squarePredictionTime < maxPredictionTime * maxPredictionTime)
			predictionTime = (float)Math.sqrt(squarePredictionTime);
	}

	// Calculate and seek/flee the predicted position of the target
	steering.linear.set(targetPosition).mulAdd(target.getLinearVelocity(), predictionTime).sub(owner.getPosition()).nor()
		.scl(getActualMaxLinearAcceleration());

	// No angular acceleration
	steering.angular = 0;

	// Output steering acceleration
	return steering;
}
 

@Override
public SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	// Check if we have a trajectory, and create one if not.
	if (target == null) {
		target = calculateTarget();
		callback.reportAchievability(isJumpAchievable);
	}

	// If the trajectory is zero, return no steering acceleration
	if (!isJumpAchievable) return steering.setZero();

	// Check if the owner has reached target position and velocity with acceptable tolerance
	if (owner.getPosition().epsilonEquals(target.getPosition(), takeoffPositionTolerance)) {
		if (DEBUG_ENABLED) GdxAI.getLogger().info("Jump", "Good position!!!");
		if (owner.getLinearVelocity().epsilonEquals(target.getLinearVelocity(), takeoffVelocityTolerance)) {
			if (DEBUG_ENABLED) GdxAI.getLogger().info("Jump", "Good Velocity!!!");
			isJumpAchievable = false;
			// Perform the jump, and return no steering (the owner is airborne, no need to steer).
			callback.takeoff(maxVerticalVelocity, airborneTime);
			return steering.setZero();
		} else {
			if (DEBUG_ENABLED)
				GdxAI.getLogger().info("Jump",
					"Bad Velocity: Speed diff. = "
						+ planarVelocity.set(target.getLinearVelocity()).sub(owner.getLinearVelocity()).len() + ", diff = ("
						+ planarVelocity + ")");
		}
	}

	// Delegate to MatchVelocity
	return super.calculateRealSteering(steering);
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {

	// Predictive or non-predictive behavior?
	T location = (predictionTime == 0) ?
	// Use the current position of the owner
	owner.getPosition()
		:
		// Calculate the predicted future position of the owner. We're reusing steering.linear here.
		steering.linear.set(owner.getPosition()).mulAdd(owner.getLinearVelocity(), predictionTime);

	// Find the distance from the start of the path
	float distance = path.calculateDistance(location, pathParam);

	// Offset it
	float targetDistance = distance + pathOffset;

	// Calculate the target position
	path.calculateTargetPosition(internalTargetPosition, pathParam, targetDistance);

	if (arriveEnabled && path.isOpen()) {
		if (pathOffset >= 0) {
			// Use Arrive to approach the last point of the path
			if (targetDistance > path.getLength() - decelerationRadius) return arrive(steering, internalTargetPosition);
		} else {
			// Use Arrive to approach the first point of the path
			if (targetDistance < decelerationRadius) return arrive(steering, internalTargetPosition);
		}
	}

	// Seek the target position
	steering.linear.set(internalTargetPosition).sub(owner.getPosition()).nor()
		.scl(getActualLimiter().getMaxLinearAcceleration());

	// No angular acceleration
	steering.angular = 0;

	// Output steering acceleration
	return steering;
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	// Predictive or non-predictive behavior?
	T location = (predictionTime == 0) ?
	// Use the current position of the owner
	owner.getPosition()
		:
		// Calculate the predicted future position of the owner. We're reusing steering.linear here.
		steering.linear.set(owner.getPosition()).mulAdd(owner.getLinearVelocity(), predictionTime);

	// Retrieve the flow vector at the specified location
	T flowVector = flowField.lookup(location);
	
	// Clear both linear and angular components
	steering.setZero();

	if (flowVector != null && !flowVector.isZero()) {
		Limiter actualLimiter = getActualLimiter();

		// Calculate linear acceleration
		steering.linear.mulAdd(flowVector, actualLimiter.getMaxLinearSpeed()).sub(owner.getLinearVelocity())
			.limit(actualLimiter.getMaxLinearAcceleration());
	}

	// Output steering
	return steering;
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	// Update the wander orientation
	float now = GdxAI.getTimepiece().getTime();
	if (lastTime > 0) {
		float delta = now - lastTime;
		wanderOrientation += MathUtils.randomTriangular(wanderRate * delta);
	}
	lastTime = now;

	// Calculate the combined target orientation
	float targetOrientation = wanderOrientation + owner.getOrientation();

	// Calculate the center of the wander circle
	wanderCenter.set(owner.getPosition()).mulAdd(owner.angleToVector(steering.linear, owner.getOrientation()), wanderOffset);

	// Calculate the target location
	// Notice that we're using steering.linear as temporary vector
	internalTargetPosition.set(wanderCenter).mulAdd(owner.angleToVector(steering.linear, targetOrientation), wanderRadius);

	float maxLinearAcceleration = getActualLimiter().getMaxLinearAcceleration();

	if (faceEnabled) {
		// Delegate to face
		face(steering, internalTargetPosition);

		// Set the linear acceleration to be at full
		// acceleration in the direction of the orientation
		owner.angleToVector(steering.linear, owner.getOrientation()).scl(maxLinearAcceleration);
	} else {
		// Seek the internal target position
		steering.linear.set(internalTargetPosition).sub(owner.getPosition()).nor().scl(maxLinearAcceleration);

		// No angular acceleration
		steering.angular = 0;

	}

	return steering;
}
 

/** Creates a {@code BlendedSteering} for the specified {@code owner}, {@code maxLinearAcceleration} and
 * {@code maxAngularAcceleration}.
 * @param owner the owner of this behavior. */
public BlendedSteering (Steerable<T> owner) {
	super(owner);

	this.list = new Array<BehaviorAndWeight<T>>();
	this.steering = new SteeringAcceleration<T>(newVector(owner));
}
 

protected SteeringAcceleration<T> face (SteeringAcceleration<T> steering, T targetPosition) {
	// Get the direction to target
	T toTarget = steering.linear.set(targetPosition).sub(owner.getPosition());

	// Check for a zero direction, and return no steering if so
	if (toTarget.isZero(getActualLimiter().getZeroLinearSpeedThreshold())) return steering.setZero();

	// Calculate the orientation to face the target
	float orientation = owner.vectorToAngle(toTarget);

	// Delegate to ReachOrientation
	return reachOrientation(steering, orientation);
}
 

protected SteeringAcceleration<T> arrive (SteeringAcceleration<T> steering, T targetPosition) {
	// Get the direction and distance to the target
	T toTarget = steering.linear.set(targetPosition).sub(owner.getPosition());
	float distance = toTarget.len();

	// Check if we are there, return no steering
	if (distance <= arrivalTolerance) return steering.setZero();

	Limiter actualLimiter = getActualLimiter();
	// Go max speed
	float targetSpeed = actualLimiter.getMaxLinearSpeed();

	// If we are inside the slow down radius calculate a scaled speed
	if (distance <= decelerationRadius) targetSpeed *= distance / decelerationRadius;

	// Target velocity combines speed and direction
	T targetVelocity = toTarget.scl(targetSpeed / distance); // Optimized code for: toTarget.nor().scl(targetSpeed)

	// Acceleration tries to get to the target velocity without exceeding max acceleration
	// Notice that steering.linear and targetVelocity are the same vector
	targetVelocity.sub(owner.getLinearVelocity()).scl(1f / timeToTarget).limit(actualLimiter.getMaxLinearAcceleration());

	// No angular acceleration
	steering.angular = 0f;

	// Output the steering
	return steering;
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	// Try to match the position of the character with the position of the target by calculating
	// the direction to the target and by moving toward it as fast as possible.
	steering.linear.set(target.getPosition()).sub(owner.getPosition()).nor().scl(getActualLimiter().getMaxLinearAcceleration());

	// No angular acceleration
	steering.angular = 0;

	// Output steering acceleration
	return steering;
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	// We just do the opposite of seek, i.e. (owner.getPosition() - target.getPosition())
	// instead of (target.getPosition() - owner.getPosition())
	steering.linear.set(owner.getPosition()).sub(target.getPosition()).nor().scl(getActualLimiter().getMaxLinearAcceleration());

	// No angular acceleration
	steering.angular = 0;

	// Output steering acceleration
	return steering;
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	shortestTime = Float.POSITIVE_INFINITY;
	firstNeighbor = null;
	firstMinSeparation = 0;
	firstDistance = 0;
	relativePosition = steering.linear;

	// Take into consideration each neighbor to find the most imminent collision.
	int neighborCount = proximity.findNeighbors(this);

	// If we have no target, then return no steering acceleration
	//
	// NOTE: You might think that the condition below always evaluates to true since
	// firstNeighbor has been set to null when entering this method. In fact, we have just
	// executed findNeighbors(this) that has possibly set firstNeighbor to a non null value
	// through the method reportNeighbor defined below.
	if (neighborCount == 0 || firstNeighbor == null) return steering.setZero();

	// If we're going to hit exactly, or if we're already
	// colliding, then do the steering based on current position.
	if (firstMinSeparation <= 0 || firstDistance < owner.getBoundingRadius() + firstNeighbor.getBoundingRadius()) {
		relativePosition.set(firstNeighbor.getPosition()).sub(owner.getPosition());
	} else {
		// Otherwise calculate the future relative position
		relativePosition.set(firstRelativePosition).mulAdd(firstRelativeVelocity, shortestTime);
	}

	// Avoid the target
	// Notice that steerling.linear and relativePosition are the same vector
	relativePosition.nor().scl(-getActualLimiter().getMaxLinearAcceleration());

	// No angular acceleration
	steering.angular = 0f;

	// Output the steering
	return steering;
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	T ownerPosition = owner.getPosition();
	float minDistanceSquare = Float.POSITIVE_INFINITY;

	// Get the updated rays
	Ray<T>[] inputRays = rayConfiguration.updateRays();

	// Process rays
	for (int i = 0; i < inputRays.length; i++) {
		// Find the collision with current ray
		boolean collided = raycastCollisionDetector.findCollision(outputCollision, inputRays[i]);

		if (collided) {
			float distanceSquare = ownerPosition.dst2(outputCollision.point);
			if (distanceSquare < minDistanceSquare) {
				minDistanceSquare = distanceSquare;
				// Swap collisions
				Collision<T> tmpCollision = outputCollision;
				outputCollision = minOutputCollision;
				minOutputCollision = tmpCollision;
			}
		}
	}

	// Return zero steering if no collision has occurred
	if (minDistanceSquare == Float.POSITIVE_INFINITY) return steering.setZero();

	// Calculate and seek the target position
	steering.linear.set(minOutputCollision.point)
		.mulAdd(minOutputCollision.normal, owner.getBoundingRadius() + distanceFromBoundary).sub(owner.getPosition()).nor()
		.scl(getActualLimiter().getMaxLinearAcceleration());

	// No angular acceleration
	steering.angular = 0;

	// Output steering acceleration
	return steering;
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	// Acceleration tries to get to the target velocity without exceeding max acceleration
	steering.linear.set(target.getLinearVelocity()).sub(owner.getLinearVelocity()).scl(1f / timeToTarget)
		.limit(getActualLimiter().getMaxLinearAcceleration());

	// No angular acceleration
	steering.angular = 0;

	// Output steering acceleration
	return steering;
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	return face(steering, target.getPosition());
}
 

@Override
public boolean processSteering (SteeringAcceleration<Vector3> steering) {
	return keepWandering;
}
 

@Override
public boolean processSteering(SteeringAcceleration<Vector3> steering) {

	// Check if steering target path segment changed.
	LinePathParam pathParam = followPathSB.getPathParam();
	int traversedSegment = pathParam.getSegmentIndex();
	if (traversedSegment > currentSegmentIndex) {
		currentSegmentIndex = traversedSegment;
	}

	if (prioritySteering.getSelectedBehaviorIndex() == 0) {
		/*
		 * Collision avoidance management
		 */
		float pr = proximity.getRadius() * 1.5f;
		if (linePath.getEndPoint().dst2(steerableBody.getPosition()) <= pr * pr) {
			// Disable collision avoidance near the end of the path since the obstacle
			// will likely prevent the entity from reaching the target.
			collisionAvoidanceSB.setEnabled(false);
			deadlockDetectionStartTime = Float.POSITIVE_INFINITY;
		} else if (deadlockDetection) {
			// Accumulate collision time during deadlock detection
			collisionDuration += GdxAI.getTimepiece().getDeltaTime();

			if (GdxAI.getTimepiece().getTime() - deadlockDetectionStartTime > DEADLOCK_TIME && collisionDuration > DEADLOCK_TIME * .6f) {
				// Disable collision avoidance since most of the deadlock detection period has been spent on collision avoidance
				collisionAvoidanceSB.setEnabled(false);
			}
		} else {
			// Start deadlock detection
			deadlockDetectionStartTime = GdxAI.getTimepiece().getTime();
			collisionDuration = 0;
			deadlockDetection = true;
		}
		return true;
	}

	/*
	 * Path following management
	 */
	float dst2FromPathEnd = steerableBody.getPosition().dst2(linePath.getEndPoint());

	// Check to see if the entity has reached the end of the path
	if (steering.isZero() && dst2FromPathEnd < followPathSB.getArrivalTolerance() * followPathSB.getArrivalTolerance()) {
		return false;
	}

	// Check if collision avoidance must be re-enabled
	if (deadlockDetection && !collisionAvoidanceSB.isEnabled() && GdxAI.getTimepiece().getTime() - deadlockDetectionStartTime > MAX_NO_COLLISION_TIME) {
			collisionAvoidanceSB.setEnabled(true);
			deadlockDetection = false;
	}
	
	// If linear speed is very low and the entity is colliding something at his feet, like a step of the stairs
	// for instance, we have to increase the acceleration to make him go upstairs. 
	float minVel = .2f;
	if (steerableBody.getLinearVelocity().len2() > minVel * minVel) {
		stationarityRayColor = null;
	} else {
		steerableBody.getGroundPosition(stationarityRayLow.origin).add(0, 0.05f, 0);
		steerableBody.getDirection(stationarityRayLow.direction).scl(1f, 0f, 1f).nor();
		stationarityRayLength = steerableBody.getBoundingRadius() + 0.4f;
		Entity hitEntityLow = GameScreen.screen.engine.rayTest(stationarityRayLow, null, GameEngine.ALL_FLAG, GameEngine.PC_FLAG, stationarityRayLength, null);
		if (hitEntityLow instanceof GameObject) {
			stationarityRayColor = Color.RED;
			stationarityRayHigh.set(stationarityRayLow);
			stationarityRayHigh.origin.add(0, .8f, 0);
			Entity hitEntityHigh = GameScreen.screen.engine.rayTest(stationarityRayHigh, null, GameEngine.ALL_FLAG, GameEngine.PC_FLAG, stationarityRayLength, null);
			if (hitEntityHigh == null) {
				// The entity is touching a small obstacle with his feet like a step of the stairs.
				// Increase the acceleration to make him go upstairs.
				steering.linear.scl(8);
			}
			else if (hitEntityHigh instanceof GameObject) {
				// The entity is touching a higher obstacle like a tree, a column or something.
				// Here we should invent something to circumvent this kind of obstacles :)
				//steering.linear.rotateRad(Constants.V3_UP, Constants.PI0_25);
			}
		} else {
			stationarityRayColor = Color.BLUE;
		}
	}

	return true;
}
 

@Override
protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) {
	return reachOrientation(steering, target.getOrientation());
}
 

protected void applySteering (SteeringAcceleration<Vector2> steering, float deltaTime) {
	boolean anyAccelerations = false;

	// Update position and linear velocity.
	if (!steeringOutput.linear.isZero()) {
		// this method internally scales the force by deltaTime
		body.applyForceToCenter(steeringOutput.linear, true);
		anyAccelerations = true;
	}

	// Update orientation and angular velocity
	if (isIndependentFacing()) {
		if (steeringOutput.angular != 0) {
			// this method internally scales the torque by deltaTime
			body.applyTorque(steeringOutput.angular, true);
			anyAccelerations = true;
		}
	} else {
		// If we haven't got any velocity, then we can do nothing.
		Vector2 linVel = getLinearVelocity();
		if (!linVel.isZero(getZeroLinearSpeedThreshold())) {
			float newOrientation = vectorToAngle(linVel);
			body.setAngularVelocity((newOrientation - getAngularVelocity()) * deltaTime); // this is superfluous if independentFacing is always true
			body.setTransform(body.getPosition(), newOrientation);
		}
	}

	if (anyAccelerations) {
		// body.activate();

		// TODO:
		// Looks like truncating speeds here after applying forces doesn't work as expected.
		// We should likely cap speeds form inside an InternalTickCallback, see
		// http://www.bulletphysics.org/mediawiki-1.5.8/index.php/Simulation_Tick_Callbacks

		// Cap the linear speed
		Vector2 velocity = body.getLinearVelocity();
		float currentSpeedSquare = velocity.len2();
		float maxLinearSpeed = getMaxLinearSpeed();
		if (currentSpeedSquare > maxLinearSpeed * maxLinearSpeed) {
			body.setLinearVelocity(velocity.scl(maxLinearSpeed / (float)Math.sqrt(currentSpeedSquare)));
		}

		// Cap the angular speed
		float maxAngVelocity = getMaxAngularSpeed();
		if (body.getAngularVelocity() > maxAngVelocity) {
			body.setAngularVelocity(maxAngVelocity);
		}
	}
}
 

protected void applySteering (SteeringAcceleration<Vector3> steering, float deltaTime) {
		boolean anyAccelerations = false;

		// Update position and linear velocity
		if (!steeringOutput.linear.isZero()) {
			// this method internally scales the force by deltaTime
			body.applyCentralForce(steeringOutput.linear);
			anyAccelerations = true;
		}

		// Update orientation and angular velocity
		if (isIndependentFacing()) {
			if (steeringOutput.angular != 0) {
				// this method internally scales the torque by deltaTime
				body.applyTorque(tmpVector3.set(0, steeringOutput.angular, 0));
				anyAccelerations = true;
			}
		}
		else {
			// If we haven't got any velocity, then we can do nothing.
			Vector3 linVel = getLinearVelocity();
			if (!linVel.isZero(getZeroLinearSpeedThreshold())) {
				// 
				// TODO: Commented out!!!
				// Looks like the code below creates troubles in combination with the applyCentralForce above
				// Maybe we should be more consistent by only applying forces or setting velocities.
				//
//				float newOrientation = vectorToAngle(linVel);
//				Vector3 angVel = body.getAngularVelocity();
//				angVel.y = (newOrientation - oldOrientation) % MathUtils.PI2;
//				if (angVel.y > MathUtils.PI) angVel.y -= MathUtils.PI2;
//				angVel.y /= deltaTime;
//				body.setAngularVelocity(angVel);
//				anyAccelerations = true;
//				oldOrientation = newOrientation;
			}
		}
		if (anyAccelerations) {
			body.activate();

			// TODO:
			// Looks like truncating speeds here after applying forces doesn't work as expected.
			// We should likely cap speeds form inside an InternalTickCallback, see
			// http://www.bulletphysics.org/mediawiki-1.5.8/index.php/Simulation_Tick_Callbacks

			// Cap the linear speed
			Vector3 velocity = body.getLinearVelocity();
			float currentSpeedSquare = velocity.len2();
			float maxLinearSpeed = getMaxLinearSpeed();
			if (currentSpeedSquare > maxLinearSpeed * maxLinearSpeed) {
				body.setLinearVelocity(velocity.scl(maxLinearSpeed / (float)Math.sqrt(currentSpeedSquare)));
			}

			// Cap the angular speed
			Vector3 angVelocity = body.getAngularVelocity();
			if (angVelocity.y > getMaxAngularSpeed()) {
				angVelocity.y = getMaxAngularSpeed();
				body.setAngularVelocity(angVelocity);
			}
		}
	}