public void Execute(int idA)
{
float radiusSum = radius + radius;
float radiusSumSq = radiusSum * radiusSum;
float3 posA = positions[idA].Value;
int3 pos = GridHash.Quantize(posA, cellRadius);
int hash = GridHash.Hash(pos);
NativeMultiHashMapIterator <int> iterator;
int idB; // neighbourId
var found = hashMap.TryGetFirstValue(hash, out idB, out iterator);
float3 delta = new float3(0, 0, 0);
int cnstrsCount = 0;
while (found)
{
float3 posB = positions[idB].Value;
float3 dir = posA - posB;
float distanceSq = math.lengthSquared(dir);
if (idA == idB || distanceSq > radiusSumSq || distanceSq <= float.Epsilon)
{
found = hashMap.TryGetNextValue(out idB, ref iterator);
continue;
}
float distance = math.sqrt(distanceSq);
float massCorr = 0.5f;
float3 dP = (distance - radiusSum) * (dir / distance) * stiffness * massCorr;
delta -= dP;
cnstrsCount++;
found = hashMap.TryGetNextValue(out idB, ref iterator);
}
deltas[idA] = new DeltaPositions
{
//Delta = math.select(default(float3), delta / cnstrsCount, cnstrsCount > 0),
Delta = delta,
ConstraintsCount = cnstrsCount
};
}
public void Execute(int idA)
{
float3 predPosA = predPositions[idA].Value;
float massInvA = massesInv[idA].Value;
NativeArray <DistanceConstraintUni> cnstrsA = distanceConstraints[idA];
int cnstrCount = 0;
float3 delta = new float3(0, 0, 0);
for (int i = 0; i < 34; i++)
{
DistanceConstraintUni cnstr = cnstrsA[i];
if (cnstr.otherId == -1)
{
break;
}
float3 predPosB = predPositions[cnstr.otherId].Value;
float massInvB = massesInv[cnstr.otherId].Value;
float3 normal = predPosA - predPosB;
float length = math.length(normal.xyz);
float invMass = massInvA + massInvB;
if (invMass <= math.epsilon_normal || length <= math.epsilon_normal)
{
return;
}
float3 dP = (1.0f / invMass) * (length - cnstr.restLength) * (normal / length) * stiffness;
delta -= dP * massInvA;
cnstrCount++;
}
deltaPositions[idA] = new DeltaPositions {
Delta = delta / cnstrCount, ConstraintsCount = cnstrCount
};
}
public void Execute(int i)
{
deltaPositions[i] = new DeltaPositions {
Delta = new float3(0), ConstraintsCount = 0
};
}