public float CalculateTD(List <Particle> particles)
{
HyperPoint <float> distanceVector = particles[_p1].Position - particles[_p2].Position;
HyperPoint <float> distanceVelocityVector = particles[_p1].Velocity - particles[_p2].Velocity;
return(2 * HyperPoint <float> .DotProduct(distanceVector, distanceVelocityVector));
}
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 float Calculate(List <Particle> particles)
{
if (Active(particles[_p1], particles[_p2]))
{
HyperPoint <float> distanceVector = particles[_p1].Position - particles[_p2].Position;
return(distanceVector.DotProduct(distanceVector) - _dist * _dist);
}
else
{
return(0);
}
}
public float Calculate(List <Particle> particles)
{
if (InRadius(particles))
{
HyperPoint <float> translateCenter = particles[_p].Position - _center;
return(translateCenter.DotProduct(translateCenter));
}
else
{
return(0f);
}
}
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;
}
public void CalculateForce(List <Particle> particles)
{
if (_isEnabled)
{
HyperPoint <float> _l = particles[_p1].Position - _mousePos;
HyperPoint <float> _lDot = particles[_p1].Velocity - new HyperPoint <float>(0, 0);
float _lAbs = _l.GetLength();
if (_lAbs != 0)
{
HyperPoint <float> _springforce = (_l / _lAbs) * ((_lAbs - _r) * _ks + (HyperPoint <float> .DotProduct(_lDot, _l) / _lAbs) * _kd);
particles[_p1].ForceAccumulator -= _springforce;
}
}
}
public float Calculate(List <Particle> particles)
{
HyperPoint <float> translateCenter = particles[_p].Position - _center;
return(translateCenter.DotProduct(translateCenter) - _radius * _radius);
}
public static float ConjGrad(int n, ImplicitMatrix <float> A, out HyperPoint <float> x, HyperPoint <float> b, float epsilon, int steps)
{
int i, iMax;
float alpha, beta, rSqrLen, rSqrLenOld, u;
HyperPoint <float> r = new HyperPoint <float>(n);
HyperPoint <float> d = new HyperPoint <float>(n);
HyperPoint <float> t = new HyperPoint <float>(n);
HyperPoint <float> temp = new HyperPoint <float>(n);
x = b;
r = b;
temp = A * x;
r = r - temp;
rSqrLen = r.GetLengthSquared();
d = r;
iMax = steps != 0 ? steps : MaxSteps;
i = 0;
if (rSqrLen > epsilon)
{
while (i < iMax)
{
i++;
t = A * d;
u = HyperPoint <float> .DotProduct(d, t);
if (u == 0)
{
Console.Out.WriteLine("(SolveConjGrad) d'Ad = 0\n");
break;
}
// How far should we go?
alpha = rSqrLen / u;
// Take a step along direction d
temp = d;
temp = temp * alpha;
x = x + temp;
if ((i & 0x3F) != 0)
{
temp = t;
temp = temp * alpha;
r = r - temp;
}
else
{
// For stability, correct r every 64th iteration
r = b;
temp = A * x;
r = r - temp;
}
rSqrLenOld = rSqrLen;
rSqrLen = r.GetLengthSquared();
// Converged! Let's get out of here
if (rSqrLen <= epsilon)
{
break;
}
// Change direction: d = r + beta * d
beta = rSqrLen / rSqrLenOld;
d = d * beta;
d = d + r;
}
}
steps = i;
return(rSqrLen);
}