//Check self collisions point-triangle
void CheckSelfCollitionsPoint(float dt, float frictionConstCloth, float dissipationConstCloth)
{
//Here I use the same metodology than before (edge-triangle collisions)
hash = new Hashing(gridSize, 1.0f / (float)gridSize, 1723);
SPHash[] spHash = new SPHash[1723];
for (int v = 0; v < particles.Count; v++)
{
int has = Mathf.Abs(hash.Hash(particles[v].Position));
if (spHash[has].indices == null)
{
spHash[has].indices = new List <int>();
}
spHash[has].indices.Add(particles[v].I);
}
for (int t = 0; t < triangles.Count; t++)
{
var tri = triangles[t];
var p0 = particles[tri.indexTriA].Position;
var p1 = particles[tri.indexTriB].Position;
var p2 = particles[tri.indexTriC].Position;
List <int> hashes = hash.TriangleBoundingBoxHashes(p0, p1, p2);
for (int h = 0; h < hashes.Count; h++)
{
if (spHash[h].indices != null)
{
for (int sph = 0; sph < spHash[h].indices.Count; sph++)
{
int idx = spHash[h].indices[sph];
//Here check that the point don't takes part of the triangle
if (idx != particles[tri.indexTriA].I && idx != particles[tri.indexTriB].I && idx != particles[tri.indexTriC].I)
{
Vector3 p = particles[idx].Position;
Vector3 corr, corr0, corr1, corr2, normalTri;
float val0, val1, val2;
if (TrianglePointDistance(
p,
p0,
p1,
p2,
0.02f, 500f, 0.0f,
out corr, out corr0, out corr1, out corr2, out normalTri,
out val0, out val1, out val2))
{
//Here same metodology than plane check collisions
//but here is posible to have the particle on the
//negative triangle normal plane. If the particle
//has to be on the negative side correct the position
//and the other way rround. Also correction velocity
//to simulate inelastic collitions.
Vector3 d0 = particles[idx].Position - corr;
float dot0 = Vector3.Dot(normalTri, d0);
if (dot0 < 0f)
{
particles[idx].Position = particles[idx].Prev - dot0 * normalTri;
particles[tri.indexTriA].Position = particles[tri.indexTriA].Prev + dot0 * normalTri * val0;
particles[tri.indexTriB].Position = particles[tri.indexTriB].Prev + dot0 * normalTri * val1;
particles[tri.indexTriC].Position = particles[tri.indexTriC].Prev + dot0 * normalTri * val2;
}
if (dot0 > 0f)
{
particles[idx].Position = particles[idx].Prev + dot0 * normalTri;
particles[tri.indexTriA].Position = particles[tri.indexTriA].Prev - dot0 * normalTri * val0;
particles[tri.indexTriB].Position = particles[tri.indexTriB].Prev - dot0 * normalTri * val1;
particles[tri.indexTriC].Position = particles[tri.indexTriC].Prev - dot0 * normalTri * val2;
}
//Velocity
Vector3 normalVelocity0 = Vector3.Dot(normalTri, particles[idx].Velocity) * normalTri;
Vector3 normalVelocity1 = Vector3.Dot(normalTri, particles[tri.indexTriA].Velocity) * normalTri;
Vector3 normalVelocity2 = Vector3.Dot(normalTri, particles[tri.indexTriB].Velocity) * normalTri;
Vector3 normalVelocity3 = Vector3.Dot(normalTri, particles[tri.indexTriC].Velocity) * normalTri;
Vector3 tangencialVelocity0 = particles[idx].Velocity - normalVelocity0;
Vector3 tangencialVelocity1 = particles[tri.indexTriA].Velocity - normalVelocity0;
Vector3 tangencialVelocity2 = particles[tri.indexTriB].Velocity - normalVelocity0;
Vector3 tangencialVelocity3 = particles[tri.indexTriC].Velocity - normalVelocity0;
Vector3 velocity0 = -normalVelocity0;
Vector3 velocity1 = -normalVelocity1;
Vector3 velocity2 = -normalVelocity2;
Vector3 velocity3 = -normalVelocity3;
Vector3 velocityBest0 = tangencialVelocity0 - frictionConstCloth * normalVelocity0.magnitude * (tangencialVelocity0 / tangencialVelocity0.magnitude) - dissipationConstCloth * normalVelocity0;
Vector3 velocityBest1 = tangencialVelocity1 - frictionConstCloth * normalVelocity1.magnitude * (tangencialVelocity1 / tangencialVelocity1.magnitude) - dissipationConstCloth * normalVelocity1;
Vector3 velocityBest2 = tangencialVelocity2 - frictionConstCloth * normalVelocity2.magnitude * (tangencialVelocity2 / tangencialVelocity2.magnitude) - dissipationConstCloth * normalVelocity2;
Vector3 velocityBest3 = tangencialVelocity3 - frictionConstCloth * normalVelocity3.magnitude * (tangencialVelocity3 / tangencialVelocity3.magnitude) - dissipationConstCloth * normalVelocity3;
//Velocity
/*if(dot0 < 0f)
* {
* particles[idx].Position = particles[idx].Position - dt * velocityBest0;
* particles[tri.indexTriA].Position = particles[tri.indexTriA].Position + dt * velocityBest1 * val0;
* particles[tri.indexTriB].Position = particles[tri.indexTriB].Position + dt * velocityBest2 * val1;
* particles[tri.indexTriC].Position = particles[tri.indexTriC].Position + dt * velocityBest3 * val2;
* }
*
* if(dot0 > 0f)
* {
* particles[idx].Position = particles[idx].Position + dt * velocityBest0;
* particles[tri.indexTriA].Position = particles[tri.indexTriA].Position - dt * velocityBest1 * val0;
* particles[tri.indexTriB].Position = particles[tri.indexTriB].Position - dt * velocityBest2 * val1;
* particles[tri.indexTriC].Position = particles[tri.indexTriC].Position - dt * velocityBest3 * val2;
* }*/
}
}
}
}
}
}
}
/*public Update(float dt, List<Triangles> triangles_p, List<Edges> edges_p, List<Particles> particles_p, List<Triangles> triangles_c, List<Edges> edges_c, List<Particles> particles_c)
* {
*
* }*/
//Check self collisions edge-triangle
void CheckSelfCollitionsEdge(float dt, float frictionConstCloth, float dissipationConstCloth)
{
//First I create the hesh table.
hash = new Hashing(gridSize, 1.0f / (float)gridSize, 1723);
SPHash[] spHash = new SPHash[1723];
//Second I put the edges on a minimun boxes and
//I put it into the hash table
for (int e = 0; e < edges.Count; e++)
{
var ed = edges[e];
var p0 = particles[ed.indexEdgeA].Position;
var p1 = particles[ed.indexEdgeB].Position;
int has = hash.LineBoxHashes(p0, p1);
if (spHash[has].indices == null)
{
spHash[has].indices = new List <int>();
}
spHash[has].indices.Add(ed.indexEdge);
}
//Now I put every triangle in a hash table (well, I cehck
//the hash number of it) and if in this place on the edge
//table there are any edge I check if there is a collition
for (int t = 0; t < triangles.Count; t++)
{
var tri = triangles[t];
var p0 = particles[tri.indexTriA].Position;
var p1 = particles[tri.indexTriB].Position;
var p2 = particles[tri.indexTriC].Position;
float w0 = 1f / particles[tri.indexTriA].Mass;
float w1 = 1f / particles[tri.indexTriB].Mass;
float w2 = 1f / particles[tri.indexTriC].Mass;
List <int> hashes = hash.TriangleBoundingBoxHashes(p0, p1, p2);
for (int h = 0; h < hashes.Count; h++)
{
if (spHash[h].indices != null)
{
for (int sph = 0; sph < spHash[h].indices.Count; sph++)
{
int idx = spHash[h].indices[sph];
var a0 = particles[edges[idx].indexEdgeA].I;
var a1 = particles[edges[idx].indexEdgeB].I;
var b0 = particles[tri.indexTriA].I;
var b1 = particles[tri.indexTriB].I;
var b2 = particles[tri.indexTriC].I;
//Here to check that is not the same edge
if (a0 != b0 && a0 != b1 && a0 != b2 && a1 != b0 && a1 != b1 && a1 != b2)
{
var ed = edges[idx];
var u0 = particles[ed.indexEdgeA].Position;
var u1 = particles[ed.indexEdgeB].Position;
Vector3 out0, out1, out2, out3, normalTri;
Vector3 corrP1, corrP2, corrT1, corrT2, corrT3;
int test;
int situation;
float valP, val1, val2, val3;
//Finally check if there is a collision
if (EdgeTriangleIntersect(u0, u1, p0, p1, p2, 0.2f, 50f, 0f, out corrP1, out corrP2, out corrT1, out corrT2, out corrT3, out normalTri, out situation,
out valP, out val1, out val2, out val3))
{
//Now I know that there is a collision so I have to
//make a correction to avoid that collision.
//The technique is the same than in the plane case
//but here the plane is the triangle and its normal.
//However here is posible for a particle to stay on the
//negative part of the trignel normal. So in edge case,
//if there is a collision I check where the collision
//have just happened if is on the 2 extrems (the points)
//just no correction however if the edge is cut I check
//where have happened and I where the the short distance
//penetrated is I correct that point because it have to
//be the last that have just entered.
//Then I do a projection depending on the side of the plane
//the dot product will be positive or negative.
//After that I modify the velocity to be able to have
//inelastic collisions.
//Position
Vector3 dA = particles[ed.indexEdgeA].Position - (corrT1 + 0.02f * normalTri);
Vector3 dB = particles[ed.indexEdgeB].Position - (corrT1 + 0.02f * normalTri);
float dotA = Vector3.Dot(normalTri, dA);
float dotB = Vector3.Dot(normalTri, dB);
if (situation == 3)
{
if (dotA < 0f)
{
particles[ed.indexEdgeA].Position = particles[ed.indexEdgeA].Prev - normalTri * dotA;
particles[tri.indexTriA].Position = particles[tri.indexTriA].Prev + normalTri * dotA * val1;
particles[tri.indexTriB].Position = particles[tri.indexTriB].Prev + normalTri * dotA * val2;
particles[tri.indexTriC].Position = particles[tri.indexTriC].Prev + normalTri * dotA * val3;
}
if (dotA > 0f)
{
particles[ed.indexEdgeA].Position = particles[ed.indexEdgeA].Prev + normalTri * dotA;
particles[tri.indexTriA].Position = particles[tri.indexTriA].Prev - normalTri * dotA * val1;
particles[tri.indexTriB].Position = particles[tri.indexTriB].Prev - normalTri * dotA * val2;
particles[tri.indexTriC].Position = particles[tri.indexTriC].Prev - normalTri * dotA * val3;
}
}
if (situation == 4)
{
if (dotB < 0f)
{
particles[ed.indexEdgeB].Position = particles[ed.indexEdgeB].Prev - normalTri * dotB;
particles[tri.indexTriA].Position = particles[tri.indexTriA].Prev + normalTri * dotB * val1;
particles[tri.indexTriB].Position = particles[tri.indexTriB].Prev + normalTri * dotB * val2;
particles[tri.indexTriC].Position = particles[tri.indexTriC].Prev + normalTri * dotB * val3;
}
if (dotB > 0f)
{
particles[ed.indexEdgeB].Position = particles[ed.indexEdgeB].Prev + normalTri * dotB;
particles[tri.indexTriA].Position = particles[tri.indexTriA].Prev - normalTri * dotB * val1;
particles[tri.indexTriB].Position = particles[tri.indexTriB].Prev - normalTri * dotB * val2;
particles[tri.indexTriC].Position = particles[tri.indexTriC].Prev - normalTri * dotB * val3;
}
}
//Velocity
Vector3 normalVelocityA = Vector3.Dot(normalTri, particles[ed.indexEdgeA].Velocity) * normalTri;
Vector3 normalVelocityB = Vector3.Dot(normalTri, particles[ed.indexEdgeB].Velocity) * normalTri;
Vector3 normalVelocity1 = Vector3.Dot(normalTri, particles[tri.indexTriA].Velocity) * normalTri;
Vector3 normalVelocity2 = Vector3.Dot(normalTri, particles[tri.indexTriB].Velocity) * normalTri;
Vector3 normalVelocity3 = Vector3.Dot(normalTri, particles[tri.indexTriC].Velocity) * normalTri;
Vector3 tangencialVelocityA = particles[ed.indexEdgeA].Velocity - normalVelocityA;
Vector3 tangencialVelocityB = particles[ed.indexEdgeB].Velocity - normalVelocityB;
Vector3 tangencialVelocity1 = particles[tri.indexTriA].Velocity - normalVelocity1;
Vector3 tangencialVelocity2 = particles[tri.indexTriB].Velocity - normalVelocity2;
Vector3 tangencialVelocity3 = particles[tri.indexTriC].Velocity - normalVelocity3;
Vector3 velocityBestA = tangencialVelocityA - frictionConstCloth * normalVelocityA.magnitude * (tangencialVelocityA / tangencialVelocityA.magnitude) - dissipationConstCloth * normalVelocityA;
Vector3 velocityBestB = tangencialVelocityB - frictionConstCloth * normalVelocityB.magnitude * (tangencialVelocityB / tangencialVelocityB.magnitude) - dissipationConstCloth * normalVelocityB;
Vector3 velocityBest1 = tangencialVelocity1 - frictionConstCloth * normalVelocity1.magnitude * (tangencialVelocity1 / tangencialVelocity1.magnitude) - dissipationConstCloth * normalVelocity1;
Vector3 velocityBest2 = tangencialVelocity2 - frictionConstCloth * normalVelocity2.magnitude * (tangencialVelocity2 / tangencialVelocity2.magnitude) - dissipationConstCloth * normalVelocity2;
Vector3 velocityBest3 = tangencialVelocity3 - frictionConstCloth * normalVelocity3.magnitude * (tangencialVelocity3 / tangencialVelocity3.magnitude) - dissipationConstCloth * normalVelocity3;
/*if(situation == 3)
* {
* if(dotA < 0f)
* {
* particles[ed.indexEdgeA].Position = particles[ed.indexEdgeA].Position - dt * velocityBestA;
* particles[tri.indexTriA].Position = particles[tri.indexTriA].Position + dt * velocityBest1 * val1;
* particles[tri.indexTriB].Position = particles[tri.indexTriB].Position + dt * velocityBest2 * val2;
* particles[tri.indexTriC].Position = particles[tri.indexTriC].Position + dt * velocityBest3 * val3;
* }
* if(dotA > 0f)
* {
* particles[ed.indexEdgeA].Position = particles[ed.indexEdgeA].Position + dt * velocityBestA;
* particles[tri.indexTriA].Position = particles[tri.indexTriA].Position - dt * velocityBest1 * val1;
* particles[tri.indexTriB].Position = particles[tri.indexTriB].Position - dt * velocityBest2 * val2;
* particles[tri.indexTriC].Position = particles[tri.indexTriC].Position - dt * velocityBest3 * val3;
* }
* }
* if(situation == 4)
* {
* if(dotB < 0f)
* {
* particles[ed.indexEdgeB].Position = particles[ed.indexEdgeB].Position - dt * velocityBestB;
* particles[tri.indexTriA].Position = particles[tri.indexTriA].Position + dt * velocityBest1 * val1;
* particles[tri.indexTriB].Position = particles[tri.indexTriB].Position + dt * velocityBest2 * val2;
* particles[tri.indexTriC].Position = particles[tri.indexTriC].Position + dt * velocityBest3 * val3;
* }
* if(dotB > 0f)
* {
* particles[ed.indexEdgeB].Position = particles[ed.indexEdgeB].Position + dt * velocityBestB;
* particles[tri.indexTriA].Position = particles[tri.indexTriA].Position - dt * velocityBest1 * val1;
* particles[tri.indexTriB].Position = particles[tri.indexTriB].Position - dt * velocityBest2 * val2;
* particles[tri.indexTriC].Position = particles[tri.indexTriC].Position - dt * velocityBest3 * val3;
* }
* }*/
}
}
}
}
}
}
}
public void SimulateOneTimeStep(float dt)
{
#region Apply External Force
for (int v = 0; v < totalVerts; v++)
{
Vector3 force = Vector3.zero;
Vector3 corr;
// add gravity acceleration (f = ma)
force += gravity * meshData.particles[v].mass;
if (PBD.ExternalForce(
dt,
meshData.particles[v].predictedPos,
meshData.particles[v].velocity,
meshData.particles[v].invMass,
force,
damping,
out corr))
{
meshData.particles[v].predictedPos += corr;
}
}
#endregion
#region Apply Wind Force
float dirX = wind.GetComponentInParent <Transform>().eulerAngles.x % 360 / 360;
float dirY = wind.GetComponentInParent <Transform>().eulerAngles.y % 360 / 360;
float dirZ = wind.GetComponentInParent <Transform>().eulerAngles.z % 360 / 360;
// TODO: change this to real direction the arrow is pointing
Vector3 windDir = new Vector3(dirX, dirY, dirZ);
float windForce = wind.windMain;
for (int t = 0; t < totalTriangles; t++)
{
Triangle tri = meshData.triangles[t];
Vector3 p0 = meshData.particles[tri.p0].predictedPos;
Vector3 p1 = meshData.particles[tri.p1].predictedPos;
Vector3 p2 = meshData.particles[tri.p2].predictedPos;
float w0 = meshData.particles[tri.p0].invMass;
float w1 = meshData.particles[tri.p1].invMass;
float w2 = meshData.particles[tri.p2].invMass;
Vector3 corr0, corr1, corr2;
PBD.WindForce(
dt,
p0, w0,
p1, w1,
p2, w2,
windDir,
windForce,
out corr0, out corr1, out corr2);
meshData.particles[tri.p0].predictedPos += corr0;
meshData.particles[tri.p1].predictedPos += corr1;
meshData.particles[tri.p2].predictedPos += corr2;
}
#endregion
#region Collision Constraints
SH sh = new SH(gridSize, invGridSize, tableSize);
SPHash[] spHash = new SPHash[tableSize];
for (int v = 0; v < totalVerts; v++)
{
int hash = Mathf.Abs(sh.Hash(meshData.particles[v].predictedPos));
if (spHash[hash].indices == null)
{
spHash[hash].indices = new List <int>();
}
spHash[hash].indices.Add(meshData.particles[v].idx);
}
for (int t = 0; t < totalTriangles; t++)
{
Triangle tri = meshData.triangles[t];
Vector3 p0 = meshData.particles[tri.p0].predictedPos;
Vector3 p1 = meshData.particles[tri.p1].predictedPos;
Vector3 p2 = meshData.particles[tri.p2].predictedPos;
float w0 = meshData.particles[tri.p0].invMass;
float w1 = meshData.particles[tri.p1].invMass;
float w2 = meshData.particles[tri.p2].invMass;
List <int> hashes = sh.TriangleBoundingBoxHashes(p0, p1, p2);
for (int h = 0; h < hashes.Count; h++)
{
if (spHash[h].indices != null)
{
for (int sph = 0; sph < spHash[h].indices.Count; sph++)
{
int idx = spHash[h].indices[sph];
if (
idx != meshData.particles[tri.p0].idx &&
idx != meshData.particles[tri.p1].idx &&
idx != meshData.particles[tri.p2].idx)
{
Vector3 p = meshData.particles[idx].predictedPos;
float w = meshData.particles[idx].invMass;
Vector3 corr, corr0, corr1, corr2;
if (PBD.TrianglePointDistanceConstraint(
p, w,
p0, w0,
p1, w1,
p2, w2,
thickness, 1f, 0.0f,
out corr, out corr0, out corr1, out corr2))
{
meshData.particles[idx].predictedPos += corr;
meshData.particles[tri.p0].predictedPos += corr0;
meshData.particles[tri.p1].predictedPos += corr1;
meshData.particles[tri.p2].predictedPos += corr2;
}
}
}
}
}
}
#endregion
#region Project Constraints
for (int iter = 0; iter < iterationSteps; iter++)
{
#region Distance Constraint
for (int e = 0; e < totalEdges; e++)
{
Vector3 corr0, corr1;
Edge edge = meshData.edges[e];
Vector3 p0 = meshData.particles[edge.p0].predictedPos;
Vector3 p1 = meshData.particles[edge.p1].predictedPos;
float w0 = meshData.particles[edge.p0].invMass;
float w1 = meshData.particles[edge.p1].invMass;
if (PBD.DistanceConstraint(
p0, w0,
p1, w1,
edge.restLength,
stretchStiffness,
compressionStiffness,
out corr0, out corr1))
{
meshData.particles[edge.p0].predictedPos += corr0;
meshData.particles[edge.p1].predictedPos += corr1;
}
}
#endregion
#region Dihedral Constraint
for (int n = 0; n < totalNeighborTriangles; n++)
{
Vector3 corr0, corr1, corr2, corr3;
NeighborTriangles neighbor = meshData.neighborTriangles[n];
Vector3 p0 = meshData.particles[neighbor.p0].predictedPos;
Vector3 p1 = meshData.particles[neighbor.p1].predictedPos;
Vector3 p2 = meshData.particles[neighbor.p2].predictedPos;
Vector3 p3 = meshData.particles[neighbor.p3].predictedPos;
float w0 = meshData.particles[neighbor.p0].invMass;
float w1 = meshData.particles[neighbor.p1].invMass;
float w2 = meshData.particles[neighbor.p2].invMass;
float w3 = meshData.particles[neighbor.p3].invMass;
if (PBD.DihedralConstraint(
p0, w0,
p1, w1,
p2, w2,
p3, w3,
neighbor.restAngle,
bendingStiffness,
out corr0, out corr1, out corr2, out corr3))
{
meshData.particles[neighbor.p0].predictedPos += corr0;
meshData.particles[neighbor.p1].predictedPos += corr1;
meshData.particles[neighbor.p2].predictedPos += corr2;
meshData.particles[neighbor.p3].predictedPos += corr3;
}
}
#endregion
}
#endregion
}
