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 static TrajectoryPoint LerpPiont(TrajectoryPoint first, TrajectoryPoint next, float factor)
{
return(new TrajectoryPoint(
math.lerp(first.position, next.position, factor),
math.lerp(first.velocity, next.velocity, factor),
math.lerp(first.orientation, next.orientation, factor)
));
}
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 TrajectoryPoint Lerp(TrajectoryPoint point1, TrajectoryPoint point2, float factor)
{
float3 dir = math.lerp(point1.orientation, point2.orientation, factor);
if (dir.x != 0f && dir.y != 0f && dir.z != 0f)
{
dir = math.normalize(dir);
}
else
{
dir = point1.orientation;
}
return(new TrajectoryPoint(
math.lerp(point1.position, point2.position, factor),
math.lerp(point1.velocity, point2.velocity, factor),
dir
));
}
public float3 CalculateVectorCost(TrajectoryPoint point, TrajectoryCostType type)
{
float3 cost = float3.zero;
switch (type)
{
//case TrajectoryCostType.Position:
// cost += CalculatePositionVectorCost(point);
// break;
//case TrajectoryCostType.Velocity:
// cost += CalculateVelocityVectorCost(point);
// break;
//case TrajectoryCostType.Orientation:
// cost += CalculateOrientationVectorCost(point);
// break;
//case TrajectoryCostType.PositionVelocity:
// cost += CalculatePositionVectorCost(point);
// cost += CalculateVelocityVectorCost(point);
// break;
//case TrajectoryCostType.PositionOrientation:
// cost += CalculatePositionVectorCost(point);
// cost += CalculateOrientationVectorCost(point);
// break;
//case TrajectoryCostType.VelocityOrientation:
// cost += CalculateVelocityVectorCost(point);
// cost += CalculateOrientationVectorCost(point);
// break;
case TrajectoryCostType.PositionVelocityOrientation:
cost += CalculatePositionVectorCost(point);
cost += CalculateVelocityVectorCost(point);
cost += CalculateOrientationVectorCost(point);
break;
case TrajectoryCostType.None:
break;
}
return(cost);
}
public float CalculateCost(TrajectoryPoint point, TrajectoryCostType type)
{
float cost = 0;
//switch (type)
//{
//case TrajectoryCostType.Position:
// cost += CalculatePositionCost(point);
// break;
//case TrajectoryCostType.Velocity:
// cost += CalculateVelocityCost(point);
// break;
//case TrajectoryCostType.Orientation:
// cost += CalculateOrientationCost(point);
// break;
//case TrajectoryCostType.PositionVelocity:
// cost += CalculatePositionCost(point);
// cost += CalculateVelocityCost(point);
// break;
//case TrajectoryCostType.PositionOrientation:
// cost += CalculatePositionCost(point);
// cost += CalculateOrientationCost(point);
// break;
//case TrajectoryCostType.VelocityOrientation:
// cost += CalculateVelocityCost(point);
// cost += CalculateOrientationCost(point);
// break;
//case TrajectoryCostType.PositionVelocityOrientation:
cost += CalculatePositionCost(point);
cost += CalculateVelocityCost(point);
cost += CalculateOrientationCost(point);
//break;
// case TrajectoryCostType.None:
//break;
//}
return(cost);
}
public float3 CalculateOrientationVectorCost(TrajectoryPoint point)
{
return(math.abs(point.orientation - orientation));
}
public float3 CalculateVelocityVectorCost(TrajectoryPoint point)
{
return(math.abs(point.velocity - velocity));
}
public float3 CalculatePositionVectorCost(TrajectoryPoint point)
{
return(math.abs(point.position - position));
}
public float CalculateOrientationCost(TrajectoryPoint point)
{
return(math.lengthsq(point.orientation - orientation));
}
public float CalculateVelocityCost(TrajectoryPoint point)
{
return(math.lengthsq(point.velocity - velocity));
}
public float CalculatePositionCost(TrajectoryPoint point)
{
return(math.lengthsq(point.position - position));
}
public void Set(TrajectoryPoint point)
{
this.position = point.position;
this.velocity = point.velocity;
this.orientation = point.orientation;
}
public TrajectoryPoint(TrajectoryPoint point)
{
position = new float3(point.position.x, point.position.y, point.position.z);
velocity = new float3(point.velocity.x, point.velocity.y, point.velocity.z);
orientation = new float3(point.orientation.x, point.orientation.y, point.orientation.z);
}
public void SetPoint(TrajectoryPoint point, int index)
{
points[index].Set(point);
}