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 void Start()
{
// For geting current pose
stateInputTrajectoryLocalSpace = new Trajectory(logicStates[0].dataState.motionDataGroups[0].animationData[0][0].trajectory.Length);
currentAnimationTrajectory = new Trajectory(logicStates[0].dataState.motionDataGroups[0].animationData[0][0].trajectory.Length);
currentPose = new PoseData(logicStates[0].dataState.motionDataGroups[0].animationData[0][0].pose.Count);
bufforPose = new PoseData(logicStates[0].dataState.motionDataGroups[0].animationData[0][0].pose.Count);
nativePose = new NativeArray <BoneData>(logicStates[0].dataState.motionDataGroups[0].animationData[0][0].pose.Count, Allocator.Persistent);
nativeTrajectory = new NativeArray <TrajectoryPoint>(logicStates[0].dataState.motionDataGroups[0].animationData[0][0].trajectory.Length, Allocator.Persistent);
#if UNITY_EDITOR
//else
//{
// throw new System.Exception(string.Format("State {0} have no animation Data", logicStates[0].dataState.GetName()));
//}
#endif
logicStatesIndexes = new Dictionary <string, int>();
for (int i = 0; i < logicStates.Count; i++)
{
logicStatesIndexes.Add(logicStates[i].GetName(), i);
}
currentStateIndex = layer.startStateIndex;
stateWeightsSpeedChanging = new List <float>();
playableGraph.AddLayerPlayable(this);
currentBlendedStates.Clear();
currentBlendedStates.Add(currentStateIndex);
stateWeightsSpeedChanging.Add(1f);
logicStates[currentStateIndex].StateStart();
mixer.SetInputWeight(0, 1f);
currentWeights.Add(1f);
if (this.layer.avatarMask != null)
{
playableGraph.SetLayerAvatarMask((uint)layer.index, layer.avatarMask);
}
playableGraph.SetLayerAdditive((uint)layer.index, layer.isAdditive);
}
public bool SwitchState(
PoseData currentPose,
Trajectory goal,
int nextStateIndex,
float blendTime,
List <float2> whereCanFindingNextPose,
string startSectionName = null
)
{
#if UNITY_EDITOR
if (logicStates[nextStateIndex].dataState.GetStateType() == MotionMatchingStateType.ContactAnimationState)
{
throw new System.Exception(
string.Format("This method cannot switch to contact state. {0} state type is {1}",
logicStates[nextStateIndex].dataState.GetName(),
logicStates[nextStateIndex].dataState.GetStateType().ToString()
));
}
#endif
if (logicStates[nextStateIndex].IsBlockedToEnter())
{
return(false);
}
if (currentStateIndex >= 0 && currentStateIndex < logicStates.Count)
{
previousStateIndex = currentStateIndex;
stateWeightsSpeedChanging[stateWeightsSpeedChanging.Count - 1] = (-(1f / blendTime));
}
currentStateIndex = nextStateIndex;
stateWeightsSpeedChanging.Add((1f / blendTime));
currentBlendedStates.Add(currentStateIndex);
logicStates[currentStateIndex].StateEnter(
currentPose,
goal,
whereCanFindingNextPose,
startSectionName
);
return(true);
}
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);
}
}
public static void GetLerpedTrajectory(ref Trajectory buffor, FrameData first, FrameData second, float factor)
{
Trajectory.Lerp(ref buffor, first.trajectory, second.trajectory, factor);
}
public void SetTrajectory(Trajectory newTrajectory)
{
this.trajectory = newTrajectory;
}
public void SetGoal(Trajectory goal)
{
logicStates[currentStateIndex].SetTrajectory(goal);
}
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
}
}
