private void EstimatePositions(float dt)
{
EstimatePositionsShader.SetFloat("dt", (float)dt);
for (int j = 0; j < Bodies.Count; j++)
{
Body3d body = Bodies[j];
if (GPUmode)
{
EstimatePositionsShader.SetInt("numParticles", body.NumParticles);
EPS_UV_UPpredictedComputeBuffer[j].SetData(body.Predicted);
EPS_UV_UPpositionsComputeBuffer[j].SetData(body.Positions);
AEFS_EPS_UVvelocityComputeBuffer[j].SetData(body.Velocities);
EstimatePositionsShader.SetBuffer(estimatePositionsShaderHandle, "Predicted", EPS_UV_UPpredictedComputeBuffer[j]);
EstimatePositionsShader.SetBuffer(estimatePositionsShaderHandle, "Positions", EPS_UV_UPpositionsComputeBuffer[j]);
EstimatePositionsShader.SetBuffer(estimatePositionsShaderHandle, "Velocities", AEFS_EPS_UVvelocityComputeBuffer[j]);
EstimatePositionsShader.Dispatch(estimatePositionsShaderHandle, (body.NumParticles / 32) + 1, 1, 1);
EPS_UV_UPpredictedComputeBuffer[j].GetData(body.Predicted);
}
else
{
for (int i = 0; i < body.NumParticles; i++)
{
body.Predicted[i] = body.Positions[i] + dt * body.Velocities[i];
}
}
}
}
public bool GPUmode = true;//true for GPU;
public void init()
{
//init applyExternalForcesShaderHandle
applyExternalForcesShaderHandle = ApplyExternalForcesShader.FindKernel("APPLYEXTERNALFORCE_1");
ApplyExternalForcesShader.SetFloat("gravity", -9.81f);
AEFS_EPS_UVvelocityComputeBuffer = new ComputeBuffer[Bodies.Count];
for (int j = 0; j < Bodies.Count; j++)
{
Body3d body = Bodies[j];
AEFS_EPS_UVvelocityComputeBuffer[j] = new ComputeBuffer(12, body.NumParticles);
}
estimatePositionsShaderHandle = EstimatePositionsShader.FindKernel("ESTIMATEPOSITIONS");
EPS_UV_UPpredictedComputeBuffer = new ComputeBuffer[Bodies.Count];
EPS_UV_UPpositionsComputeBuffer = new ComputeBuffer[Bodies.Count];
AEFS_EPS_UVvelocityComputeBuffer = new ComputeBuffer[Bodies.Count];
for (int j = 0; j < Bodies.Count; j++)
{
Body3d body = Bodies[j];
EPS_UV_UPpredictedComputeBuffer[j] = new ComputeBuffer(16, body.NumParticles);
EPS_UV_UPpositionsComputeBuffer[j] = new ComputeBuffer(16, body.NumParticles);
AEFS_EPS_UVvelocityComputeBuffer[j] = new ComputeBuffer(16, body.NumParticles);
}
updatePositionsShaderHandle = UpdatePositionsShader.FindKernel("UPDATEPOSITION");
updateVelocitiesShaderHandle = UpdateVelocitiesShader.FindKernel("UPDATEVELOCITIES");
}
//private void ConstrainVelocities()
//{
// for (int i = 0; i < Bodies.Count; i++)
// Bodies[i].ConstrainVelocities();
//}
private void UpdatePositions()
{
for (int j = 0; j < Bodies.Count; j++)
{
Body3d body = Bodies[j];
if (GPUmode)
{
UpdatePositionsShader.SetInt("numParticles", body.NumParticles);
EPS_UV_UPpredictedComputeBuffer[j].SetData(body.Predicted);
EPS_UV_UPpositionsComputeBuffer[j].SetData(body.Positions);
UpdatePositionsShader.SetBuffer(updatePositionsShaderHandle, "Predicted", EPS_UV_UPpredictedComputeBuffer[j]);
UpdatePositionsShader.SetBuffer(updatePositionsShaderHandle, "Positions", EPS_UV_UPpositionsComputeBuffer[j]);
UpdatePositionsShader.Dispatch(updatePositionsShaderHandle, (body.NumParticles / 32) + 1, 1, 1);
EPS_UV_UPpositionsComputeBuffer[j].GetData(body.Positions);
}
else
{
for (int i = 0; i < body.NumParticles; i++)
{
body.Positions[i] = body.Predicted[i];
}
}
}
}
internal FEMTetConstraint3d(Body3d body, int p0, int p1, int p2, int p3, double youngsModulus) : base(body)
{
P0 = p0;
P1 = p1;
P2 = p2;
P3 = p3;
YoungsModulus = youngsModulus;
PoissonRatio = 0.3;
Correction = new Vector3d[4];
Vector3d x0 = Body.Positions[P0];
Vector3d x1 = Body.Positions[P1];
Vector3d x2 = Body.Positions[P2];
Vector3d x3 = Body.Positions[P3];
RestVolume = Math.Abs((1.0f / 6.0) * Vector3d.Dot(x3 - x0, Vector3d.Cross(x2 - x0, x1 - x0)));
Matrix3x3d restMatrix = new Matrix3x3d();
restMatrix.SetColumn(0, x0 - x3);
restMatrix.SetColumn(1, x1 - x3);
restMatrix.SetColumn(2, x2 - x3);
InvRestMatrix = restMatrix.Inverse;
}
internal StrainTetConstraint3d(Body3d body, int p0, int p1, int p2, int p3, double stiffness) : base(body)
{
P0 = p0;
P1 = p1;
P2 = p2;
P3 = p3;
Stiffness = stiffness;
NormalizeStretch = false;
NormalizeShear = false;
Correction = new Vector3[4];
Vector3 x0 = body.Positions[P0];
Vector3 x1 = body.Positions[P1];
Vector3 x2 = body.Positions[P2];
Vector3 x3 = body.Positions[P3];
Matrix3x3d restMatrix = new Matrix3x3d();
restMatrix.SetColumn(0, MathConverter.ToVector3d(x1 - x0));
restMatrix.SetColumn(1, MathConverter.ToVector3d(x2 - x0));
restMatrix.SetColumn(2, MathConverter.ToVector3d(x3 - x0));
InvRestMatrix = restMatrix.Inverse;
}
private void CreateSpheres()
{
if (sphereMaterial == null)
{
return;
}
Spheres = new List <List <GameObject> >();
for (int j = 0; j < Solver.Bodies.Count; j++)
{
Body3d body = Solver.Bodies[j];
int numParticles = body.NumParticles;
float diam = (float)body.ParticleDiameter;
List <GameObject> spheres = new List <GameObject>(numParticles);
for (int i = 0; i < numParticles; i++)
{
Vector3 pos = body.Positions[i];
GameObject sphere = GameObject.CreatePrimitive(PrimitiveType.Sphere);
sphere.transform.parent = transform;
sphere.transform.position = pos;
sphere.transform.localScale = new Vector3(diam, diam, diam);
sphere.GetComponent <Collider>().enabled = false;
sphere.GetComponent <MeshRenderer>().material = sphereMaterial;
spheres.Add(sphere);
}
Spheres.Add(spheres);
}
}
private void AppyExternalForces(double dt)
{
ApplyExternalForcesShader.SetFloat("dt", (float)dt);
for (int j = 0; j < Bodies.Count; j++)
{
Body3d body = Bodies[j];
if (GPUmode)
{
ApplyExternalForcesShader.SetInt("numParticles", body.NumParticles);
ApplyExternalForcesShader.SetFloat("damping", (float)body.Dampning);
AEFS_EPS_UVvelocityComputeBuffer[j].SetData(body.Velocities);
ApplyExternalForcesShader.SetBuffer(applyExternalForcesShaderHandle, "Velocities", AEFS_EPS_UVvelocityComputeBuffer[j]);
ApplyExternalForcesShader.Dispatch(applyExternalForcesShaderHandle, (body.NumParticles / 32) + 1, 1, 1);
AEFS_EPS_UVvelocityComputeBuffer[j].GetData(body.Velocities);
}
else
{
for (int i = 0; i < body.NumParticles; i++)
{
body.Velocities[i] -= (body.Velocities[i] * (float)body.Dampning) * (float)dt;
}
for (int i = 0; i < Forces.Count; i++)
{
Forces[i].ApplyForce(dt, body);
}
}
}
}
public void AddBody(Body3d body)
{
if (Bodies.Contains(body))
{
return;
}
Bodies.Add(body);
}
internal BodyPlaneContact3d(Body3d body0, int i0, Vector3d normal, double dist)
{
Body0 = body0;
this.i0 = i0;
Normal = normal;
Distance = dist;
}
internal DistanceConstraint3d(Body3d body, int i0, int i1, double stiffness) : base(body)
{
this.i0 = i0;
this.i1 = i1;
CompressionStiffness = stiffness;
StretchStiffness = stiffness;
RestLength = (Body.Positions[i0] - Body.Positions[i1]).Magnitude;
}
public override void ApplyForce(double dt, Body3d body)
{
int len = body.NumParticles;
for (int i = 0; i < len; i++)
{
body.Velocities[i] += (float)dt * MathConverter.ToVector3(Gravity);
}
}
public override void ApplyForce(double dt, Body3d body)
{
int len = body.NumParticles;
for (int i = 0; i < len; i++)
{
body.Velocities[i] += dt * Gravity;
}
}
public override void ApplyForce(double dt, Body3d body)
{
string BufferName = "Velocities";
CurrentShader.SetBuffer(_kernel, BufferName, body.GPUVelocities);
CurrentShader.SetFloat("Gravity", (float)Gravity.y);
CurrentShader.SetFloat("dt", (float)dt);
CurrentShader.SetInt("MatterParticles", body.NumParticles);
CurrentShader.Dispatch(_kernel, ShaderHelper.GetNumberOfDispatchGroups(body.NumParticles, BLOCK_SIZE), 1, 1);
}
private void UpdatePositions()
{
for (int j = 0; j < Bodies.Count; j++)
{
Body3d body = Bodies[j];
for (int i = 0; i < body.NumParticles; i++)
{
body.Positions[i] = body.Predicted[i];
}
}
}
private void EstimatePositions(float dt)
{
for (int j = 0; j < Bodies.Count; j++)
{
Body3d body = Bodies[j];
for (int i = 0; i < body.NumParticles; i++)
{
body.Predicted[i] = body.Positions[i] + dt * body.Velocities[i];
}
}
}
internal BendingConstraint3d(Body3d body, int i0, int i1, int i2, double stiffness) : base(body)
{
this.i0 = i0;
this.i1 = i1;
this.i2 = i2;
Stiffness = stiffness;
Vector3d center = (Body.Positions[i0] + Body.Positions[i1] + Body.Positions[i2]) / 3.0;
RestLength = (Body.Positions[i2] - center).Magnitude;
}
private void DrawOutline(Camera camera)
{
Vector3 min = new Vector3(-GRID_SIZE, 0, -GRID_SIZE);
Vector3 max = new Vector3(GRID_SIZE, 0, GRID_SIZE);
DrawLines.DrawGrid(camera, Color.white, min, max, 1, transform.localToWorldMatrix);
for (int j = 0; j < Solver.Bodies.Count; j++)
{
Body3d body = Solver.Bodies[j];
//DrawLines.DrawBounds(camera, Color.green, body.Bounds, Matrix4x4d.Identity);
}
}
internal BodyBodyContact3d(Body3d body0, int i0, Body3d body1, int i1)
{
Body0 = body0;
this.i0 = i0;
Body1 = body1;
this.i1 = i1;
Diameter = Body0.ParticleRadius + Body1.ParticleRadius;
Diameter2 = Diameter * Diameter;
double sum = Body0.ParticleMass + Body1.ParticleMass;
Mass0 = Body0.ParticleMass / sum;
Mass1 = Body1.ParticleMass / sum;
}
internal IsometricBendingConstraint3d(Body3d body, int i0, int i1, int i2, int i3, float stiffness) : base(body)
{
this.i0 = i0;
this.i1 = i1;
this.i2 = i2;
this.i3 = i3;
BendStiffness = stiffness;
Vector3d p0 = body.Positions[i0];
Vector3d p1 = body.Positions[i1];
Vector3d p2 = body.Positions[i2];
Vector3d p3 = body.Positions[i3];
// Compute matrix Q for quadratic bending
Vector3d[] x = new Vector3d[] { p2, p3, p0, p1 };
Vector3d e0 = x[1] - x[0];
Vector3d e1 = x[2] - x[0];
Vector3d e2 = x[3] - x[0];
Vector3d e3 = x[2] - x[1];
Vector3d e4 = x[3] - x[1];
double c01 = CotTheta(e0, e1);
double c02 = CotTheta(e0, e2);
double c03 = CotTheta(e0 * -1.0, e3);
double c04 = CotTheta(e0 * -1.0, e4);
double A0 = 0.5 * e0.Cross(e1).Magnitude;
double A1 = 0.5 * e0.Cross(e2).Magnitude;
double coef = -3.0 / (2.0 * (A0 + A1));
double[] K = new double[] { c03 + c04, c01 + c02, -c01 - c03, -c02 - c04 };
double[] K2 = new double[] { coef *K[0], coef *K[1], coef *K[2], coef *K[3] };
Q = new Matrix4x4d();
for (int j = 0; j < 4; j++)
{
for (int k = 0; k < j; k++)
{
Q[j, k] = Q[k, j] = K[j] * K2[k];
}
Q[j, j] = K[j] * K2[j];
}
}
private void AppyExternalForces(float dt)
{
for (int j = 0; j < Bodies.Count; j++)
{
Body3d body = Bodies[j];
for (int i = 0; i < body.NumParticles; i++)
{
body.Velocities[i] -= (body.Velocities[i] * body.Dampning) * dt;
}
for (int i = 0; i < Forces.Count; i++)
{
Forces[i].ApplyForce(dt, body);
}
}
}
public void UpdateSpheres()
{
if (Spheres != null)
{
for (int j = 0; j < Solver.Bodies.Count; j++)
{
Body3d body = Solver.Bodies[j];
List <GameObject> spheres = Spheres[j];
for (int i = 0; i < spheres.Count; i++)
{
Vector3 pos = body.Positions[i];
spheres[i].transform.position = new Vector3((float)pos.x, (float)pos.y, (float)pos.z);
}
}
}
}
private void UpdateVelocities(double dt)
{
double invDt = 1.0 / dt;
double threshold2 = SleepThreshold * dt;
threshold2 *= threshold2;
UpdateVelocitiesShader.SetFloat("invDt", (float)invDt);
UpdateVelocitiesShader.SetFloat("threshold2", (float)threshold2);
for (int j = 0; j < Bodies.Count; j++)
{
Body3d body = Bodies[j];
if (GPUmode)
{
UpdateVelocitiesShader.SetInt("numParticles", body.NumParticles);
EPS_UV_UPpredictedComputeBuffer[j].SetData(body.Predicted);
EPS_UV_UPpositionsComputeBuffer[j].SetData(body.Positions);
AEFS_EPS_UVvelocityComputeBuffer[j].SetData(body.Velocities);
UpdateVelocitiesShader.SetBuffer(updateVelocitiesShaderHandle, "Predicted", EPS_UV_UPpredictedComputeBuffer[j]);
UpdateVelocitiesShader.SetBuffer(updateVelocitiesShaderHandle, "Positions", EPS_UV_UPpositionsComputeBuffer[j]);
UpdateVelocitiesShader.SetBuffer(updateVelocitiesShaderHandle, "Velocities", AEFS_EPS_UVvelocityComputeBuffer[j]);
UpdateVelocitiesShader.Dispatch(updateVelocitiesShaderHandle, (body.NumParticles / 32) + 1, 1, 1);
AEFS_EPS_UVvelocityComputeBuffer[j].GetData(body.Velocities);
}
else
{
for (int i = 0; i < body.NumParticles; i++)
{
Vector3 d = body.Predicted[i] - body.Positions[i];
body.Velocities[i] = d * (float)invDt;
double m = body.Velocities[i].sqrMagnitude;
if (m < threshold2)
{
body.Velocities[i] = Vector3.zero;
}
}
}
}
}
internal override void FindContacts(IList <Body3d> bodies, List <CollisionContact3d> contacts)
{
for (int j = 0; j < bodies.Count; j++)
{
Body3d body = bodies[j];
int numParticles = body.NumParticles;
double radius = body.ParticleRadius;
for (int i = 0; i < numParticles; i++)
{
double d = Vector3.Dot(Normal, body.Predicted[i]) + Distance - radius;
if (d < 0.0)
{
contacts.Add(new BodyPlaneContact3d(body, i, Normal, Distance));
}
}
}
}
internal ShapeMatchingConstraint3d(Body3d body, double mass, double stiffness) : base(body)
{
Stiffness = stiffness;
InvRestMatrix = Matrix3x3d.Identity;
RestCm = Vector3.zero;
double wsum = 0.0;
int numParticles = Body.NumParticles;
for (int i = 0; i < numParticles; i++)
{
RestCm += Body.Positions[i] * (float)mass;
wsum += mass;
}
RestCm /= (float)wsum;
Matrix3x3d A = new Matrix3x3d();
RestPositions = new Vector3[numParticles];
for (int i = 0; i < numParticles; i++)
{
Vector3 q = Body.Positions[i] - RestCm;
A[0, 0] += mass * q.x * q.x;
A[0, 1] += mass * q.x * q.y;
A[0, 2] += mass * q.x * q.z;
A[1, 0] += mass * q.y * q.x;
A[1, 1] += mass * q.y * q.y;
A[1, 2] += mass * q.y * q.z;
A[2, 0] += mass * q.z * q.x;
A[2, 1] += mass * q.z * q.y;
A[2, 2] += mass * q.z * q.z;
RestPositions[i] = q;
}
InvRestMatrix = A.Inverse;
}
internal DihedralConstraint3d(Body3d body, int i0, int i1, int i2, int i3, float stiffness) : base(body)
{
this.i0 = i0;
this.i1 = i1;
this.i2 = i2;
this.i3 = i3;
BendStiffness = stiffness;
Vector3 p0 = body.Positions[i0];
Vector3 p1 = body.Positions[i1];
Vector3 p2 = body.Positions[i2];
Vector3 p3 = body.Positions[i3];
Vector3 n1 = Vector3.Cross((p2 - p0), (p3 - p0));
n1 /= n1.sqrMagnitude;
Vector3 n2 = Vector3.Cross((p3 - p1), (p2 - p1));
n2 /= n2.sqrMagnitude;
n1.Normalize();
n2.Normalize();
float dot = Vector3.Dot(n1, n2);
if (dot < -1.0)
{
dot = -1.0f;
}
if (dot > 1.0)
{
dot = 1.0f;
}
RestAngle = Math.Acos(dot);
}
private void UpdateVelocities(float dt)
{
float invDt = 1.0f / dt;
float threshold2 = SleepThreshold * dt;
threshold2 *= threshold2;
for (int j = 0; j < Bodies.Count; j++)
{
Body3d body = Bodies[j];
for (int i = 0; i < body.NumParticles; i++)
{
Vector3f d = body.Predicted[i] - body.Positions[i];
body.Velocities[i] = d * invDt;
double m = body.Velocities[i].SqrMagnitude;
if (m < threshold2)
{
body.Velocities[i] = Vector3f.Zero;
}
}
}
}
internal Constraint3d(Body3d body)
{
Body = body;
}
internal StaticConstraint3d(Body3d body, int i) : base(body)
{
i0 = i;
Position = body.Positions[i];
}
public abstract void ApplyForce(double dt, Body3d body);