public static void Lerp(ref Trajectory buffor, Trajectory first, Trajectory next, float factor)
{
for (int i = 0; i < first.Length; i++)
{
buffor.SetPoint(TrajectoryPoint.Lerp(first.GetPoint(i), next.GetPoint(i), factor), i);
}
}
public void SetPastPoints(ref Trajectory trajectory)
{
this.logicStates[currentStateIndex].GetCurrentAnimationTrajectory();
for (int i = 0; i < currentAnimationTrajectory.Length; i++)
{
if (this.logicStates[currentStateIndex].dataState.motionDataGroups[0].animationData[0].trajectoryPointsTimes[i] < 0)
{
TrajectoryPoint tp = currentAnimationTrajectory.GetPoint(i);
tp.TransformToWorldSpace(transform);
trajectory.SetPoint(tp, i);
}
else
{
return;
}
}
}
public static void CreateConstantTrajectoryWithCollision(
ref Trajectory trajectory_C,
ref Trajectory trajectory_NC,
List <float> pointsTimes,
float3 objectPosition,
float3 objectForward,
float3 strafeForward,
float3 desiredVel,
float maxSpeed,
bool strafe,
int firstIndexWithFutureTime,
float capsuleHeight,
float capsuleRadius,
LayerMask mask,
bool orientationFromCollisionTrajectory
)
{
float speedStep = maxSpeed / (trajectory_NC.Length - firstIndexWithFutureTime);
float3 velDir;
if (math.lengthsq(desiredVel) <= 0.0001f)
{
velDir = float3.zero;
}
else
{
velDir = math.normalize(desiredVel);
}
float stepTime;
for (int pointIndex = firstIndexWithFutureTime; pointIndex < trajectory_NC.Length; pointIndex++)
{
if (pointIndex == firstIndexWithFutureTime)
{
stepTime = pointsTimes[pointIndex];
}
else
{
stepTime = pointsTimes[pointIndex] - pointsTimes[pointIndex - 1];
}
float3 newPosition = velDir * speedStep * (float)(pointIndex - firstIndexWithFutureTime + 1) + objectPosition;
float3 newVelocity = desiredVel;
float3 newOrientation = CalculateFinalOrientation(
strafe,
strafeForward,
objectForward,
trajectory_NC.GetPoint(pointIndex).position,
pointIndex == firstIndexWithFutureTime ? objectPosition : trajectory_NC.GetPoint(pointIndex - 1).position
);
trajectory_NC.SetPoint(
newPosition,
newVelocity,
newOrientation,
pointIndex
);
float3 finalOrientation;
if (orientationFromCollisionTrajectory)
{
finalOrientation = CalculateFinalOrientation(
strafe,
strafeForward,
objectForward,
trajectory_C.GetPoint(pointIndex).position,
pointIndex == firstIndexWithFutureTime ? objectPosition : trajectory_C.GetPoint(pointIndex - 1).position
);
}
else
{
finalOrientation = CalculateFinalOrientation(
strafe,
strafeForward,
objectForward,
trajectory_NC.GetPoint(pointIndex).position,
pointIndex == firstIndexWithFutureTime ? objectPosition : trajectory_NC.GetPoint(pointIndex - 1).position
);
}
bool isColliding = false;
float3 colisionCheckDelta = pointIndex == firstIndexWithFutureTime?
trajectory_NC.GetPoint(pointIndex).position - objectPosition:
trajectory_NC.GetPoint(pointIndex).position - trajectory_NC.GetPoint(pointIndex - 1).position;
float3 colisionCheckStart = pointIndex == firstIndexWithFutureTime ? objectPosition : trajectory_C.GetPoint(pointIndex - 1).position; //trajectory_C.GetPoint(pointIndex - 1).position,
float3 startDesiredDeltaPos_C = CheckCollision(
colisionCheckDelta,
colisionCheckStart,
capsuleHeight,
capsuleRadius - 0.05f,
0.05f,
mask,
ref isColliding
);
float3 newPosition_C;
if (isColliding)
{
float3 finaldesiredDeltaPos_C = CheckCollision(
startDesiredDeltaPos_C,
colisionCheckStart,
capsuleHeight,
capsuleRadius - 0.05f,
0.05f,
mask,
ref isColliding
);
newPosition_C = colisionCheckStart + finaldesiredDeltaPos_C;
}
else
{
newPosition_C = colisionCheckStart + startDesiredDeltaPos_C;
}
float3 newVelocity_C = pointIndex != firstIndexWithFutureTime ?
(trajectory_C.GetPoint(pointIndex).position - trajectory_C.GetPoint(pointIndex - 1).position) / stepTime
: (trajectory_C.GetPoint(pointIndex).position - objectPosition) / stepTime;
trajectory_C.SetPoint(
newPosition_C,
newVelocity_C,
finalOrientation,
pointIndex
);
}
}
public static void CreateCollisionTrajectory(
ref Trajectory trajectory_C,
ref Trajectory trajectory_NC,
List <float> pointsTimes,
float3 objectPosition,
ref float3 objectPositionBuffor,
// Orientation
float3 objectForward,
float3 strafeForward,
// Trajectory working settings
float3 desiredVel,
float acceleration,
float bias,
float stiffnes,
float maxTimeToCalculateFactor,
float sharpTurnMultiplier,
bool strafe,
//
int firstIndexWithFutureTime,
//Collsions settings
float capsuleHeight,
float capsuleRadius,
LayerMask mask,
bool orientationFromCollisionTrajectory
)
{
float3 lastPositionDelta = objectPosition - objectPositionBuffor;
objectPositionBuffor = objectPosition;
float desSpeed = math.length(desiredVel);
float stepTime;
float3 currentDelta_NC;
float3 currentDelta_C;
float3 pointLastdelta_NC = float3.zero;
float3 pointLastdelta_C = float3.zero;
float3 castStart = float3.zero;
for (int pointIndex = firstIndexWithFutureTime; pointIndex < trajectory_NC.Length; pointIndex++)
{
trajectory_NC.SetPointPos(
trajectory_NC.GetPoint(pointIndex).position + lastPositionDelta,
pointIndex
);
trajectory_C.SetPointPos(
trajectory_C.GetPoint(pointIndex).position + lastPositionDelta,
pointIndex
);
if (pointIndex == firstIndexWithFutureTime)
{
stepTime = pointsTimes[pointIndex];
currentDelta_NC = trajectory_NC.GetPoint(pointIndex).position - objectPosition;
currentDelta_C = trajectory_C.GetPoint(pointIndex).position - objectPosition;
}
else
{
stepTime = pointsTimes[pointIndex] - pointsTimes[pointIndex - 1];
currentDelta_NC = trajectory_NC.GetPoint(pointIndex).position - trajectory_NC.GetPoint(pointIndex - 1).position + pointLastdelta_NC;
currentDelta_C = trajectory_C.GetPoint(pointIndex).position - trajectory_C.GetPoint(pointIndex - 1).position + pointLastdelta_C;
}
float3 desiredDeltaPosition_NC = desiredVel * stepTime;
float finalFactor = CalculateFinalFactor(
pointsTimes[pointIndex],
maxTimeToCalculateFactor,
bias,
acceleration,
stepTime,
desSpeed,
stiffnes,
sharpTurnMultiplier,
currentDelta_NC,
desiredDeltaPosition_NC
);
float3 finalDelta_NC = float3Extension.MoveFloat3WithSpeed(currentDelta_NC, desiredDeltaPosition_NC, finalFactor, Time.deltaTime);
float3 newPosition_NC = pointIndex != firstIndexWithFutureTime?
trajectory_NC.GetPoint(pointIndex - 1).position + finalDelta_NC
: objectPosition + finalDelta_NC;
pointLastdelta_NC = newPosition_NC - trajectory_NC.GetPoint(pointIndex).position;
#region Orientation calculation
float3 finalOrientation;
if (orientationFromCollisionTrajectory)
{
finalOrientation = CalculateFinalOrientation(
strafe,
strafeForward,
objectForward,
trajectory_C.GetPoint(pointIndex).position,
pointIndex == firstIndexWithFutureTime ? objectPosition : trajectory_C.GetPoint(pointIndex - 1).position
);
}
else
{
finalOrientation = CalculateFinalOrientation(
strafe,
strafeForward,
objectForward,
trajectory_NC.GetPoint(pointIndex).position,
pointIndex == firstIndexWithFutureTime ? objectPosition : trajectory_NC.GetPoint(pointIndex - 1).position
);
}
#endregion
trajectory_NC.SetPointPos(newPosition_NC, pointIndex);
bool isColliding = false;
float3 colisionCheckDelta = pointIndex == firstIndexWithFutureTime?
trajectory_NC.GetPoint(pointIndex).position - objectPosition:
trajectory_NC.GetPoint(pointIndex).position - trajectory_NC.GetPoint(pointIndex - 1).position;
float3 colisionCheckStart = pointIndex == firstIndexWithFutureTime ? objectPosition : castStart; //trajectory_C.GetPoint(pointIndex - 1).position,
float3 startDesiredDeltaPos_C = CheckCollision(
colisionCheckDelta,
colisionCheckStart,
capsuleHeight,
capsuleRadius - 0.05f,
0.05f,
mask,
ref isColliding
);
float3 newPosition_C;
if (isColliding)
{
float3 finaldesiredDeltaPos_C = CheckCollision(
startDesiredDeltaPos_C,
colisionCheckStart,
capsuleHeight,
capsuleRadius - 0.05f,
0.05f,
mask,
ref isColliding
);
castStart = pointIndex == firstIndexWithFutureTime ?
objectPosition + finaldesiredDeltaPos_C :
castStart + finaldesiredDeltaPos_C;
float3 finalDelta_C = float3Extension.MoveFloat3WithSpeed(currentDelta_C, finaldesiredDeltaPos_C, 2f * finalFactor, Time.deltaTime);
newPosition_C = pointIndex != firstIndexWithFutureTime?
trajectory_C.GetPoint(pointIndex - 1).position + finalDelta_C
: objectPosition + finalDelta_C;
}
else
{
float3 finalDelta_C = float3Extension.MoveFloat3WithSpeed(currentDelta_C, startDesiredDeltaPos_C, finalFactor, Time.deltaTime);
newPosition_C = pointIndex != firstIndexWithFutureTime?
trajectory_C.GetPoint(pointIndex - 1).position + finalDelta_C
: objectPosition + finalDelta_C;
castStart = newPosition_C;
}
pointLastdelta_C = newPosition_C - trajectory_C.GetPoint(pointIndex).position;
float3 newVelocity_C = pointIndex != firstIndexWithFutureTime ?
(trajectory_C.GetPoint(pointIndex).position - trajectory_C.GetPoint(pointIndex - 1).position) / stepTime
: (trajectory_C.GetPoint(pointIndex).position - objectPosition) / stepTime;
trajectory_C.SetPoint(newPosition_C, newVelocity_C, finalOrientation, pointIndex);
}
}