void SatisfyConstraints()
{
const int numIterations = 5;
for (int i = 0; i < numIterations; i++)
{
// Constraint (avoids)
if (PlantManager.Exists)
{
for (int a = 0; a < PlantManager.Instance._avoids.Count; ++a)
{
PlantAvoid pa = PlantManager.Instance._avoids[a];
// points
for (int v = 0; v < _points.Length; v++)
{
Vector3 pos = _parent.position + GetPointPos(v);
Vector3 dist = pos - pa.transform.position;
float r = pa.radius + Plant.SCALE * 0.3f;
if (dist.magnitude < r)
{
pos = (pa.transform.position + (dist.normalized * r)) - _parent.position;
_points[v].curr_mat.SetColumn(3,
new Vector4(pos.x, pos.y, pos.z, 1f));
}
}
// segments
for (int v = 0; v < _pos_constraints.Length; v++)
{
int i0 = _pos_constraints[v].index_0;
int i1 = _pos_constraints[v].index_1;
Vector3 p0 = _parent.position + GetPointPos(i0);
Vector3 p1 = _parent.position + GetPointPos(i1);
Vector3 closest = UtilMath.ClosestPointOnLineSegment3D(p0, p1, pa.transform.position);
Vector3 dist = closest - pa.transform.position;
float r = pa.radius + Plant.SCALE * 0.1f;
if (dist.magnitude < r)
{
dist = (dist.normalized * r) - dist;
// set point
p0 = GetPointPos(i0) + dist;
p1 = GetPointPos(i1) + dist;
SetPointPos(i0, p0);
SetPointPos(i1, p1);
// force
//float multi = 1f - (dist.magnitude / pa.radius);
//Vector3 push = (dist.normalized * pa.radius) * multi;
//_angle_constraint_force[i0] += push;
//_angle_constraint_force[i1] += push;
}
}
}
}
// Constraint (floor)
for (int v = 0; v < _points.Length; v++)
{
Vector3 cp = _points[v].curr_mat.GetColumn(3);
if (cp.y + _parent.position.y < 0f)
{
_points[v].curr_mat.SetColumn(3, new Vector4(cp.x, -_parent.position.y, cp.z, 1f));
}
}
for (int k = 0; k < _pos_constraints.Length; k++)
{
ConstraintPosition c = _pos_constraints[k];
// positions constraint
Vector3 p0 = _points[c.index_0].curr_mat.GetColumn(3);
Vector3 p1 = _points[c.index_1].curr_mat.GetColumn(3);
Vector3 delta = p1 - p0;
float len = delta.magnitude;
float diff = (len - c.rest_length) / len;
//p0 += delta * 0.5f * diff;
//p1 -= delta * 0.5f * diff;
// mass change
// push child more than parent
p0 += delta * 0.8f * diff;
p1 -= delta * 0.2f * diff;
_points[c.index_0].curr_mat.SetColumn(3, new Vector4(p0.x, p0.y, p0.z, 1f));
_points[c.index_1].curr_mat.SetColumn(3, new Vector4(p1.x, p1.y, p1.z, 1f));
if (_points[c.index_0].is_fixed)
{
_points[c.index_0].curr_mat = _points[c.index_0].prev_mat;
}
if (_points[c.index_1].is_fixed)
{
_points[c.index_1].curr_mat = _points[c.index_1].prev_mat;
}
}
}
}