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 float CalculateCost(Trajectory toTrajectory, TrajectoryCostType type)
{
float cost = 0f;
for (int i = 0; i < Length; i++)
{
cost += points[i].CalculateCost(toTrajectory.GetPoint(i), type);
}
return(cost);
}
protected override void Enter(PoseData currentPose, Trajectory previouStateGoal, List <float2> whereCanFindingBestPose)
{
NativeArray <float2> findingIntervals = new NativeArray <float2>(whereCanFindingBestPose.ToArray(), Allocator.TempJob);
NativeArray <FrameContact> contactPointsNative = new NativeArray <FrameContact>(logicLayer.contactPoints.Count, Allocator.TempJob);
//making native contactsl
for (int i = 0; i < logicLayer.contactPoints.Count; i++)
{
contactPointsNative[i] = logicLayer.contactPoints[i].frameContact;
}
// Geting native pose
for (int i = 0; i < currentPose.Count; i++)
{
logicLayer.nativePose[i] = currentPose.GetBoneData(i);
}
//Geting natve trajectory
for (int i = 0; i < previouStateGoal.Length; i++)
{
logicLayer.nativeTrajectory[i] = previouStateGoal.GetPoint(i);
}
ImpactAnimationFinding(
logicLayer.nativePose,
findingIntervals,
contactPointsNative,
logicLayer.nativeTrajectory
);
findingIntervals.Dispose();
contactPointsNative.Dispose();
currentDataIndex = logicLayer.bestPoseInfo.clipIndex;
currentClipLocalTime = (float)logicLayer.bestPoseInfo.localTime;
playableGraph.CreateBlendMotionMatchingAnimation(
dataState.motionDataGroups[currentMotionDataGroupIndex].animationData[currentDataIndex],
currentDataIndex,
stateMixer,
currentClipLocalTime,
1f,
blendingSpeeds,
currentWeights,
animationsSequences,
this.logicLayer.GetPassIK(),
this.logicLayer.GetFootPassIK(),
1f
);
currentPlayedClipsIndexes.Add(currentDataIndex);
}
protected override void Enter(PoseData currentPose, Trajectory previouStateGoal, List <float2> whereCanFindingBestPose)
{
for (int i = 0; i < currentPose.Count; i++)
{
logicLayer.nativePose[i] = currentPose.GetBoneData(i);
}
for (int i = 0; i < previouStateGoal.Length; i++)
{
logicLayer.nativeTrajectory[i] = previouStateGoal.GetPoint(i);
}
if (this.dataState.sectionsDependencies != null)
{
SetCurrentSection(this.dataState.startSection);
}
SwitchMotionDataGroup();
GetCurrentClipsInfo();
MotionMatchingFinding(
logicLayer.nativePose,
logicLayer.nativeTrajectory
);
JobHandle.ScheduleBatchedJobs();
JobHandle.CompleteAll(jobsHandle);
JoinJobsOutput();
currentDataIndex = logicLayer.bestPoseInfo.clipIndex;
currentPlayedClipsIndexes.Add(currentDataIndex);
playableGraph.CreateBlendMotionMatchingAnimation(
dataState.motionDataGroups[currentMotionDataGroupIndex].animationData[currentDataIndex],
logicLayer.bestPoseInfo.clipIndex,
stateMixer,
logicLayer.bestPoseInfo.localTime,
this.dataState.mmFeatures.blendTime,
blendingSpeeds,
currentWeights,
animationsSequences,
this.logicLayer.GetPassIK(),
this.logicLayer.GetFootPassIK(),
1.0f,
this.dataState.mmFeatures.minWeightToAchive
);
//CreateMMAnimation(this.dataState.mmFeatures.blendTime, 1f);
}
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;
}
}
}
protected override void Enter(
PoseData currentPose,
Trajectory previouStateGoal,
List <float2> whereCanFindingBestPose
)
{
NativeArray <float2> findingIntervals = new NativeArray <float2>(whereCanFindingBestPose.ToArray(), Allocator.TempJob);
for (int i = 0; i < currentPose.Count; i++)
{
logicLayer.nativePose[i] = currentPose.GetBoneData(i);
}
for (int i = 0; i < previouStateGoal.Length; i++)
{
logicLayer.nativeTrajectory[i] = previouStateGoal.GetPoint(i);
}
SingleAnimationFinding(
logicLayer.nativePose,
logicLayer.nativeTrajectory,
findingIntervals
);
findingIntervals.Dispose();
currentDataIndex = logicLayer.bestPoseInfo.clipIndex;
currentClipLocalTime = (float)logicLayer.bestPoseInfo.localTime;
playableGraph.CreateBlendMotionMatchingAnimation(
dataState.motionDataGroups[currentMotionDataGroupIndex].animationData[currentDataIndex],
currentDataIndex,
stateMixer,
currentClipLocalTime,
dataState.saFeatures.blendTime,
blendingSpeeds,
currentWeights,
animationsSequences,
this.logicLayer.GetPassIK(),
this.logicLayer.GetFootPassIK(),
1f
);
currentPlayedClipsIndexes.Add(currentDataIndex);
}
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);
}
}
protected override void Enter(
PoseData currentPose,
Trajectory previouStateGoal,
List <float2> whereCanFindingBestPose
)
{
// valus initzialization
gettingAdaptedPoints = true;
targetContactPointIndex = 0;
NativeArray <float2> findingIntervals = new NativeArray <float2>(whereCanFindingBestPose.ToArray(), Allocator.TempJob);
NativeArray <FrameContact> contactPointsNative = new NativeArray <FrameContact>(logicLayer.contactPoints.Count, Allocator.TempJob);
//making native contactsl
for (int i = 0; i < logicLayer.contactPoints.Count; i++)
{
contactPointsNative[i] = logicLayer.contactPoints[i].frameContact;
}
// making native pose
for (int i = 0; i < currentPose.Count; i++)
{
logicLayer.nativePose[i] = currentPose.GetBoneData(i);
}
for (int i = 0; i < previouStateGoal.Length; i++)
{
logicLayer.nativeTrajectory[i] = previouStateGoal.GetPoint(i);
}
ContactAnimationFinding(
logicLayer.nativePose,
findingIntervals,
contactPointsNative,
logicLayer.nativeTrajectory
);
findingIntervals.Dispose();
contactPointsNative.Dispose();
currentDataIndex = logicLayer.bestPoseInfo.clipIndex;
currentClipLocalTime = (float)logicLayer.bestPoseInfo.localTime;
playableGraph.CreateBlendMotionMatchingAnimation(
dataState.motionDataGroups[currentMotionDataGroupIndex].animationData[currentDataIndex],
currentDataIndex,
stateMixer,
currentClipLocalTime,
1f,
blendingSpeeds,
currentWeights,
animationsSequences,
this.logicLayer.GetPassIK(),
this.logicLayer.GetFootPassIK(),
1f
);
currentPlayedClipsIndexes.Add(currentDataIndex);
if (dataState.csFeatures.contactStateType == ContactStateType.NormalContacts)
{
switch (this.contactType)
{
case ContactStateMovemetType.StartContact:
SC_Enter();
break;
case ContactStateMovemetType.ContactLand:
CL_Enter();
break;
case ContactStateMovemetType.StartContactLand:
SCL_Enter();
break;
case ContactStateMovemetType.StartLand:
SL_Enter();
break;
case ContactStateMovemetType.Contact:
CL_Enter();
break;
}
if (dataState.csFeatures.rotateToStart)
{
Vector3 dir = logicLayer.contactPoints[1].frameContact.position - logicLayer.contactPoints[0].frameContact.position;
dir = Vector3.ProjectOnPlane(dir, Vector3.up);
dir = this.GetCurrentMMData().fromFirstToSecondContactRot *dir;
rotationToContact = Quaternion.LookRotation(dir, Vector3.up);
float rotationTime = (this.GetCurrentMMData().GetContactStartTime(0) - currentClipLocalTime) / dataState.speedMultiplier;
if (rotationTime == 0)
{
rotationTime = 0.00001f;
}
else if (rotationTime < 0)
{
rotationTime = Mathf.Abs(rotationTime);
}
degreeSpeed = Quaternion.Angle(this.Transform.rotation, rotationToContact) / rotationTime;
}
#if UNITY_EDITOR
startTime = currentClipLocalTime;
previewPos = Transform.position;
#else
if (dataState.csFeatures.postionCorrection == ContactPointPositionCorrectionType.LerpPosition)
{
startTime = currentClipLocalTime;
previewPos = Transform.position;
}
#endif
}
}