void Vector2::SafeNormalize()
{
float lenSqr = LengthSqr();
if (lenSqr > Math::EPSILON)
{
float invLen = 1.0f / Math::Sqrt(lenSqr);
x *= invLen; y *= invLen;
}
}
Vector2 Vector2::SafeNormalized() const
{
float lenSqr = LengthSqr();
if (lenSqr > Math::EPSILON)
{
float invLen = 1.0f / Math::Sqrt(lenSqr);
return Vector2(x * invLen, y * invLen);
}
return Vector2(0.0f, 0.0f);
}
void CalcForceGompper(Particle *xi, const int numNeighbors, Particle **const neighbors, const double kb)
{
// if ( xi->r.p[0] > maxX ) maxX = xi->r.p[0];
// DEBUG stuff
/* for (int i=0; i < numNeighbors; i++)
{
Vector3D tmp;
Subtr(tmp, xi, neighbors[i]);
const double dist = Length(tmp);
if ( dist > maxBendingDist ) maxBendingDist = dist;
}*/
// mainNumerator: Will be set to one of the terms in the numerator.
Vector3D mainNumerator;
mainNumerator.el[0] = mainNumerator.el[1] = mainNumerator.el[2] = 0;
// Will be set to the denominator.
double denominator = 0;
// We want to differentiate with respect to the i'th node ("virtual" index numDerivatives-1) and with respect to the neighbours of the i'th node (indecies 0<=l<=numDerivatives-2).
const int numDerivatives = numNeighbors + 1;
// The derivatives of the numerator in the energy with respect to the i'th node and its neighbours.
Matrix3D *derivNumerators = new Matrix3D[numDerivatives];
// The derivatives of the denominator in the energy with respect to the i'th node and its neighbours.
Vector3D *derivDenominators = new Vector3D[numDerivatives];
for (int i=0; i < numDerivatives; i++)
{
for ( int k=0; k < 3; k++)
{
derivDenominators[i].el[k] = 0;
for (int l=0; l < 3; l++)
derivNumerators[i].el[k][l] = 0;
}
}
// We need to calculate several sums over the neighbouring sites of i. This is done in the for-loop.
for (int curNeighborIdx = 0; curNeighborIdx < numNeighbors; ++curNeighborIdx)
{
// Get the neighbours of xi.
const Particle *xj = neighbors[curNeighborIdx];
// Neighbor list with periodic mapping
const Particle *xjM1 = (curNeighborIdx == 0) ? neighbors[numNeighbors-1] : neighbors[curNeighborIdx-1];
const Particle *xjP1 = (curNeighborIdx == numNeighbors-1) ? neighbors[0] : neighbors[curNeighborIdx+1];
Vector3D tmp;
Subtr(tmp, xi, xj);
const double dxijLen2 = LengthSqr(tmp);
// Cosine of the angles between the neighbours.
const double cosThetaM1 = CalcCosTheta(xi, xj, xjM1);
const double cosThetaP1 = CalcCosTheta(xi, xj, xjP1);
// The sum of the two cotangens.
const double Tij = CalcCot(cosThetaM1) + CalcCot(cosThetaP1);
Vector3D ijDifference;
Subtr(ijDifference, xi, xj);
// Update the two terms which are independent of the node xl (the derivatives are with respect to node xl).
Multiply(tmp, ijDifference, Tij);
AddTo( mainNumerator, tmp);
denominator += dxijLen2 * Tij;
// Now for the other terms, dependent on xl.
// TODO: The only terms which are not 0 are jM1, j, jP1 and i
for (int gradientNodeIdx = 0; gradientNodeIdx < numDerivatives; ++gradientNodeIdx)
{
const Particle *const xl = (gradientNodeIdx == numDerivatives-1) ? xi : neighbors[gradientNodeIdx];
// Derivatives of the two cotangens.
Vector3D TijDeriv;
CalcCotDerivativeGompperAnalyt(TijDeriv, xi, xj, xjM1, xl->p.identity);
Vector3D tmp;
CalcCotDerivativeGompperAnalyt(tmp, xi, xj, xjP1, xl->p.identity);
AddTo( TijDeriv, tmp);
// Kronecker Deltas.
const int ilDelta = (xi->p.identity == xl->p.identity) ? 1 : 0;
const int jlDelta = (xj->p.identity == xl->p.identity) ? 1 : 0;
// Update
Matrix3D tmpM;
DyadicProduct(tmpM, ijDifference, TijDeriv);
AddTo( derivNumerators[gradientNodeIdx], tmpM );
AddScalarTo( derivNumerators[gradientNodeIdx], Tij*(ilDelta-jlDelta) );
Multiply(tmp, ijDifference, Tij*2. * (ilDelta-jlDelta) );
AddTo( derivDenominators[gradientNodeIdx], tmp );
Multiply(tmp, TijDeriv, dxijLen2);
AddTo( derivDenominators[gradientNodeIdx], tmp );
}
}
// Calculate the contribution to the force for each node.
for (int gradientNodeIdx = 0; gradientNodeIdx < numDerivatives; ++gradientNodeIdx)
{
Particle *const xl = (gradientNodeIdx == numDerivatives-1) ? xi : neighbors[gradientNodeIdx];
// -1: The force is minus the gradient of the energy.
// Note: left vector-matrix product: mainNumerator * derivNumerators[gradientNodeIdx]
Vector3D force, tmp;
LeftVectorMatrix(force, mainNumerator, derivNumerators[gradientNodeIdx]);
Multiply(force, force, 4.0 / denominator );
Multiply(tmp, derivDenominators[gradientNodeIdx], - 2.0 * LengthSqr(mainNumerator) / (denominator*denominator));
AddTo(force, tmp);
Multiply(force, force, (-1.0) * (kb / 2.0));
// if ( xl->p.identity == 0)
// printf(" Adding to node = %d when treating node %d: f = %.12e %.12e %.12e\n", xl->p.identity, xi->p.identity, force.el[0], force.el[1], force.el[2]);
xl->f.f[0] += force.el[0];
xl->f.f[1] += force.el[1];
xl->f.f[2] += force.el[2];
// DEBUG stuff
/* for (int i=0; i < numNeighbors; i++)
{
const double f = Length(force);
if ( f > maxBendingForce ) maxBendingForce = f;
}*/
}
// Clean up
delete []derivNumerators;
delete []derivDenominators;
}
bool tressfx_vec3::operator>(float val) const
{
return (LengthSqr() > val*val);
}
double Vector3d::Length () const
{
return (double)sqrt( LengthSqr() );
}
bool Vector3d::IsZero (double Tolerance) const
{
return (LengthSqr() <= Tolerance*Tolerance);
}
bool Vector2D::IsLengthLessThan(float val) const
{
return LengthSqr() < val*val;
}
bool Vector2D::IsLengthGreaterThan(float val) const
{
return LengthSqr() > val*val;
}
inline bool Vector::operator<( Vector v )
{
return LengthSqr() < v.LengthSqr();
}
inline bool Vector::operator==( Vector v )
{
return LengthSqr() == v.LengthSqr();
}
