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