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); } }