public void Execute() { for (int idA = 0; idA < length; idA++) { float3 predPosA = predPositions[idA].Value; float massInvA = massesInv[idA].Value; NativeArray <DistanceConstraintUni> cnstrsA = distanceConstraints[idA]; float3 delta = new float3(0, 0, 0); int cnstrCount = 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) { continue; } float3 dP = (1.0f / invMass) * (length - cnstr.restLength) * (normal / length) * stiffness; delta -= dP * massInvA; cnstrCount++; } predPositions[idA] = new PredictedPositions { Value = predPosA + delta / cnstrCount }; } }
private void AddSoftbody(DefKit.TetMesh tetMesh) { int[] constraintsCounter = new int[tetMesh.pointsCount]; for (int i = 0; i < tetMesh.edgesCount; i++) { //bilateral constraints Entity distanceConstraint = EntityManager.CreateEntity(typeof(DistanceConstraint)); EntityManager.SetComponentData(distanceConstraint, new DistanceConstraint { idA = tetMesh.edges[i].idA, idB = tetMesh.edges[i].idB, restLength = tetMesh.edges[i].restLength }); constraintsCounter[tetMesh.edges[i].idA]++; constraintsCounter[tetMesh.edges[i].idB]++; } int maxCount = 0; for (int i = 0; i < tetMesh.pointsCount; i++) { maxCount = math.max(maxCount, constraintsCounter[i]); constraintsCounter[i] = 0; } NativeArray <Entity> particles = new NativeArray <Entity>(tetMesh.pointsCount, Allocator.Temp); EntityManager.CreateEntity(ParticleArchetype, particles); for (int i = 0; i < tetMesh.pointsCount; i++) { EntityManager.SetComponentData(particles[i], new Position { Value = tetMesh.nodesPositions[i] }); EntityManager.SetComponentData(particles[i], new MassInv { Value = 1 }); EntityManager.AddComponent(particles[i], ComponentType.FixedArray(typeof(DistanceConstraintUni), maxCount)); EntityManager.AddSharedComponentData(particles[i], ParticleLook); //fill the arrays with -1s (end of entry marker) NativeArray <DistanceConstraintUni> cnstrsArr = EntityManager.GetFixedArray <DistanceConstraintUni>(particles[i]); for (int c = 0; c < maxCount; c++) { cnstrsArr[c] = new DistanceConstraintUni { otherId = -1, restLength = 0 } } ; } //unilateral constraints for (int i = 0; i < tetMesh.edgesCount; i++) { int idA = tetMesh.edges[i].idA; int idB = tetMesh.edges[i].idB; float restLength = tetMesh.edges[i].restLength; NativeArray <DistanceConstraintUni> cnstrsArrA = EntityManager.GetFixedArray <DistanceConstraintUni>(particles[idA]); NativeArray <DistanceConstraintUni> cnstrsArrB = EntityManager.GetFixedArray <DistanceConstraintUni>(particles[idB]); cnstrsArrA[constraintsCounter[idA]] = new DistanceConstraintUni { otherId = idB, restLength = restLength }; cnstrsArrB[constraintsCounter[idB]] = new DistanceConstraintUni { otherId = idA, restLength = restLength }; constraintsCounter[idA]++; constraintsCounter[idB]++; } particles.Dispose(); }