/* * public void AddParticle(Point3 index) * { * LatticeLocation l = new LatticeLocation(); * mLatticeLocations.Add(l); * mLattice[index] = l; * l.mIndex = index; * l.Body = this; * l.Region = null; * * for (int xo = -1; xo <= 1; ++xo) * { * for (int yo = -1; yo <= 1; ++yo) * { * for (int zo = -1; zo <= 1; ++zo) * { * Point3 check = new Point3(index.x + xo, index.y + yo, index.z + zo); * if (!(xo == 0 && yo == 0 && zo == 0) && GetLatticeLocation(check) != null) * { * l.mImmediateNeighbors.Add(GetLatticeLocation(check)); * l.mImmediateNeighborsGrid[xo + 1, yo + 1, zo + 1] = GetLatticeLocation(check); * GetLatticeLocation(check).mImmediateNeighbors.Add(l); * GetLatticeLocation(check).mImmediateNeighborsGrid[-xo + 1, -yo + 1, -zo + 1] = l; * } * else * { * l.mImmediateNeighborsGrid[xo + 1, yo + 1, zo + 1] = null; * } * } * } * } * * l.SmParticle = new SmParticle(); * mParticles.Add(l.SmParticle); * l.SmParticle.LP = l; * l.SmParticle.mX0 = Vector3.Scale(new Vector3((float)index.x, (float)index.y, (float)index.z), mSpacing); * l.SmParticle.Mass = DefaultParticleMass; * l.SmParticle.mX = l.SmParticle.mX0; * l.SmParticle.mV = Vector3.zero; * l.SmParticle.mF = Vector3.zero; * } */ public void Complete() { InitializeParticles(); for (int i = 0; i != mLatticeLocations.Count; ++i) { LatticeLocation l = mLatticeLocations[i]; l.IsEdge = (l.mImmediateNeighbors.Count != 26); l.CalculateNeighborhood(); } GenerateSMRegions(); for (int i = 0; i != mRegions.Count; ++i) { SmRegion r = mRegions[i]; for (int j = 0; j != r.mParticles.Count; ++j) { SmParticle p = r.mParticles[j]; p.mParentRegions.Add(r.LP); } } CalculateInvariants(); //InitializeCells(); // Cells help with rendering }
public void CalculateInvariants() { Debug.Log("Calculating invariants..."); for (int i = 0; i != mParticles.Count; ++i) { SmParticle particle = mParticles[i]; particle.PerRegionMass = particle.Mass / particle.mParentRegions.Count; } for (int i = 0; i != mParticles.Count; ++i) { SmParticle p = mParticles[i]; p.mSumData.mM.m00 = p.PerRegionMass; p.mSumData.mV = p.PerRegionMass * p.mX0; } SumParticlesToRegions(); for (int i = 0; i != mRegions.Count; ++i) { SmRegion r = mRegions[i]; r.mM = r.mSumData.mM.m00; r.mEx0 = r.mSumData.mV; r.mC0 = r.mEx0 / r.mM; } Debug.Log(" done."); }
/* * public void UpdateCellPositions() * { * for (int i = 0; i != mCells.Count; ++i) * { * SmCell cell = mCells[i]; * cell.UpdateVertexPositions(); * } * } */ // Fast summation public void SumParticlesToRegions() { for (int i = 0; i != mSummations[0].Count; ++i) { Summation bar = mSummations[0][i]; bar.SumFromChildren(); } for (int i = 0; i != mSummations[1].Count; ++i) { Summation plate = mSummations[1][i]; plate.SumFromChildren(); } for (int i = 0; i != mRegions.Count; ++i) { SmRegion region = mRegions[i]; region.SumFromChildren(); } }
// Helper functions; public void GenerateSMRegions() { for (int i = 0; i != mLatticeLocationsWithExistentRegions.Count; ++i) { LatticeLocation l = mLatticeLocationsWithExistentRegions[i]; l.Region = new SmRegion(); mRegions.Add(l.Region); l.Region.LP = l; for (int j = 0; j != l.mNeighborhood.Count; ++j) { LatticeLocation l2 = l.mNeighborhood[j]; /* * l.Region.mParticles.Add(l2.SmParticle); */ for (int k = 0; k != 8; ++k) { if (false == l.Region.mParticles.Contains(l2.mParticles[k])) { l.Region.mParticles.Add(l2.mParticles[k]); } } } l.Region.mParticles.Sort(SmParticle.CompareSmParticle); l.Region.mEigenVectors = Matrix3x3.identity; } for (int i = 0; i != mRegions.Count; ++i) { SmRegion region = mRegions[i]; region.GenerateChildSums(1); } }
public void PerformRegionDamping() { if (RegionDamping == 0.0f) { return; } for (int i = 0; i != mParticles.Count; ++i) { SmParticle particle = mParticles[i]; particle.mSumData.mV = particle.PerRegionMass * particle.mV; particle.mSumData.mM.SetColumn(0, Vector3.Cross(particle.mX, particle.mSumData.mV)); Vector3 x = particle.mX; particle.mSumData.mM.m21 = particle.PerRegionMass * (x.z * x.z + x.y * x.y); particle.mSumData.mM.m01 = particle.PerRegionMass * (-x.x * x.y); particle.mSumData.mM.m02 = particle.PerRegionMass * (-x.x * x.z); particle.mSumData.mM.m11 = particle.PerRegionMass * (x.z * x.z + x.x * x.x); particle.mSumData.mM.m12 = particle.PerRegionMass * (-x.z * x.y); particle.mSumData.mM.m22 = particle.PerRegionMass * (x.y * x.y + x.x * x.x); } SumParticlesToRegions(); for (int i = 0; i != mRegions.Count; ++i) { SmRegion region = mRegions[i]; Vector3 v = Vector3.zero; Vector3 L = Vector3.zero; Matrix3x3 I = Matrix3x3.zero; Matrix3x3 FmixixiT = new Matrix3x3( region.mSumData.mM.m21, region.mSumData.mM.m01, region.mSumData.mM.m02, region.mSumData.mM.m01, region.mSumData.mM.m11, region.mSumData.mM.m12, region.mSumData.mM.m02, region.mSumData.mM.m12, region.mSumData.mM.m22); v = (1.0f / region.mM) * region.mSumData.mV; L = region.mSumData.mM.GetColumn(0) - Vector3.Cross(region.mC, region.mSumData.mV); I = FmixixiT - region.mM * MrMatrix(ref region.mC); Vector3 w = I.inverse * L; region.mSumData.mV = v; region.mSumData.mM.SetColumn(0, w); region.mSumData.mM.SetColumn(1, Vector3.Cross(w, region.mC)); } SumRegionsToParticles(); for (int i = 0; i != mParticles.Count; ++i) { SmParticle particle = mParticles[i]; if (particle.mParentRegions.Count > 0) { Vector3 Fv = particle.mSumData.mV; Vector3 Fw = particle.mSumData.mM.GetColumn(0); Vector3 Fwc = particle.mSumData.mM.GetColumn(1); Vector3 dv = (1.0f / particle.mParentRegions.Count) * (Fv + Vector3.Cross(Fw, particle.mX) - Fwc - (float)particle.mParentRegions.Count * particle.mV); particle.mV = particle.mV + RegionDamping * dv; } } }
// Simulation public void ShapeMatch() { if (IsDirty) { CalculateInvariants(); IsDirty = false; } for (int i = 0; i != mParticles.Count; ++i) { SmParticle particle = mParticles[i]; particle.mSumData.mV = particle.PerRegionMass * particle.mX; particle.mSumData.mM = Matrix3x3.MultiplyWithTranspose(particle.PerRegionMass * particle.mX, particle.mX0); } SumParticlesToRegions(); for (int i = 0; i != mRegions.Count; ++i) { SmRegion r = mRegions[i]; Vector3 Fmixi = r.mSumData.mV; Matrix3x3 Fmixi0T = r.mSumData.mM; r.mC = (1 / r.mM) * Fmixi; //9 r.mA = Fmixi0T - Matrix3x3.MultiplyWithTranspose(r.mM * r.mC, r.mC0); Matrix3x3 S = r.mA.transpose * r.mA; S = r.mEigenVectors.transpose * S * r.mEigenVectors; float[] eigenValues = new float[3]; Jacobi.jacobi(3, ref S, ref eigenValues, ref r.mEigenVectors); for (int j = 0; j != 3; ++j) { if (eigenValues[j] <= 0.0f) { eigenValues[j] = 0.05f; } eigenValues[j] = 1.0f / Mathf.Sqrt(eigenValues[j]); } Matrix3x3 DPrime = new Matrix3x3(eigenValues[0], 0, 0, 0, eigenValues[1], 0, 0, 0, eigenValues[2]); S = r.mEigenVectors * DPrime * r.mEigenVectors.transpose; r.mR = r.mA * S; if (r.mR.determinant < 0) { r.mR *= (-1.0f); } r.mT = r.mC - r.mR * r.mC0; r.mSumData.mM = r.mR; r.mSumData.mV = r.mT; } SumRegionsToParticles(); for (int i = 0; i != mParticles.Count; ++i) { SmParticle particle = mParticles[i]; float invNumParentRegions = 1.0f / particle.mParentRegions.Count; particle.mG = (invNumParentRegions * particle.mSumData.mM) * particle.mX0 + invNumParentRegions * particle.mSumData.mV; particle.mR = invNumParentRegions * particle.mSumData.mM; } }
/* * void OnDrawGizmos() * { * } */ void OnDrawGizmosSelected() { /* * if (null == mMeshFilters) * return; * * Gizmos.color = Color.blue; * Gizmos.DrawWireCube(mBounds.center, mBounds.size); * * Gizmos.color = Color.green; * for (int i = 0; i != mMeshFilters.Length; ++i) * { * MeshFilter mf = mMeshFilters[i]; * Gizmos.DrawWireCube(mf.mesh.bounds.center, mf.mesh.bounds.size); * } * * if (null == mCellInfos) * return; */ /* * int[] dimensions = new int[3]{mCellInfos.GetLength(0), mCellInfos.GetLength(1), mCellInfos.GetLength(2)}; * for (int x = 0; x != dimensions[0]; ++x) * { * for (int y = 0; y != dimensions[1]; ++y) * { * for (int z = 0; z != dimensions[2]; ++z) * { * SmCellInfo info = mCellInfos[x,y,z]; * if (info.mValid) * { * Gizmos.color = new Color(0, 0, 0, 0.1f); * Gizmos.DrawCube(info.mBounds.center, info.mBounds.size); * * Gizmos.color = Color.black; * Gizmos.DrawWireCube(info.mBounds.center, info.mBounds.size); * * Gizmos.color = Color.red; * Gizmos.DrawSphere(info.mBounds.center, 0.05f); * * for (int w = 0; w != mDeformableModel.mParticles.Count; ++w) * { * Gizmos.DrawSphere(mDeformableModel.mParticles[w].mX, 0.03f); * } * * * LatticeLocation ll = mDeformableModel.GetLatticeLocation(new Point3(x,y,z)); * Gizmos.color = Color.gray; * for (int w = 0; w != info.mVertices.Count; ++w) * { * VertexPosition vp = info.mVertices[w]; * Vector3 vPos = * ll.mParticles[0].mX0 * (1.0f - vp.mOriginPosition.x) * (1.0f - vp.mOriginPosition.y) * (1.0f - vp.mOriginPosition.z) + * ll.mParticles[1].mX0 * (vp.mOriginPosition.x) * (1.0f - vp.mOriginPosition.y) * (1.0f - vp.mOriginPosition.z) + * ll.mParticles[3].mX0 * (1.0f - vp.mOriginPosition.x) * (vp.mOriginPosition.y) * (1.0f - vp.mOriginPosition.z) + * ll.mParticles[2].mX0 * (1.0f - vp.mOriginPosition.x) * (1.0f - vp.mOriginPosition.y) * (vp.mOriginPosition.z) + * ll.mParticles[6].mX0 * (vp.mOriginPosition.x) * (1.0f - vp.mOriginPosition.y) * (vp.mOriginPosition.z) + * ll.mParticles[5].mX0 * (1.0f - vp.mOriginPosition.x) * (vp.mOriginPosition.y) * (vp.mOriginPosition.z) + * ll.mParticles[4].mX0 * (vp.mOriginPosition.x) * (vp.mOriginPosition.y) * (1.0f - vp.mOriginPosition.z) + * ll.mParticles[7].mX0 * (vp.mOriginPosition.x) * (vp.mOriginPosition.y) * (vp.mOriginPosition.z); * * Gizmos.DrawSphere(vPos, 0.01f); * } * } * } * } * } */ if (null != mDeformableModel) { for (int j = 0; j != mDeformableModel.mParticles.Count; ++j) { SmParticle p = mDeformableModel.mParticles[j]; Gizmos.color = Color.red; Gizmos.DrawSphere(transform.TransformPoint(p.mX), 0.03f); } for (int j = 0; j != mDeformableModel.mRegions.Count; ++j) { SmRegion r = mDeformableModel.mRegions[j]; Bounds bounds = new Bounds(transform.TransformPoint(r.mParticles[0].mX), Vector3.zero); for (int k = 0; k != r.mParticles.Count; ++k) { bounds.Encapsulate(transform.TransformPoint(r.mParticles[k].mX)); } Gizmos.color = new Color(r.color.r, r.color.g, r.color.b, 0.05f); Gizmos.DrawCube(bounds.center, bounds.size); Gizmos.color = r.color; Gizmos.DrawWireCube(bounds.center, bounds.size); } } }