/*
* 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);
}
}
}
