public void Execute(int i)
{
float positionDelta = math.length(velocities[i]) * deltaTime;
SplineMath.SplineGraphFollowState followState = followStates[i];
Unity.Mathematics.Random random = randoms[i];
SplineMath.AdvanceTFromDelta(
ref followState,
ref random,
positionDelta,
splineGraph.vertices.data,
splineGraph.vertices.count,
splineGraph.edgePoolChildren.data,
splineGraph.edgePoolParents.data,
splineGraph.payload.edgeParentToChildSplines.data,
splineGraph.payload.edgeChildToParentSplines.data
);
followStates[i] = followState;
randoms[i] = random;
Int16 edgeIndex = followState.DecodeEdgeIndex();
SplineMath.Spline spline = (followState.DecodeIsReverse() == 0)
? splineGraph.payload.edgeParentToChildSplines.data[edgeIndex]
: splineGraph.payload.edgeChildToParentSplines.data[edgeIndex];
Int16 vertexIndexChild = splineGraph.edgePoolChildren.data[edgeIndex].vertexIndex;
Int16 vertexIndexParent = splineGraph.edgePoolParents.data[edgeIndex].vertexIndex;
if (followState.DecodeIsReverse() == 1)
{
Int16 vertexIndexTemp = vertexIndexChild;
vertexIndexChild = vertexIndexParent;
vertexIndexParent = vertexIndexTemp;
}
quaternion rotationParent = splineGraph.payload.rotations.data[vertexIndexParent];
quaternion rotationChild = splineGraph.payload.rotations.data[vertexIndexChild];
float3 positionPrevious = positions[i];
quaternion rotationPrevious = rotations[i];
float3 positionOnSpline = SplineMath.EvaluatePositionFromT(spline, followState.t);
positions[i] = positionOnSpline;
// rotations[i] = SplineMath.EvaluateRotationFromT(spline, followState.t);
// rotations[i] = math.slerp(rotationParent, rotationChild, followState.t);
rotations[i] = SplineMath.EvaluateRotationWithRollFromT(spline, rotationParent, rotationChild, followState.t);
// For now, simply evaluate the current leash value by lerping between the parent and child leash values, rather than using spline interpolation.
// This seems good enough for now (there is a bug in the spline interpolation code commented out below.)
float2 leashParent = splineGraph.payload.leashes.data[vertexIndexParent];
float2 leashChild = splineGraph.payload.leashes.data[vertexIndexChild];
float2 leashMaxOS = math.lerp(leashParent, leashChild, followState.t);
// SplineMath.Spline splineLeash = (followState.DecodeIsReverse() == 0)
// ? splineGraph.payload.edgeParentToChildSplinesLeashes.data[edgeIndex]
// : splineGraph.payload.edgeChildToParentSplinesLeashes.data[edgeIndex];
// float2 leashMaxOS = SplineMath.EvaluatePositionFromT(splineLeash, followState.t).xy;
float2 leashOS = leashMaxOS * leashes[i];
float3 leashWS = math.mul(rotations[i], new float3(leashOS, 0.0f));
positions[i] += leashWS;
if (avoidanceSoftBodySphereRadius > 1e-5f)
{
float3 avoidanceDirection = positions[i] - avoidanceSoftBodySphereOrigin;
float avoidanceLength = math.length(avoidanceDirection);
avoidanceDirection *= (avoidanceLength > 1e-5f) ? (1.0f / avoidanceLength) : 0.0f;
float avoidanceOffset = math.saturate((avoidanceLength / avoidanceSoftBodySphereRadius) * -0.5f + 1.0f) * avoidanceSoftBodySphereRadius;
if (avoidanceOffset > 0.0f)
{
float3 leashPlaneNormal = math.mul(rotations[i], new float3(0.0f, 0.0f, 1.0f));
float3 avoidanceDirectionLeashPlaneT = avoidanceDirection - leashPlaneNormal * math.dot(leashPlaneNormal, avoidanceDirection);
avoidanceDirectionLeashPlaneT = (math.lengthsq(avoidanceDirectionLeashPlaneT) > 1e-3f)
? avoidanceDirectionLeashPlaneT
: leashWS;
avoidanceDirectionLeashPlaneT = (math.lengthsq(avoidanceDirectionLeashPlaneT) > 1e-3f)
? avoidanceDirectionLeashPlaneT
: new float3(0.0f, 1.0f, 0.0f);
avoidanceDirectionLeashPlaneT = math.normalize(avoidanceDirectionLeashPlaneT);
positions[i] += avoidanceDirectionLeashPlaneT * avoidanceOffset;
}
}
positions[i] = math.lerp(positions[i], positionPrevious, dampeningPosition);
rotations[i] = math.slerp(rotations[i], rotationPrevious, dampeningRotation);
// {
// float3 acceleration = SplineMath.EvaluateAccelerationFromT(spline, followState.t);
// float3 directionRightWS = math.mul(rotations[i], new float3(1.0f, 0.0f, 0.0f));
// float accelerationRight = math.dot(directionRightWS, acceleration);
// float rollAngle = math.lerp(-0.25f * math.PI, 0.25f * math.PI, math.saturate((accelerationRight * rollFromAccelerationScale) * -0.5f + 0.5f));
// rotations[i] = math.mul(rotations[i], quaternion.AxisAngle(new float3(0.0f, 0.0f, 1.0f), rollAngle));
// }
}
// Debug only:
void Update()
{
Verify();
if (!isEditingEnabled)
{
return;
}
// Debug.Log("debugSpawnDeltaTime = " + debugSpawnDeltaTime);
debugSpawnDeltaTime += Time.deltaTime;
if (debugIsSpawnEnabled && (debugSpawnDeltaTime > 1.0f))
{
debugSpawnDeltaTime = 0;
int isReverse = debugIsReverse ? 1 : 0;
SplineMath.FindTFromClosestPointOnSplineGraph(
out float t,
out float d,
out Int16 edgeIndex,
isReverse,
debugPosition,
splineGraph.vertices.data,
splineGraph.vertices.count,
splineGraph.edgePoolChildren.data,
splineGraph.edgePoolParents.data,
splineGraph.payload.edgeParentToChildSplines.data,
splineGraph.payload.edgeChildToParentSplines.data
);
SplineMath.SplineGraphFollowState state = new SplineMath.SplineGraphFollowState(t, edgeIndex, 0, isReverse);
// Debug.Log("Spawning at:" + state.DebugString());
debugFollowStates.Add(state);
UInt32 seed = randomSeedGenerator.NextUInt();
while (seed == 0)
{
seed = randomSeedGenerator.NextUInt();
}
debugFollowRandoms.Add(new Unity.Mathematics.Random(seed));
}
for (int i = 0; i < debugFollowStates.Count;)
{
SplineMath.SplineGraphFollowState state = debugFollowStates[i];
Unity.Mathematics.Random random = debugFollowRandoms[i];
float delta = Time.deltaTime * debugVelocity;
SplineMath.AdvanceTFromDelta(
ref state,
ref random,
delta,
splineGraph.vertices.data,
splineGraph.vertices.count,
splineGraph.edgePoolChildren.data,
splineGraph.edgePoolParents.data,
splineGraph.payload.edgeParentToChildSplines.data,
splineGraph.payload.edgeChildToParentSplines.data
);
// Debug.Log("Updating at: " + state.DebugString());
if (state.DecodeIsComplete() == 1)
{
debugFollowStates.RemoveAt(i);
debugFollowRandoms.RemoveAt(i);
}
else
{
debugFollowStates[i] = state;
debugFollowRandoms[i] = random;
++i;
}
}
}