/** Quadratic Bezier curve derivative * @param out The {@link Vector} to set to the result. * @param t The location (ranging 0..1) on the curve. * @param p0 The first bezier point. * @param p1 The second bezier point. * @param p2 The third bezier point. * @param tmp A temporary vector to be used by the calculation. * @return The value specified by out for chaining */ public static <T extends Vector<T>> T quadratic_derivative (final T out, final float t, final T p0, final T p1, final T p2, final T tmp) { // B2'(t) = 2 * (1 - t) * (p1 - p0) + 2 * t * (p2 - p1) final float dt = 1f - t; return out.set(p1).sub(p0).scl(2).scl(1 - t).add(tmp.set(p2).sub(p1).scl(t).scl(2)); }
public CatmullRomSpline set (final T[] controlPoints, final boolean continuous) { if (tmp == null) tmp = controlPoints[0].cpy(); if (tmp2 == null) tmp2 = controlPoints[0].cpy(); if (tmp3 == null) tmp3 = controlPoints[0].cpy(); this.controlPoints = controlPoints; this.continuous = continuous; this.spanCount = continuous ? controlPoints.length : controlPoints.length - 3; return this; }
/** Simple linear interpolation derivative * @param out The {@link Vector} to set to the result. * @param t The location (ranging 0..1) on the line. * @param p0 The start point. * @param p1 The end point. * @param tmp A temporary vector to be used by the calculation. * @return The value specified by out for chaining */ public static <T extends Vector<T>> T linear_derivative (final T out, final float t, final T p0, final T p1, final T tmp) { // B1'(t) = p1-p0 return out.set(p1).sub(p0); }
/** Simple linear interpolation * @param out The {@link Vector} to set to the result. * @param t The location (ranging 0..1) on the line. * @param p0 The start point. * @param p1 The end point. * @param tmp A temporary vector to be used by the calculation. * @return The value specified by out for chaining */ public static <T extends Vector<T>> T linear (final T out, final float t, final T p0, final T p1, final T tmp) { // B1(t) = p0 + (p1-p0)*t return out.set(p0).scl(1f - t).add(tmp.set(p1).scl(t)); // Could just use lerp... }
@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 public boolean reportNeighbor (Steerable<T> neighbor) { // Calculate the time to collision relativePosition.set(neighbor.getPosition()).sub(owner.getPosition()); relativeVelocity.set(neighbor.getLinearVelocity()).sub(owner.getLinearVelocity()); float relativeSpeed2 = relativeVelocity.len2(); // Collision can't happen when the agents have the same linear velocity. // Also, note that timeToCollision would be NaN due to the indeterminate form 0/0 and, // since any comparison involving NaN returns false, it would become the shortestTime, // so defeating the algorithm. if (relativeSpeed2 == 0) return false; float timeToCollision = -relativePosition.dot(relativeVelocity) / relativeSpeed2; // If timeToCollision is negative, i.e. the owner is already moving away from the the neighbor, // or it's not the most imminent collision then no action needs to be taken. if (timeToCollision <= 0 || timeToCollision >= shortestTime) return false; // Check if it is going to be a collision at all float distance = relativePosition.len(); float minSeparation = distance - (float)Math.sqrt(relativeSpeed2) * timeToCollision /* shortestTime */; if (minSeparation > owner.getBoundingRadius() + neighbor.getBoundingRadius()) return false; // Store most imminent collision data shortestTime = timeToCollision; firstNeighbor = neighbor; firstMinSeparation = minSeparation; firstDistance = distance; firstRelativePosition.set(relativePosition); firstRelativeVelocity.set(relativeVelocity); return true; }
float distanceSquare = ownerPosition.dst2(outputCollision.point); if (distanceSquare < minDistanceSquare) { minDistanceSquare = distanceSquare; steering.linear.set(minOutputCollision.point) .mulAdd(minOutputCollision.normal, owner.getBoundingRadius() + distanceFromBoundary).sub(owner.getPosition()).nor() .scl(getActualLimiter().getMaxLinearAcceleration());
/** Returns the square distance of the nearest point on line segment {@code a-b}, from point {@code c}. Also, the {@code out} * vector is assigned to the nearest point. * @param out the output vector that contains the nearest point on return * @param a the start point of the line segment * @param b the end point of the line segment * @param c the point to calculate the distance from */ public float calculatePointSegmentSquareDistance (T out, T a, T b, T c) { out.set(a); tmpB.set(b); tmpC.set(c); T ab = tmpB.sub(a); float abLen2 = ab.len2(); if (abLen2 != 0) { float t = (tmpC.sub(a)).dot(ab) / abLen2; out.mulAdd(ab, MathUtils.clamp(t, 0, 1)); } return out.dst2(c); }
@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) { // 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 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 public boolean reportNeighbor (Steerable<T> neighbor) { toAgent.set(owner.getPosition()).sub(neighbor.getPosition()); float distanceSqr = toAgent.len2(); if(distanceSqr == 0) return true; float maxAcceleration = getActualLimiter().getMaxLinearAcceleration(); // Calculate the strength of repulsion through inverse square law decay float strength = getDecayCoefficient() / distanceSqr; if (strength > maxAcceleration) strength = maxAcceleration; // Add the acceleration // Optimized code for linear.mulAdd(toAgent.nor(), strength); linear.mulAdd(toAgent, strength / (float)Math.sqrt(distanceSqr)); return true; }
/** @return The span closest to the specified value, restricting to the specified spans. */ public int nearest (final T in, int start, final int count) { while (start < 0) start += spanCount; int result = start % spanCount; float dst = in.dst2(controlPoints[result]); for (int i = 1; i < count; i++) { final int idx = (start + i) % spanCount; final float d = in.dst2(controlPoints[idx]); if (d < dst) { dst = d; result = idx; } } return result; }
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); }
@Override protected SteeringAcceleration<T> calculateRealSteering (SteeringAcceleration<T> steering) { steering.setZero(); centerOfMass = steering.linear; int neighborCount = proximity.findNeighbors(this); if (neighborCount > 0) { // The center of mass is the average of the sum of positions centerOfMass.scl(1f / neighborCount); // Now seek towards that position. centerOfMass.sub(owner.getPosition()).nor().scl(getActualLimiter().getMaxLinearAcceleration()); } return steering; }
@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; }
/** Utility method that creates a new vector. * <p> * This method is used internally to instantiate vectors of the correct type parameter {@code T}. This technique keeps the API * simple and makes the API easier to use with the GWT backend because avoids the use of reflection. * * @param location the location whose position is used to create the new vector * @return the newly created vector */ protected T newVector (Location<T> location) { return location.getPosition().cpy().setZero(); } }
@Override public boolean reportNeighbor (Steerable<T> neighbor) { // Calculate the position of the hiding spot for this obstacle T hidingSpot = getHidingPosition(neighbor.getPosition(), neighbor.getBoundingRadius(), target.getPosition()); // Work in distance-squared space to find the closest hiding // spot to the owner float distance2 = hidingSpot.dst2(owner.getPosition()); if (distance2 < distance2ToClosest) { distance2ToClosest = distance2; bestHidingSpot.set(hidingSpot); return true; } return false; }