public override void CalculatePositionDeltas(HalfEdge halfedge,List<ObiClothParticle> particles, float dt)
{
// We can skip this for fixed particles:
if (particles[pIndex].w == 0) return;
// Wake the particle up.
particles[pIndex].asleep = false;
Vector3 positionDiff = particles[pIndex].predictedPosition - point;
float distance = positionDiff.magnitude;
float surfaceDistance = Vector3.Dot(positionDiff,normal);
if (LinearStiffness > 0){
Vector3 correctionVector = Vector3.zero;
if (distance > radius){
float correctionFactor = distance - radius;
correctionVector += positionDiff / distance * correctionFactor;
}
if (surfaceDistance < backstop){
correctionVector += normal * Mathf.Min(surfaceDistance - backstop,0);
}
if (correctionVector != Vector3.zero){
particles[pIndex].positionDelta -= LinearStiffness * correctionVector;
particles[pIndex].numConstraints++;
}
}
}
public override void CalculatePositionDeltas(HalfEdge halfedge, List<ObiClothParticle> particles, float dt)
{
if (!enabled || restRi == null) return;
// Get current center of mass:
float totalMass = 0;
Vector3 cm = Vector3.zero;
foreach(ObiClothParticle p in particles){
float mass = 1/(p.w + epsilon);
totalMass += mass;
cm += p.predictedPosition * mass;
}
if (totalMass == 0) return;
cm /= totalMass;
// Get A:
Matrix4x4 A = Matrix4x4.zero;
A[3,3] = 1;
foreach(ObiClothParticle particle in particles){
Vector3 q = restRi[particle.index];
Vector3 p = particle.predictedPosition - cm;
float mass = 1/(particle.w + epsilon);
p *= mass;
A[0,0] += p[0] * q[0]; A[0,1] += p[0] * q[1]; A[0,2] += p[0] * q[2];
A[1,0] += p[1] * q[0]; A[1,1] += p[1] * q[1]; A[1,2] += p[1] * q[2];
A[2,0] += p[2] * q[0]; A[2,1] += p[2] * q[1]; A[2,2] += p[2] * q[2];
}
// Extract rotation component from matrix:
Matrix4x4 R = A.PolarDecomposition(1e-4f);
Matrix4x4 D = R;
A = A * invRestMatrix;
// Mix in the amount of allowed linear deformation:
if (linearDeformation > 0){
float detCubicRoot = Mathf.Clamp(Mathf.Pow(A.Determinant(),1/3f),0.9f,2);
if (!Single.IsNaN(detCubicRoot) && !Single.IsInfinity(detCubicRoot) && detCubicRoot != 0){
// We divide A by the cubic root of the determinant to ensure volume conservation:
D = A.MultiplyValue(linearDeformation/detCubicRoot).Add(R.MultiplyValue(1-linearDeformation));
}
}
foreach(ObiClothParticle particle in particles){
if (particle.w > 0){
Vector3 goal = cm + D.MultiplyPoint3x4(restRi[particle.index]);
particle.positionDelta += (goal - particle.predictedPosition) * LinearStiffness;
particle.numConstraints++;
}
}
}
public override void CalculatePositionDeltas(HalfEdge halfedge,List<ObiClothParticle> particles, float dt)
{
// If there is no rigidbody, it is kinematic or is sleeping, and the particle is asleep, we can skip this constraint.
if ((rigidbody == null || rigidbody.IsSleeping()) && particle.asleep) return;
// If both the particle and the rigidbody are fixed, skip this.
if (weightSum == 0) return;
//Calculate relative normal and tangent velocities at nearest point:
Vector3 rigidbodyVelocityAtContact = (rigidbody == null || rigidbody.isKinematic) ? Vector3.zero : transform.InverseTransformVector(rigidbody.GetPointVelocity(wspoint));
Vector3 relativeVelocity = (particle.predictedPosition - particle.position) / dt - rigidbodyVelocityAtContact;
float relativeNormalVelocity = Vector3.Dot(relativeVelocity,normal);
Vector3 tangentSpeed = relativeVelocity - relativeNormalVelocity * normal;
float relativeTangentVelocity = tangentSpeed.magnitude;
Vector3 tangent = tangentSpeed / (relativeTangentVelocity + epsilon);
//Calculate normal impulse correction:
float nvCorrection = relativeNormalVelocity + distance / dt;
float niCorrection = nvCorrection / weightSum;
//Accumulate impulse:
float newImpulse = Mathf.Min(normalImpulse + niCorrection,0);
//Calculate change impulse change and set new impulse:
float normalChange = newImpulse - normalImpulse;
normalImpulse = newImpulse;
// If this turns out to be a real (non-speculative) contact, compute friction impulse.
float tangentChange = 0;
if (nvCorrection < 0 && frictionCoeff > 0){ // Real contact
float tiCorrection = - relativeTangentVelocity / weightSum;
//Accumulate tangent impulse using coulomb friction model:
float frictionCone = - normalImpulse * frictionCoeff;
float newTangentImpulse = Mathf.Clamp(tangentImpulse + tiCorrection,-frictionCone, frictionCone);
//Calculate change impulse change and set new impulse:
tangentChange = newTangentImpulse - tangentImpulse;
tangentImpulse = newTangentImpulse;
}
if (normalChange != 0 || tangentChange != 0){
// wake the particle up:
particle.asleep = false;
particle.numConstraints++;
}
//Add impulse to particle and rigidbody
Vector3 impulse = (normal * normalChange - tangent * tangentChange);
particle.positionDelta -= impulse * particle.w * dt;
// add impulse to rigid body, if any:
if (rigidbody != null){
rigidbody.AddForceAtPosition(transform.TransformVector(impulse),wspoint,ForceMode.Impulse);
}
}
public override void CalculatePositionDeltas(HalfEdge halfedge,List<ObiClothParticle> particles, float dt)
{
// If there is no rigidbody, it is kinematic or is sleeping, and the particle is asleep, we can skip this constraint.
if (particle1.asleep && particle2.asleep) return;
// If both the particle and the rigidbody are fixed, skip this.
if (weightSum == 0) return;
//Calculate relative normal and tangent velocities at nearest point:
Vector3 relativeVelocity = (particle1.predictedPosition - particle1.position) / dt - (particle2.predictedPosition - particle2.position) / dt;
float relativeNormalVelocity = Vector3.Dot(relativeVelocity,normal);
Vector3 tangentSpeed = relativeVelocity - relativeNormalVelocity * normal;
float relativeTangentVelocity = tangentSpeed.magnitude;
Vector3 tangent = tangentSpeed / (relativeTangentVelocity + epsilon);
//Calculate normal impulse correction:
float nvCorrection = relativeNormalVelocity + distance / dt;
float niCorrection = nvCorrection / weightSum;
//Accumulate impulse:
float newImpulse = Mathf.Min(normalImpulse + niCorrection,0);
//Calculate change impulse change and set new impulse:
float normalChange = newImpulse - normalImpulse;
normalImpulse = newImpulse;
// If this turns out to be a real (non-speculative) contact, compute friction impulse.
float tangentChange = 0;
if (nvCorrection < 0){ // Real contact
float tiCorrection = - relativeTangentVelocity / weightSum;
//Accumulate tangent impulse using coulomb friction model:
float frictionCone = - normalImpulse * frictionCoeff;
float newTangentImpulse = Mathf.Clamp(tangentImpulse + tiCorrection,-frictionCone, frictionCone);
//Calculate change impulse change and set new impulse:
tangentChange = newTangentImpulse - tangentImpulse;
tangentImpulse = newTangentImpulse;
}
if (normalChange != 0 || tangentChange != 0){
// wake the particle up:
particle1.asleep = false;
particle2.asleep = false;
particle1.numConstraints++;
particle2.numConstraints++;
}
//Add impulse to both particles:
Vector3 impulse = (normal * normalChange - tangent * tangentChange);
particle1.positionDelta -= impulse * particle1.w * dt;
particle2.positionDelta += impulse * particle2.w * dt;
}
public override void CalculatePositionDeltas(HalfEdge halfedge, List<ObiClothParticle> particles, float dt)
{
if (!enabled || !halfedge.IsClosed) return;
float currentVolume = 0;
ObiClothParticle p1;
ObiClothParticle p2;
ObiClothParticle p3;
// calculate current mesh volume:
foreach(HalfEdge.HEFace face in halfedge.heFaces){
p1 = particles[halfedge.heEdges[face.edges[0]].endVertex];
p2 = particles[halfedge.heEdges[face.edges[1]].endVertex];
p3 = particles[halfedge.heEdges[face.edges[2]].endVertex];
currentVolume += Vector3.Dot(Vector3.Cross(p1.predictedPosition,p2.predictedPosition),p3.predictedPosition)/6f;
}
// calculate gradients and weighted sum:
Vector3[] gradients = new Vector3[particles.Count];
float gradientSum = 0;
foreach(HalfEdge.HEFace face in halfedge.heFaces){
p1 = particles[halfedge.heEdges[face.edges[0]].endVertex];
p2 = particles[halfedge.heEdges[face.edges[1]].endVertex];
p3 = particles[halfedge.heEdges[face.edges[2]].endVertex];
Vector3 n = Vector3.Cross(p2.predictedPosition-p1.predictedPosition,p3.predictedPosition-p1.predictedPosition);
gradients[p1.index] += n;
gradients[p2.index] += n;
gradients[p3.index] += n;
}
for(int i = 0; i < gradients.Length; i++){
gradientSum += particles[i].w * gradients[i].sqrMagnitude;
}
if (!Mathf.Approximately(gradientSum,0)){
// calculate constraint scaling factor:
float s = (currentVolume - pressure * halfedge.MeshVolume) / gradientSum;
// apply position correction to all particles:
foreach(ObiClothParticle p in particles){
p.positionDelta -= s * p.w * gradients[p.index] * LinearStiffness;
p.numConstraints++;
}
}
}
public override void CalculatePositionDeltas(HalfEdge halfedge,List<ObiClothParticle> particles, float dt)
{
// If both particles are asleep, skip the constraint.
if (particles[p1].asleep && particles[p2].asleep) return;
// If at least one of the particles is awake, wake the other one up.
particles[p1].asleep = false;
particles[p2].asleep = false;
Vector3 positionDiff = particles[p1].predictedPosition-particles[p2].predictedPosition;
float distance = positionDiff.magnitude;
if (distance > epsilon && (LinearStiffness > 0 || LinearCompressionStiffness > 0)){
float correctionFactor = (distance - restLenght * scale);
Vector3 correctionVector = positionDiff / distance * correctionFactor;
float wsum = particles[p1].w + particles[p2].w;
if (wsum > 0){
if (correctionFactor > 0){
particles[p1].positionDelta -= LinearStiffness * correctionVector * particles[p1].w/wsum;
particles[p2].positionDelta += LinearStiffness * correctionVector * particles[p2].w/wsum;
}else{
particles[p1].positionDelta -= LinearCompressionStiffness * correctionVector * particles[p1].w/wsum;
particles[p2].positionDelta += LinearCompressionStiffness * correctionVector * particles[p2].w/wsum;
}
particles[p1].numConstraints++;
particles[p2].numConstraints++;
}
// return false if the spring should break:
if (!shouldBreak)
shouldBreak = distance >= tearDistance;
}
}
public override void CalculatePositionDeltas(HalfEdge halfedge,List<ObiClothParticle> particles, float dt)
{
// move particle to pin position:
if (rigidbody != null){
float rigidbodyWeight = (rigidbody == null || rigidbody.isKinematic) ? 0 : 1/rigidbody.mass;
float weightSum = particles[pIndex].w + rigidbodyWeight;
if (weightSum == 0) return;
Vector3 wsOffset = rigidbody.transform.TransformPoint(offset);
Vector3 positionChange = (particles[pIndex].predictedPosition-transform.InverseTransformPoint(wsOffset)) / weightSum;
if (particles[pIndex].w > 0){
particles[pIndex].positionDelta -= positionChange * particles[pIndex].w;
particles[pIndex].numConstraints++;
}
// apply impulse to rigidbody:
if (!rigidbody.isKinematic){
rigidbody.AddForceAtPosition(transform.TransformVector(positionChange) / dt,wsOffset,ForceMode.Impulse);
}
}
}
public override void CalculatePositionDeltas(HalfEdge halfedge,List<ObiClothParticle> particles, float dt)
{
// If all particles are asleep, skip constraint.
if (particles[pIndex1].asleep && particles[pIndex2].asleep && particles[pIndex3].asleep) return;
// If at least one of the particles is awake, wake the other one up.
particles[pIndex1].asleep = false;
particles[pIndex2].asleep = false;
particles[pIndex3].asleep = false;
w = particles[pIndex1].w + particles[pIndex2].w + 2*particles[pIndex3].w;
if (w > 0 && LinearStiffness > 0){
Vector3 center = (particles[pIndex1].predictedPosition + particles[pIndex2].predictedPosition + particles[pIndex3].predictedPosition)/3f;
Vector3 dirCenter = particles[pIndex3].predictedPosition - center;
float distCenter = dirCenter.magnitude;
if (distCenter > 0){
float diff = 1.0f - ((K + restLenght) / distCenter);
if (diff >= 0){ // remove this to force a certain curvature.
Vector3 dirForce = dirCenter * diff;
particles[pIndex1].positionDelta += LinearStiffness * (2.0f*particles[pIndex1].w/w) * dirForce;
particles[pIndex2].positionDelta += LinearStiffness * (2.0f*particles[pIndex2].w/w) * dirForce;
particles[pIndex3].positionDelta -= LinearStiffness * (4.0f*particles[pIndex3].w/w) * dirForce;
particles[pIndex1].numConstraints++;
particles[pIndex2].numConstraints++;
particles[pIndex3].numConstraints++;
}
}
}
}
public IEnumerator Generate()
{
nodes.Clear();
if (mesh == null)
yield break;
vertices = mesh.vertices;
int[] triangles = mesh.triangles;
float size = Mathf.Max(mesh.bounds.size.x,mesh.bounds.size.y,mesh.bounds.size.z) + boundsPadding;
Bounds bounds = new Bounds(mesh.bounds.center,Vector3.one*size);
// Use the half-edge structure to generate angle-weighted normals:
HalfEdge he = new HalfEdge(mesh);
if (Application.isPlaying){
CoroutineJob.RunSynchronously(he.Generate()); //While playing, do this synchronously. We don't want to wait too much.
}else{
CoroutineJob generateHalfEdge = new CoroutineJob();
generateHalfEdge.asyncThreshold = 2000; //If this takes more than 2 seconds in the editor, do it asynchronously.
EditorCoroutine.StartCoroutine(generateHalfEdge.Start(he.Generate()));
//Wait for the half-edge generation to complete.
CoroutineJob.ProgressInfo progress = null;
while(!generateHalfEdge.IsDone){
try{
progress = generateHalfEdge.Result as CoroutineJob.ProgressInfo;
}catch(Exception e){
Debug.LogException(e);
yield break;
}
yield return progress;
}
}
//Calculate angle weighted normals, for correct inside/outside determination.
Vector3[] normals = he.AngleWeightedNormals();
yield return new CoroutineJob.ProgressInfo("Building BIH...",0.1f);
// Generate BIH to speed up NN triangle queries.
bih = new BIH();
bih.Generate(bounds,vertices,normals,triangles,8,0.8f);
// Breadth first construction:
Queue<ADFNode> queue = new Queue<ADFNode>();
ADFNode root = new ADFNode(bounds);
queue.Enqueue(root);
nodes.Add(root);
int counter = 0;
while(queue.Count > 0)
{
ADFNode node = queue.Dequeue();
// Here provide an upper-bound estimation of remaining time. Note that in some cases (high maxDepth and high maxError) the process will probably finish sooner than predicted.
if (counter % 10 == 0)
yield return new CoroutineJob.ProgressInfo("Generating distance field level "+node.depth+"...",0.1f + (node.depth/(float)maxDepth)*0.9f);
node.distances = new float[8];
if (highQualityGradient)
node.gradients = new Vector3[8];
// Sample distance at the 8 node corners:
for (int i = 0; i < 8; i++){
BIH.SurfaceInfo si = bih.DistanceToSurface(node.bounds.center + Vector3.Scale(node.bounds.extents,corners[i]));
node.distances[i] = si.signedDistance;
if (highQualityGradient)
node.gradients[i] = si.vectorToSurface;
}
if (node.depth >= maxDepth) continue;
// Measure distances at the 6 node faces, and the center of the node.
float[] realDistances = new float[7];
for (int i = 0; i < 7; i++)
realDistances[i] = bih.DistanceToSurface(node.bounds.center + Vector3.Scale(node.bounds.extents,errorSamples[i] * 0.5f)).signedDistance;
// Get interpolated estimation of distance at the center of possible child nodes:
float[] interpolatedDistances = new float[7];
for (int i = 0; i < 7; i++)
interpolatedDistances[i] = node.SampleDistanceAt(node.bounds.center + Vector3.Scale(node.bounds.extents,errorSamples[i] * 0.5f));
// Calculate mean squared error between measured distances and interpolated ones:
float mse = 0;
for (int i = 0; i < 7; i++){
float d = realDistances[i] - interpolatedDistances[i];
mse += d*d;
}
mse /= 7f;
// If error > threshold, subdivide the node.
if (mse > maxError){
node.children = new int[8];
for (int i = 0; i < 8; i++){
// Calculate child bounds and create the node:
Vector3 childCenter = node.bounds.center + Vector3.Scale(node.bounds.extents,corners[i] * 0.5f);
Vector3 childSize = node.bounds.size*0.5f;
ADFNode child = new ADFNode(new Bounds(childCenter,childSize));
child.depth = node.depth+1;
// Set our children index.
node.children[i] = nodes.Count;
// Add it to nodes list and store it for evaluation.
nodes.Add(child);
queue.Enqueue(child);
}
}
counter++;
}
// Get rid of octree.
bih = null;
}
/** * Calculates position deltas (corrections). Will use the provided particle list to * obtain the required particle data, and the half edge structure for fast mesh queries. */ public abstract void CalculatePositionDeltas(HalfEdge halfedge, List<ObiClothParticle> particles, float dt);