//Tests whether point p is in polygon. ref: http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
public bool IsInPolygon(HyperPoint <float> p, Matrix <float> m)
{
if (_points != null && _points.Count > 0)
{
int nvert = _points.Count;
int i, j;
bool c = false;
for (i = 0, j = nvert - 1; i < nvert; j = i++)
{
HyperPoint <float> verti = ConvertToPolygonSpace(_points[i], m);
HyperPoint <float> vertj = ConvertToPolygonSpace(_points[j], m);
if (((verti.Y > p.Y) != (vertj.Y > p.Y)) &&
(p.X < (vertj.X - verti.X) * (p.Y - verti.Y) / (vertj.Y - verti.Y) + verti.X))
{
c = !c;
}
}
return(c);
}
else
{
return(false);
}
}
public static HyperPoint <float> HGMult(this HyperPoint <float> p, Matrix <float> m)
{
p = new HyperPoint <float>(p, 1f);
p = (HyperPoint <float>)(m * p);
p = p.GetLowerDim(2);
return(p);
}
private HyperPoint <float> calculateForces(Particle p, List <HyperPoint <int> > contactPoints)
{
HyperPoint <float> linearForce = new HyperPoint <float>(0f, 0f);
float rotationForce = 0f;
Matrix <float> m = p.WorldMatrix;
m = Matrix <float> .Resize(new HyperPoint <float>(_positionFactor, _positionFactor)) * m;
foreach (HyperPoint <int> contactPoint in contactPoints)
{
foreach (HyperPoint <int> adjecentPoint in adjecent)
{
if (p.Polygon.IsInPolygon((contactPoint + adjecentPoint).ConvertTo <float>(), m))
{
float factor = 0.5f;
HyperPoint <float> force =
new HyperPoint <float>(_uField[contactPoint + adjecentPoint], _vField[contactPoint + adjecentPoint]) *
Math.Min(_dField[contactPoint + adjecentPoint], 1.0f);
force = force * factor;
linearForce += force;
rotationForce += HyperPoint <float> .Cross2D(contactPoint.ConvertTo <float>() - p.Position, force);
}
}
}
return(new HyperPoint <float>(linearForce, rotationForce));
}
private HyperPoint <float> ConvertToPolygonSpace(HyperPoint <float> p, Matrix <float> m)
{
p = new HyperPoint <float>(p, 1f);
p = (HyperPoint <float>)(m * p);
p = p.GetLowerDim(2);
return(p);
}
public Particle(HyperPoint <float> constructPos, HyperPoint <float> constructVel, float massa = 1f)
{
Size = 0.03f;
Color = new HyperPoint <float>(1, 1, 1);
_constructPos = constructPos;
ConstructVel = constructVel;
_forceAccumulator = new HyperPoint <float>(0, 0);
_massa = massa;
_visible = true;
reset();
}
public ContinuousField(params int[] size)
{
int totalSize = 1;
for (int i = 0; i < size.Length; i++)
{
totalSize *= size[i];
}
_data = new float[totalSize];
_size = new HyperPoint <int>(size);
}
public float CalculateTD(List <Particle> particles)
{
if (InRadius(particles))
{
return(0f);
}
else
{
return(HyperPoint <float> .DotProduct(2 *particles[_p].Position - 2 *_center, particles[_p].Velocity));
}
}
public void CalculateForce(List <Particle> particles)
{
Particle p1 = particles[_p1];
Particle p2 = particles[_p2];
HyperPoint <float> _l = p1.Position - p2.Position;
HyperPoint <float> _lDot = p1.Velocity - p2.Velocity;
float _lAbs = _l.GetLength();
HyperPoint <float> _springforce = (_l / _lAbs) * ((_lAbs - _r) * _ks + (HyperPoint <float> .DotProduct(_lDot, _l) / _lAbs) * _kd);
p1.ForceAccumulator -= _springforce;
p2.ForceAccumulator += _springforce;
}
/// <summary>
/// Calculate the force
/// </summary>
public void CalculateForce(List <Particle> particles)
{
foreach (int p in _particles)
{
List <HyperPoint <int> > contactPoints = FindBounderyCells(particles[p]);
HyperPoint <float> forces = calculateForces(particles[p], contactPoints);
particles[p].ForceAccumulator += forces.GetLowerDim(2);
particles[p].AngularForce += forces[2];
//HyperPoint<float> position = particles[p].Position*_positionFactor;
//particles[p].ForceAccumulator += new HyperPoint<float>(_uField[position], _vField[position]) * Math.Min(_dField[position], 1.0f);
}
}
public float Calculate(List <Particle> particles)
{
if (InRadius(particles))
{
HyperPoint <float> translateCenter = particles[_p].Position - _center;
return(translateCenter.DotProduct(translateCenter));
}
else
{
return(0f);
}
}
private void DoConstraints(List <IConstraint> constraints, List <Particle> particles, float dt, float ks, float kd)
{
ImplicitMatrix <float> W = ImplicitMatrix <float> .Identity(particles.Count * 2);
HyperPoint <float> Q = new HyperPoint <float>(particles.Count * 2);
HyperPoint <float> q = new HyperPoint <float>(particles.Count * 2);
HyperPoint <float> qDot = new HyperPoint <float>(particles.Count * 2);
HyperPoint <float> QDak = new HyperPoint <float>(particles.Count * 2);
for (int i = 0; i < particles.Count; i++)
{
q[i * 2 + 0] = particles[i].Position[0];
q[i * 2 + 1] = particles[i].Position[1];
qDot[i * 2 + 0] = particles[i].Velocity[0];
qDot[i * 2 + 1] = particles[i].Velocity[1];
W[i * 2 + 0, i * 2 + 0] = 1 / particles[i].Massa;
W[i * 2 + 1, i * 2 + 1] = 1 / particles[i].Massa;
Q[i * 2 + 0] = particles[i].ForceAccumulator[0];
Q[i * 2 + 1] = particles[i].ForceAccumulator[1];
}
HyperPoint <float> c = new HyperPoint <float>(constraints.Count);
HyperPoint <float> cDot = new HyperPoint <float>(constraints.Count);
for (int i = 0; i < constraints.Count; i++)
{
c[i] = constraints[i].Calculate(particles);
cDot[i] = constraints[i].CalculateTD(particles);
}
ImplicitMatrix <float> J = JacobianMatrix.Create(particles, constraints);
ImplicitMatrix <float> JDot = JacobianMatrix.CreateDerivative(particles, constraints);
ImplicitMatrix <float> JT = J.Transpose();
ImplicitMatrix <float> A = J * W * JT;
HyperPoint <float> b1 = -JDot * qDot;
HyperPoint <float> b2 = J * W * Q;
HyperPoint <float> b3 = ks * c;
HyperPoint <float> b4 = kd * cDot;
HyperPoint <float> b = b1 - b2 - b3 - b4;
HyperPoint <float> x;
LinearSolver.ConjGrad(b.Dim, A, out x, b, ConstraintsEpsilon, Steps);
QDak = JT * x;
for (int i = 0; i < particles.Count; i++)
{
particles[i].ForceConstraint = new HyperPoint <float>(QDak[i * 2 + 0], QDak[i * 2 + 1]);
}
}
private void OnUpdateFrame(object sender, FrameEventArgs frameEventArgs)
{
if (dsim)
{
HyperPoint <float> mousePos = ((HyperPoint <float>)(mouseTranslation * new HyperPoint <float>(Mouse.X, Mouse.Y, 1))).GetLowerDim(2);
mouseForce.MousePos = ((HyperPoint <float>)(mouseTranslation * new HyperPoint <float>(Mouse.X, Mouse.Y, 1))).GetLowerDim(2);
int steps;
steps = Math.Max(1, Convert.ToInt32(Math.Round(1 / dt * SpeedUp / 60.0f)));
if (dump_frames)
{
steps = 1;
}
for (int i = 0; i < steps; i++)
{
//update boundary
liqSystem.FillBoundryIndexes();
particles.ForEach(liqSystem.AddBoundries);
liqSystem.FillOuterBoundryCells();
//update sources
liqSystem.ResetSources();
if (!mouseForce.IsEnabled)
{
liqSystem.UI(mousePos, Mouse[MouseButton.Left], Mouse[MouseButton.Right], 5.0f, 100.0f);
}
particles.ForEach(x => liqSystem.AddForces(x, dt));
//update liquid system
liqSystem.SimulationStep(dt, 0.000f, 0.000f);
_uField._data = liqSystem.u;
_vField._data = liqSystem.v;
_dField._data = liqSystem.densityField;
//update particle system
_solverEnvironment.SimulationStep(particles, dt);
}
}
else
{
//todo reset
}
// add game logic, input handling
if (Keyboard[Key.Escape])
{
Exit();
}
}
public void Draw(Matrix <float> m)
{
if (_points != null && _points.Count > 0)
{
GL.Begin(PrimitiveType.LineLoop);
for (int i = 0; i < _points.Count; i++)
{
HyperPoint <float> p = _points[i];
p = ConvertToPolygonSpace(p, m);
GLMath2.Vertex2(p);
}
GL.End();
}
}
private List <HyperPoint <int> > FindBounderyCells(Particle p)
{
List <HyperPoint <int> > output = new List <HyperPoint <int> >();
if (p.Polygon.Points.Count > 0)
{
HyperPoint <float> min = new HyperPoint <float>(2), max = new HyperPoint <float>(2);
HyperPoint <int> imin = new HyperPoint <int>(2), imax = new HyperPoint <int>(2);
List <HyperPoint <float> > points = p.Polygon.Points;
Matrix <float> m = p.WorldMatrix;
min.X = points.Min(point => point.HGMult(m).X);
max.X = points.Max(point => point.HGMult(m).X);
min.Y = points.Min(point => point.HGMult(m).Y);
max.Y = points.Max(point => point.HGMult(m).Y);
min = min * _positionFactor;
max = max * _positionFactor;
imin.X = (int)Math.Floor(min.X);
imax.X = (int)Math.Ceiling(max.X);
imin.Y = (int)Math.Floor(min.Y);
imax.Y = (int)Math.Ceiling(max.Y);
m = Matrix <float> .Resize(new HyperPoint <float>(_positionFactor, _positionFactor)) * m;
for (int i = imin.X; i <= imax.X; i++)
{
for (int j = imin.Y; j <= imax.Y; j++)
{
bool pin = p.Polygon.IsInPolygon(new HyperPoint <float>(i, j), m);
if (pin)
{
bool[] result = new bool[4];
for (int k = 0; k < adjecent.Length; k++)
{
result[k] = p.Polygon.IsInPolygon(new HyperPoint <float>(i, j) + adjecent[k].ConvertTo <float>(), m);
}
if (Array.Exists(result, x => x))
{
output.Add(new HyperPoint <int>(i, j));
}
}
}
}
}
return(output);
}
private void UpdateVirtualScreenSize()
{
float MinSize = 2f;
if (MinSize / Width * Height < MinSize)
{
VirtualScreenSize = new HyperPoint <float>(MinSize / Height * Width, MinSize);
}
else
{
VirtualScreenSize = new HyperPoint <float>(MinSize, MinSize / Width * Height);
}
UpdateMouseMatrix();
}
private void OnMouseDown(object sender, MouseButtonEventArgs e)
{
float _dist = 0.06f;
HyperPoint <float> _mousepos =
((HyperPoint <float>)(mouseTranslation * new HyperPoint <float>(e.X, e.Y, 1))).GetLowerDim(2);
foreach (Particle _p in particles)
{
if ((_mousepos - _p.Position).GetLengthSquared() < _dist * _dist)
{
mouseForce.MousePos = _mousepos;
mouseForce.Particle = _p.Index;
mouseForce.Enable();
}
}
}
private void CreateHair()
{
systemName = "Hair";
double beta = Math.PI / 4;
HyperPoint <float> startPosition = new HyperPoint <float>(0, 1f);
HyperPoint <float> unitSize = new HyperPoint <float>(0, -0.1f);
float stepRight = Convert.ToSingle(Math.Cos(beta / 2) * unitSize.GetLength());
float stepDown = -Convert.ToSingle(Math.Sqrt(unitSize.Y * unitSize.Y - stepRight * stepRight));
float springDist = Convert.ToSingle(2 * unitSize.GetLength() * Math.Sin(beta));
int particleCount = 20;
List <Particle> hair = new List <Particle>();
for (int i = 0; i < particleCount; i++)
{
Particle p;
if (i % 2 == 0)
{
p = new Particle(startPosition + new HyperPoint <float>(0, i * stepDown));
}
else
{
p = new Particle(startPosition + new HyperPoint <float>(stepRight, i * stepDown));
}
AddParticle(p);
hair.Add(p);
}
for (int i = 0; i < particleCount - 2; i++)
{
SpringForce sp = new SpringForce(i, i + 2, springDist, 1, 1);
Add(sp);
}
for (int i = 0; i < particleCount - 1; i++)
{
RodConstraint rc = new RodConstraint(i, i + 1, unitSize.GetLength());
Add(rc);
}
Add(new CircularWireConstraint(hair[0].Index, startPosition - new HyperPoint <float>(0f, -0.1f), 0.1f));
List <int> particleIndexes = hair.ConvertAll(x => x.Index);
Add(new ViscousDragForce(particleIndexes, 1f));
Add(new GravityForce(particleIndexes, new HyperPoint <float>(0, -0.1f)));
}
private void UpdateVirtualScreenSize()
{
float MinSize = 1f;
float margin = 0.1f;
VirtualScreenSize1 = new HyperPoint <float>(-margin, -margin);
if (MinSize / Width * Height < MinSize)
{
VirtualScreenSize2 = new HyperPoint <float>(MinSize / Height * Width + margin, MinSize + margin);
}
else
{
VirtualScreenSize2 = new HyperPoint <float>(MinSize + margin, MinSize / Width * Height + margin);
}
UpdateMouseMatrix();
}
private static int GetAdjacentCubesCount(Cube cube, Dictionary <Point, Cube> cubes, Point[] directions)
{
int count = 0;
foreach (Point dir in directions)
{
Point p = cube.Point switch {
HyperPoint hp when dir is HyperPoint d => new HyperPoint(hp.W + d.W, hp.X + d.X, hp.Y + d.Y, hp.Z + d.Z),
CubePoint cp when dir is CubePoint d => new CubePoint(cp.X + d.X, cp.Y + d.Y, cp.Z + d.Z),
_ => new Point(cube.Point.X + dir.X, cube.Point.Y + dir.Y)
};
if (cubes.GetValueOrDefault(p)?.State == ON_STATE)
{
count++;
}
}
return(count);
}
