public static void WeightedSum(float w1, float w2, float w3, ref ObiPathFrame c1, ref ObiPathFrame c2, ref ObiPathFrame c3, ref ObiPathFrame sum)
{
sum.position.x = c1.position.x * w1 + c2.position.x * w2 + c3.position.x * w3;
sum.position.y = c1.position.y * w1 + c2.position.y * w2 + c3.position.y * w3;
sum.position.z = c1.position.z * w1 + c2.position.z * w2 + c3.position.z * w3;
sum.tangent.x = c1.tangent.x * w1 + c2.tangent.x * w2 + c3.tangent.x * w3;
sum.tangent.y = c1.tangent.y * w1 + c2.tangent.y * w2 + c3.tangent.y * w3;
sum.tangent.z = c1.tangent.z * w1 + c2.tangent.z * w2 + c3.tangent.z * w3;
sum.normal.x = c1.normal.x * w1 + c2.normal.x * w2 + c3.normal.x * w3;
sum.normal.y = c1.normal.y * w1 + c2.normal.y * w2 + c3.normal.y * w3;
sum.normal.z = c1.normal.z * w1 + c2.normal.z * w2 + c3.normal.z * w3;
sum.binormal.x = c1.binormal.x * w1 + c2.binormal.x * w2 + c3.binormal.x * w3;
sum.binormal.y = c1.binormal.y * w1 + c2.binormal.y * w2 + c3.binormal.y * w3;
sum.binormal.z = c1.binormal.z * w1 + c2.binormal.z * w2 + c3.binormal.z * w3;
sum.color.x = c1.color.x * w1 + c2.color.x * w2 + c3.color.x * w3;
sum.color.y = c1.color.y * w1 + c2.color.y * w2 + c3.color.y * w3;
sum.color.z = c1.color.z * w1 + c2.color.z * w2 + c3.color.z * w3;
sum.color.w = c1.color.w * w1 + c2.color.w * w2 + c3.color.w * w3;
sum.thickness = c1.thickness * w1 + c2.thickness * w2 + c3.thickness * w3;
}
public void LateUpdate()
{
ObiPathFrame section = generator.GetSectionAt(m);
transform.position = generator.transform.TransformPoint(section.position);
transform.rotation = generator.transform.rotation * (Quaternion.LookRotation(section.tangent, section.binormal));
}
public ObiPathFrame GetSectionAt(float mu)
{
float edgeMu = smoothSections * Mathf.Clamp01(mu);
int index = (int)edgeMu;
float sectionMu = edgeMu - index;
int counter = 0;
int chunkIndex = -1;
int indexInChunk = -1;
for (int i = 0; i < smoothChunks.Count; ++i)
{
if (counter + smoothChunks[i].Count > index)
{
chunkIndex = i;
indexInChunk = index - counter;
}
counter += smoothChunks[i].Count;
}
ObiList <ObiPathFrame> chunk = smoothChunks[chunkIndex];
ObiPathFrame s1 = chunk[indexInChunk];
ObiPathFrame s2 = chunk[Mathf.Min(indexInChunk + 1, chunk.Count - 1)];
return((1 - sectionMu) * s1 + sectionMu * s2);
}
/**
* This method uses a variant of Chainkin's algorithm to produce a smooth curve from a set of control points. It is specially fast
* because it directly calculates subdivision level k, instead of recursively calculating levels 1..k.
*/
private void Chaikin(ObiList <ObiPathFrame> input, ObiList <ObiPathFrame> output, uint k)
{
// no subdivision levels, no work to do. just copy the input to the output:
if (k == 0 || input.Count < 3)
{
output.SetCount(input.Count);
for (int i = 0; i < input.Count; ++i)
{
output[i] = input[i];
}
return;
}
// calculate amount of new points generated by each inner control point:
int pCount = (int)Mathf.Pow(2, k);
// precalculate some quantities:
int n0 = input.Count - 1;
float twoRaisedToMinusKPlus1 = Mathf.Pow(2, -(k + 1));
float twoRaisedToMinusK = Mathf.Pow(2, -k);
float twoRaisedToMinus2K = Mathf.Pow(2, -2 * k);
float twoRaisedToMinus2KMinus1 = Mathf.Pow(2, -2 * k - 1);
// allocate ouput:
output.SetCount((n0 - 1) * pCount + 2);
// calculate initial curve points:
output[0] = (0.5f + twoRaisedToMinusKPlus1) * input[0] + (0.5f - twoRaisedToMinusKPlus1) * input[1];
output[pCount * n0 - pCount + 1] = (0.5f - twoRaisedToMinusKPlus1) * input[n0 - 1] + (0.5f + twoRaisedToMinusKPlus1) * input[n0];
// calculate internal points:
for (int j = 1; j <= pCount; ++j)
{
// precalculate coefficients:
float F = 0.5f - twoRaisedToMinusKPlus1 - (j - 1) * (twoRaisedToMinusK - j * twoRaisedToMinus2KMinus1);
float G = 0.5f + twoRaisedToMinusKPlus1 + (j - 1) * (twoRaisedToMinusK - j * twoRaisedToMinus2K);
float H = (j - 1) * j * twoRaisedToMinus2KMinus1;
for (int i = 1; i < n0; ++i)
{
ObiPathFrame.WeightedSum(F, G, H,
ref input.Data[i - 1],
ref input.Data[i],
ref input.Data[i + 1],
ref output.Data[(i - 1) * pCount + j]);
}
}
// make first and last curve points coincide with original points:
output[0] = input[0];
output[output.Count - 1] = input[input.Count - 1];
}
public void Transport(ObiPathFrame frame, float twist)
{
// Calculate delta rotation:
Quaternion rotQ = Quaternion.FromToRotation(tangent, frame.tangent);
Quaternion twistQ = Quaternion.AngleAxis(twist, frame.tangent);
Quaternion finalQ = twistQ * rotQ;
// Rotate previous frame axes to obtain the new ones:
normal = finalQ * normal;
binormal = finalQ * binormal;
tangent = frame.tangent;
position = frame.position;
thickness = frame.thickness;
color = frame.color;
}
private void PathFrameFromParticle(ObiRopeBase actor, ref ObiPathFrame frame, int particleIndex)
{
// Update current frame values from particles:
frame.position = w2l.MultiplyPoint3x4(actor.GetParticlePosition(particleIndex));
frame.thickness = actor.GetParticleMaxRadius(particleIndex);
frame.color = actor.GetParticleColor(particleIndex);
// Use particle orientation if possible:
if (actor.usesOrientedParticles)
{
Quaternion orientation = w2lRotation * Quaternion.SlerpUnclamped(actor.GetParticleOrientation(particleIndex), actor.GetParticleOrientation(Mathf.Max(0, particleIndex - 1)), 0.5f);
frame.normal = orientation * Vector3.up;
frame.binormal = orientation * Vector3.right;
frame.tangent = orientation * Vector3.forward;
}
}
// Update is called once per frame
void UpdatePlugs(ObiActor actor)
{
var rope = actor as ObiRopeBase;
// cache the rope's transform matrix/quaternion:
Matrix4x4 l2w = rope.transform.localToWorldMatrix;
Quaternion l2wRot = l2w.rotation;
int instanceIndex = 0;
// place prefabs at the start/end of each curve:
for (int c = 0; c < smoother.smoothChunks.Count; ++c)
{
ObiList <ObiPathFrame> curve = smoother.smoothChunks[c];
if ((plugTears && c > 0) ||
(plugStart && c == 0))
{
var instance = GetOrCreatePrefabInstance(instanceIndex++);
instance.SetActive(true);
ObiPathFrame frame = curve[0];
instance.transform.position = l2w.MultiplyPoint3x4(frame.position);
instance.transform.rotation = l2wRot * (Quaternion.LookRotation(-frame.tangent, frame.binormal));
instance.transform.localScale = instanceScale;
}
if ((plugTears && c < smoother.smoothChunks.Count - 1) ||
(plugEnd && c == smoother.smoothChunks.Count - 1))
{
var instance = GetOrCreatePrefabInstance(instanceIndex++);
instance.SetActive(true);
ObiPathFrame frame = curve[curve.Count - 1];
instance.transform.position = l2w.MultiplyPoint3x4(frame.position);
instance.transform.rotation = l2wRot * (Quaternion.LookRotation(frame.tangent, frame.binormal));
instance.transform.localScale = instanceScale;
}
}
// deactivate remaining instances:
for (int i = instanceIndex; i < instances.Count; ++i)
{
instances[i].SetActive(false);
}
}
/**
* Generates smooth curve chunks.
*/
public void GenerateSmoothChunks(ObiRopeBase actor, uint smoothingLevels)
{
using (m_GenerateSmoothChunksPerfMarker.Auto())
{
smoothChunks.Clear();
smoothSections = 0;
smoothLength = 0;
if (!Application.isPlaying)
{
actor.RebuildElementsFromConstraints();
}
AllocateRawChunks(actor);
w2l = actor.transform.worldToLocalMatrix;
w2lRotation = w2l.rotation;
// keep track of the first element of each chunk
int chunkStart = 0;
ObiPathFrame frame_0 = new ObiPathFrame(); // "next" frame
ObiPathFrame frame_1 = new ObiPathFrame(); // current frame
ObiPathFrame frame_2 = new ObiPathFrame(); // previous frame
// generate curve for each rope chunk:
for (int i = 0; i < rawChunks.Count; ++i)
{
int elementCount = rawChunks[i].Count - 1;
// Initialize frames:
frame_0.Reset();
frame_1.Reset();
frame_2.Reset();
PathFrameFromParticle(actor, ref frame_1, actor.elements[chunkStart].particle1, false);
frame_2 = frame_1;
for (int m = 1; m <= rawChunks[i].Count; ++m)
{
int index;
if (m >= elementCount)
{
// second particle of last element in the chunk.
index = actor.elements[chunkStart + elementCount - 1].particle2;
}
else
{
//first particle of current element.
index = actor.elements[chunkStart + m].particle1;
}
// generate curve frame from particle:
PathFrameFromParticle(actor, ref frame_0, index);
if (actor.usesOrientedParticles)
{
// copy frame directly.
frame_2 = frame_1;
}
else
{
// perform parallel transport, using forward / backward average to calculate tangent.
frame_1.tangent = ((frame_1.position - frame_2.position) + (frame_0.position - frame_1.position)).normalized;
frame_2.Transport(frame_1, twist);
}
// in case we wrapped around the rope, average first and last frames:
if (chunkStart + m > actor.activeParticleCount)
{
frame_2 = rawChunks[0][0] = 0.5f * frame_2 + 0.5f * rawChunks[0][0];
}
frame_1 = frame_0;
rawChunks[i][m - 1] = frame_2;
}
// increment chunkStart by the amount of elements in this chunk:
chunkStart += elementCount;
// adaptive curvature-based decimation:
if (Decimate(rawChunks[i], smoothChunks[i], decimation))
{
// if any decimation took place, swap raw and smooth chunks:
var aux = rawChunks[i];
rawChunks[i] = smoothChunks[i];
smoothChunks[i] = aux;
}
// get smooth curve points:
Chaikin(rawChunks[i], smoothChunks[i], smoothingLevels);
// count total curve sections and total curve length:
smoothSections += smoothChunks[i].Count - 1;
smoothLength += CalculateChunkLength(smoothChunks[i]);
}
}
}
protected virtual IEnumerator CreateStretchShearConstraints(List <Vector3> particleNormals)
{
stretchShearConstraintsData = new ObiStretchShearConstraintsData();
stretchShearConstraintsData.AddBatch(new ObiStretchShearConstraintsBatch());
stretchShearConstraintsData.AddBatch(new ObiStretchShearConstraintsBatch());
// rotation minimizing frame:
ObiPathFrame frame = new ObiPathFrame();
frame.Reset();
for (int i = 0; i < totalParticles - 1; i++)
{
var batch = stretchShearConstraintsData.batches[i % 2] as ObiStretchShearConstraintsBatch;
Vector2Int indices = new Vector2Int(i, i + 1);
Vector3 d = positions[indices.y] - positions[indices.x];
restLengths[i] = d.magnitude;
frame.Transport(positions[indices.x], d.normalized, 0);
orientations[i] = Quaternion.LookRotation(frame.tangent, particleNormals[indices.x]);
restOrientations[i] = orientations[i];
// Also set the orientation of the next particle. If it is not the last one, we will overwrite it.
// This makes sure that open rods provide an orientation for their last particle (or rather, a phantom segment past the last particle).
orientations[indices.y] = orientations[i];
restOrientations[indices.y] = orientations[i];
batch.AddConstraint(indices, indices.x, restLengths[i], Quaternion.identity);
batch.activeConstraintCount++;
if (i % 500 == 0)
{
yield return(new CoroutineJob.ProgressInfo("ObiRod: generating structural constraints...", i / (float)(totalParticles - 1)));
}
}
// if the path is closed, add the last, loop closing constraint to a new batch to avoid sharing particles.
if (path.Closed)
{
var loopClosingBatch = new ObiStretchShearConstraintsBatch();
stretchShearConstraintsData.AddBatch(loopClosingBatch);
Vector2Int indices = new Vector2Int(m_ActiveParticleCount - 1, 0);
Vector3 d = positions[indices.y] - positions[indices.x];
restLengths[m_ActiveParticleCount - 2] = d.magnitude;
frame.Transport(positions[indices.x], d.normalized, 0);
orientations[m_ActiveParticleCount - 1] = Quaternion.LookRotation(frame.tangent, particleNormals[indices.x]);
restOrientations[m_ActiveParticleCount - 1] = orientations[m_ActiveParticleCount - 1];
loopClosingBatch.AddConstraint(indices, indices.x, restLengths[m_ActiveParticleCount - 2], Quaternion.identity);
loopClosingBatch.activeConstraintCount++;
}
// Recalculate rest length:
m_RestLength = 0;
foreach (float length in restLengths)
{
m_RestLength += length;
}
}
