void compute(const Vector3<T>& v1, const Vector3<T>& v2, const Vector3<T>& v3) noexcept
{
// https://github.com/ands/trianglepacker/blob/master/trianglepacker.h at 185 line
Vector3<T> tv[3];
tv[0] = v2 - v1;
tv[1] = v3 - v2;
tv[2] = v1 - v3;
T len2[3];
len2[0] = length2(tv[0]);
len2[1] = length2(tv[1]);
len2[2] = length2(tv[2]);
std::uint8_t maxi; T maxl = len2[0]; maxi = 0;
if (len2[1] > maxl) { maxl = len2[1]; maxi = 1; }
if (len2[2] > maxl) { maxl = len2[2]; maxi = 2; }
std::uint8_t nexti = (maxi + 1) % 3;
auto ww = std::sqrt(maxl);
auto xx = -dot(tv[maxi], tv[nexti]) / ww;
auto hh = length((tv[maxi] + tv[nexti]) - normalize(tv[maxi]) * (ww - xx));
this->w = std::ceil(ww);
this->h = std::ceil(hh);
}
bool FloatRect::intersects(const Sphere &sphere){
float radius2 = sphere.r * sphere.r;
return length2(sphere.x - this->x, sphere.y - this->y) < radius2 ||
length2(sphere.x - (this->x + this->w), sphere.y - this->y) < radius2 ||
length2(sphere.x - this->x, sphere.y - (this->y + this->h)) < radius2 ||
length2(sphere.x - (this->x + this->w), sphere.y - (this->y + this->h)) < radius2;
}
void draw_sec(CHpoints *p) {
dpoint c;
CHpoints *p1,*p2,*p3;
double radius;
if ((length2(before(p)->node,p->node) >
length2(p->node,next(p)->node)) &&
(length2(before(p)->node,p->node) >
length2(before(p)->node,next(p)->node)))
p2=next(p); /* the angle at next(p) is the biggest */
else if ((length2(p->node,next(p)->node) >
length2(before(p)->node,next(p)->node)) &&
(length2(p->node,next(p)->node) >
length2(p->node,before(p)->node)))
p2=before(p); /* the angle at before(p) is the biggest */
else
p2=p; /* the angle at p is the biggest */
p1=before(p2);
p3=next(p2);
if (angle(p1,p2,p3)<0) {
c.x=(midpoint(p1->node,p3->node)).x; /* center is midpoint of */
c.y=(midpoint(p1->node,p3->node)).y; /* p1 and p3 */
radius=sqrt((double)length2(p1->node,p3->node))/2.00;
}
else {
c=centre(p1->node,p2->node,p3->node);
radius=sqrt((double)radius2(p->node,c));
}
printf("The center is (%d,%d)\n",(int)c.x,(int)c.y);
printf("The radius is %9.2f\n",radius);
}
void longer(line *p_line1, line *p_line2,
line **pp_line)
{
if(length2(p_line1) >= length2(p_line2))
{
*pp_line = p_line1;
}
else
{
*pp_line = p_line2;
}
}
bool MotionManager::isShakingImpl( float minShakeDeltaThreshold )
{
const vec3& accel = getAcceleration();
std::unique_lock<std::mutex> lock( sMutex ); // lock after we get the acceleration so there is no deadlock
bool isShaking = false;
if( mPrevAcceleration != vec3( 0 ) ) {
mShakeDelta = length2(accel - mPrevAcceleration);
if( length2(accel - mPrevAcceleration) > (minShakeDeltaThreshold * minShakeDeltaThreshold) )
isShaking = true;
}
mPrevAcceleration = accel;
return isShaking;
}
vfloat sin2vec(const vec& r1, const vec& r2) {
// sinus of angle between vectors
pvecerror("vfloat sin2vec(const vec& r1, const vec& r2)");
vfloat lr1 = length2(r1);
vfloat lr2 = length2(r2);
if (lr1 == 0 || lr2 == 0) {vecerror=1; return 0;}
vfloat sn = length(r1||r2);
sn = sn * sn;
sn = sqrt(sn / (lr1 * lr2));
//mcout<<"r1="<<r1<<"r2="<<r2<<"sin="<<sn<<'\n';
return sn;
//return sin(ang2vec(r1,r2));
}
static void handleCollisionHuge(Particle *p)
{
Vec3 phuge;
Vec3 dv, dr, dx1, dx2, d;
Vec3 comv, comv1; /* Center Of Mass velocity */
Vec3 dv1, dv2; /* Change of velocity after collision */
float dvt1;
/* Difference of velocity in the direction of the tangent */
float dt;
float drsq, dvsq, dvdr, mindist;
/* First, backtrack the movements of the particle to the moment they
* were just touching */
sub(&p->pos, &huge.pos, &dr);
sub(&p->vel, &huge.vel, &dv);
drsq = length2(&dr);
dvsq = length2(&dv); /* Square of the velocity difference */
dvdr = dot(&dv, &dr);
mindist = config.radius + config.radiusHuge;
dt = (dvdr + sqrt(dvdr*dvdr + dvsq*(mindist * mindist - drsq))) / dvsq;
if (fabs(dt) > 2 * config.timeStep)
return;
scale(&p->vel, -dt, &dx1);
scale(&huge.vel, -dt, &dx2);
add(&p->pos, &dx1, &p->pos);
add(&huge.pos, &dx2, &huge.pos);
normalize(&dr, &d);
scale(&huge.vel, config.massHuge, &phuge);
add(&p->vel, &phuge, &comv);
scale(&comv, 1.0/(1 + config.massHuge), &comv);
sub(&p->vel, &comv, &comv1);
dvt1 = dot(&comv1, &d);
scale(&d, -2*dvt1, &dv1);
scale(&d, 2*dvt1/config.massHuge, &dv2);
scale(&dv1, dt, &dx1);
scale(&dv2, dt, &dx2);
add(&p->pos, &dx1, &p->pos);
add(&p->vel, &dv1, &p->vel);
add(&huge.pos, &dx2, &huge.pos);
add(&huge.vel, &dv2, &huge.vel);
return;
}
/* Calculate Pearson correlation coefficient */
double pearson_correlation_coefficient(double* vectX, double* vectY, size_t dim)
{
double sumXX = length2(vectX, dim);
double sumYY = length2(vectY, dim);
double sumX = sum(vectX, dim);
double sumY = sum(vectY, dim);
double N = (double) dim;
double d = (dot_product(vectX, vectY, dim) - (sumX * sumY) / N);
double f = sqrt((sumXX - sumX * sumX / N) * (sumYY - sumY * sumY / N));
return d / f;
}
static void kineticHelper(Particle *p, void *data)
{
assert(isSaneVector(p->vel));
double *twiceK = (double*) data;
*twiceK += p->m * length2(p->vel);
}
// ---------------------------------------------------------------------------
//
// ---------------------------------------------------------------------------
//
void CAiwImagePrintIf::ConstructL()
{
TFileName file( KResource );
file = PathInfo::RomRootPath();
TBuf<KResource> length2(KResourceFile);
file.SetLength(KDriver + length2.Length());
file.Replace(KDriver, length2.Length(), KResourceFile);
BaflUtils::NearestLanguageFile( iEikEnv.FsSession(), file );
iResourceOffset = iEikEnv.AddResourceFileL( file );
iDRM = DRMCommon::NewL();
User::LeaveIfError( iDRM->Connect() );
iNumberOfUnSuppFiles = 0;
TFileName printNameFile( KResource );
printNameFile = PathInfo::PhoneMemoryRootPath();
TBuf<KResource> length3(KParamFile);
printNameFile.SetLength(KDriver + length3.Length());
printNameFile.Replace(KDriver, length3.Length(), KParamFile);
iPrintFileName = HBufC::NewL(printNameFile.Length() );
iPrintFileName->Des().Copy(printNameFile);
TFileName unSuppFile( KResource );
unSuppFile = PathInfo::PhoneMemoryRootPath();
TBuf<KResource> lengthUn(KUnSuppFile);
unSuppFile.SetLength(KDriver + lengthUn.Length());
unSuppFile.Replace(KDriver, lengthUn.Length(), KUnSuppFile);
iUnsuppFileName = HBufC::NewL(unSuppFile.Length() );
iUnsuppFileName->Des().Copy(unSuppFile);
}
/* p is the particle we're checking, ps is the first particle in the box */
static Particle *collideWith(const Particle *p, Particle *ps)
{
Particle *other = ps;
Vec3 diff;
const float r = config.radius;
if (ps == NULL)
return NULL;
do
{
if (p == other)
{
other = other->next;
continue;
}
sub(&p->pos, &other->pos, &diff);
if (length2(&diff) < 4*r*r)
return other;
other = other->next;
} while (other != ps);
return NULL;
}
VEC2 *normalized2(VEC2 *vec)
{
float length;
length = length2(vec);
vec->x /= length;
vec->y /= length;
return (vec);
}
Vec3& normalize()
{
T nor2 = length2();
if (nor2 > 0) {
T invNor = 1 / sqrt(nor2);
x *= invNor, y *= invNor, z *= invNor;
}
return *this;
}
// **** vector ****
vfloat cos2vec(const vec& r1, const vec& r2) {
// cosinus of angle between vectors
// If one of vectors has zero length, it returns 2.
pvecerror("vfloat cos2vec(const vec& r1, const vec& r2)");
vfloat lr1 = length2(r1);
vfloat lr2 = length2(r2);
//mcout<<"cos2vec:\n";
//Iprintn(mcout, lr1);
//Iprintn(mcout, lr2);
if (lr1 == 0 || lr2 == 0) {vecerror = 1; return 0;}
vfloat cs = r1 * r2;
int sign = 1;
if (cs < 0) sign = -1;
cs = cs * cs;
cs = sign * sqrt(cs / (lr1 * lr2));
//mcout<<"r1="<<r1<<"r2="<<r2<<"cos="<<cs<<'\n';
return cs;
//return r1*r2/(lr1*lr2);
}
static inline void quaternionTFToMsg(const geometry_msgs::Quaternion& bt, geometry_msgs::Quaternion& msg) {
if (fabs(length2(bt) - 1 ) > QUATERNION_TOLERANCE) {
nh.loginfo("TF to MSG: Quaternion Not Properly Normalized");
geometry_msgs::Quaternion bt_temp = bt;
bt_temp = normalize(bt_temp);
msg.x = bt_temp.x; msg.y = bt_temp.y; msg.z = bt_temp.z; msg.w = bt_temp.w;
} else {
msg.x = bt.x; msg.y = bt.y; msg.z = bt.z; msg.w = bt.w;
}
};
void TestCppTools::Quantity_test()
{
Length length1(1.);
Length length2(length1 * 5);
Length length3;
length3 = length2;
//qDebug() << length1 << length2;
QCOMPARE(sizeof(Length), sizeof(double));
}
inline constexpr Vector3<T> normalize(const Vector3<T>& v) noexcept
{
T magSq = length2(v);
if (magSq > 0.0f)
{
T invSqrt = 1.0f / std::sqrt(magSq);
return v * invSqrt;
}
return v;
}
void OBB::computeAxes(void)
{
axis[0] = corner[1] - corner[0];
axis[1] = corner[3] - corner[0];
// Make the length of each axis 1/edge length so we know any
// dot product must be less than 1 to fall within the edge.
for (int a = 0; a < 2; ++a) {
axis[a] /= length2(axis[a]);
origin[a] = dot(corner[0],axis[a]);
}
}
glm::mat4 alignZAxisWithTarget( vec3 targetDir, vec3 upDir )
{
// Ensure that the target direction is non-zero.
if( length2( targetDir ) == 0 )
targetDir = vec3( 0, 0, 1 );
// Ensure that the up direction is non-zero.
if( length2( upDir ) == 0 )
upDir = vec3( 0, 1, 0 );
// Check for degeneracies. If the upDir and targetDir are parallel
// or opposite, then compute a new, arbitrary up direction that is
// not parallel or opposite to the targetDir.
if( length2( cross( upDir, targetDir ) ) == 0 ) {
upDir = cross( targetDir, vec3( 1, 0, 0 ) );
if( length2( upDir ) == 0 )
upDir = cross( targetDir, vec3( 0, 0, 1 ) );
}
// Compute the x-, y-, and z-axis vectors of the new coordinate system.
vec3 targetPerpDir = cross( upDir, targetDir );
vec3 targetUpDir = cross( targetDir, targetPerpDir );
// Rotate the x-axis into targetPerpDir (row 0),
// rotate the y-axis into targetUpDir (row 1),
// rotate the z-axis into targetDir (row 2).
vec3 row[3];
row[0] = normalize( targetPerpDir );
row[1] = normalize( targetUpDir );
row[2] = normalize( targetDir );
const float v[16] = { row[0].x, row[0].y, row[0].z, 0,
row[1].x, row[1].y, row[1].z, 0,
row[2].x, row[2].y, row[2].z, 0,
0, 0, 0, 1 };
return glm::make_mat4( v );
}
ItemPtr getClosestItem(Ray ray) {
ItemPtr closestItem = NULL;
real distance2 = 1e300;
for(int i=0;i<itemNumber;i++) {
Vector intersection = getIntersectPoint(&items[i], ray);
real cur2 = length2(intersection);
if(cur2 != 0 && cur2 < distance2) {
distance2 = cur2;
closestItem = &items[i];
}
}
return closestItem;
}
Vector getNormal(ItemPtr item, Ray ray, Vector point) {
switch(item->type) {
case SPHERE:
case LIGHT:
if(length2(ray.from-item->center)<item->radius * item->radius) {
//inside!
return fast_normalize(item->center-point);
}
return fast_normalize(point-item->center);
case CHECKER:
return item->normal;
default:
return ZERO;
}
}
void TestStyles::testCopyParagraphStyle()
{
QTextLength length1(QTextLength::FixedLength, 10.0);
QTextLength length2(QTextLength::FixedLength, 20.0);
QTextLength length3(QTextLength::FixedLength, 30.0);
KoParagraphStyle style1;
KoParagraphStyle style2;
style2.setParentStyle(&style1);
style1.setLeftMargin(length1);
style1.setRightMargin(length3);
style2.setRightMargin(length2);
KoParagraphStyle newStyle;
newStyle.copyProperties(&style2);
QCOMPARE(newStyle.leftMargin(), 10.0);
QCOMPARE(newStyle.rightMargin(), 20.0);
}
static void Fstack(Particle *p1, Particle *p2, int monomerDistance)
{
assert(isBase(p1->type) && isBase(p2->type));
if (!interactions.enableStack)
return;
Vec3 r = nearestImageVector(p1->pos, p2->pos);
double rSq = length2(r);
if (rSq > truncationLenSq)
return;
double rEqSq = neighbourStackDistance2(p1->type, p2->type,
monomerDistance);
double FperDist = calcFLJperDistance(STACK_COUPLING, rEqSq, rSq);
Vec3 F = scale(r, FperDist);
debugVectorSanity(F, "Fstack");
p1->F = add(p1->F, F);
p2->F = sub(p2->F, F);
}
static void Fexclusion(Particle *p1, Particle *p2)
{
if (!interactions.enableExclusion)
return;
if (!feelExclusion(p1, p2))
return;
double cutOffSq = getExclusionCutOff2(p1->type, p2->type);
Vec3 r = nearestImageVector(p1->pos, p2->pos);
double rSq = length2(r);
if (rSq > cutOffSq)
return;
double FperDist = calcFLJperDistance(EXCLUSION_COUPLING, cutOffSq, rSq);
Vec3 F = scale(r, FperDist);
debugVectorSanity(F, "Fexclusion");
p1->F = add(p1->F, F);
p2->F = sub(p2->F, F);
}
Vector getIntersectPointSphere(ItemPtr item, Ray ray) {
Vector centerRay = item->center - ray.from;
real dotValue = dot(ray.ray, centerRay);
if(dotValue<0) {
//behind
return ZERO;
}
//http://en.wikipedia.org/wiki/Line%E2%80%93sphere_intersection
real discriminant = dotValue*dotValue - length2(centerRay) + item->radius*item->radius;
if(discriminant<0) {
//does not intersect
return ZERO;
}
discriminant=sqrt(discriminant);
real close = dotValue - discriminant;
real further = dotValue + discriminant;
return ray.ray * (close > 0 ? close : further);
}
virtual float3 sample_direct(UniformRandomGenerator & gen, const float3 & P, float3 & Wi, float & pdf) const override final
{
if (dot(quad->normal, P - quad->base) <= 0.0f)
return float3(0, 0, 0);
float2 xi = { gen.random_float(), gen.random_float() };
const float3 q = quad->base + xi.x*quad->edge0 + xi.y*quad->edge1;
float3 pt = q - P;
float rSq = length2(pt);
const float distance = std::sqrt(rSq);
pt /= distance;
const float cosTheta = -dot(quad->normal, pt);
Wi = pt;
pdf = rSq / (cosTheta * quad->area);
return intensity * (1.f / (distance)); // linear
}
static Particle *collides(const Particle *p)
{
Particle *other;
Box *b;
int ix, iy, iz;
int nx, ny, nz;
#ifdef BROWNIAN
const float d = config.radius + config.radiusHuge;
Vec3 dhuge;
sub(&p->pos, &huge.pos, &dhuge);
if (length2(&dhuge) < d*d)
return &huge;
#endif
nx = p->pos.x / config.boxSize;
ny = p->pos.y / config.boxSize;
nz = p->pos.z / config.boxSize;
b = boxFromIndex(nx, ny, nz);
other = collideWith(p, b->p);
if (other != NULL)
return other;
for (ix = -1; ix <= 1; ix++)
for (iy = -1; iy <= 1; iy++)
for (iz = -1; iz <= 1; iz++)
{
if (ix == 0 && iy == 0 && iz == 0)
continue;
b = boxFromIndex(nx+ix, ny+iy, nz+iz);
other = collideWith(p, b->p);
if (other != NULL)
return other;
}
return NULL;
}
void TestStyles::testApplyParagraphStyleWithParent()
{
KoParagraphStyle style1;
style1.setStyleId(1002);
KoParagraphStyle style2;
style2.setStyleId(1003);
KoParagraphStyle style3;
style3.setStyleId(1004);
style3.setParentStyle(&style2);
style2.setParentStyle(&style1);
style1.setAlignment(Qt::AlignRight);
QCOMPARE(style1.alignment(), Qt::AlignRight);
QCOMPARE(style2.alignment(), Qt::AlignRight);
QCOMPARE(style3.alignment(), Qt::AlignRight);
style2.setAlignment(Qt::AlignCenter);
QCOMPARE(style1.alignment(), Qt::AlignRight);
QCOMPARE(style2.alignment(), Qt::AlignCenter);
QCOMPARE(style3.alignment(), Qt::AlignCenter);
style3.setAlignment(Qt::AlignLeft | Qt::AlignAbsolute);
QCOMPARE(style1.alignment(), Qt::AlignRight);
QCOMPARE(style2.alignment(), Qt::AlignCenter);
QCOMPARE(style3.alignment(), Qt::AlignLeft | Qt::AlignAbsolute);
style3.setLineSpacing(23.45);
style3.setLineHeightPercent(150);
style3.setLineHeightAbsolute(8.0);
QCOMPARE(style3.lineHeightPercent(), 0.0);
QCOMPARE(style3.lineHeightAbsolute(), 8.0);
QCOMPARE(style3.lineSpacing(), 23.45);
QVERIFY(!style3.hasNormalLineHeight());
style3.setNormalLineHeight();
QCOMPARE(style3.lineHeightPercent(), 0.0);
QCOMPARE(style3.lineHeightAbsolute(), 0.0);
QCOMPARE(style3.lineSpacing(), 0.0);
QVERIFY(style3.hasNormalLineHeight());
style3.setLineHeightPercent(150);
style3.setLineSpacing(56.78);
QCOMPARE(style3.lineHeightPercent(), 150.0);
QCOMPARE(style3.lineHeightAbsolute(), 0.0);
QCOMPARE(style3.lineSpacing(), 56.78);
QVERIFY(!style3.hasNormalLineHeight());
QTextLength length0(QTextLength::FixedLength, 0.0);
QTextLength length1(QTextLength::FixedLength, 10.0);
QTextLength length2(QTextLength::FixedLength, 20.0);
style1.setLeftMargin(length1);
QCOMPARE(style1.leftMargin(), 10.0);
QCOMPARE(style2.leftMargin(), 10.0);
QCOMPARE(style3.leftMargin(), 10.0);
style2.setRightMargin(length2);
QCOMPARE(style1.rightMargin(), 0.0);
QCOMPARE(style2.rightMargin(), 20.0);
QCOMPARE(style3.rightMargin(), 20.0);
// now actually apply it.
QTextBlockFormat rawFormat;
style1.applyStyle(rawFormat);
KoParagraphStyle format(rawFormat, rawFormat.toCharFormat());
QCOMPARE(rawFormat.properties().count(), 4);
QCOMPARE(format.alignment(), Qt::AlignRight);
QCOMPARE(rawFormat.property(KoParagraphStyle::StyleId).toInt(), 1002);
//since we have not specified any NextStyle it should be the same as the current style
QCOMPARE(rawFormat.property(KoParagraphStyle::StyleId).toInt(), rawFormat.property(KoParagraphStyle::NextStyle).toInt());
QCOMPARE(format.leftMargin(), 10.0);
QCOMPARE(format.rightMargin(), 0.0);
style2.applyStyle(rawFormat);
KoParagraphStyle format2(rawFormat, rawFormat.toCharFormat());
QCOMPARE(rawFormat.properties().count(), 5);
QCOMPARE(format2.alignment(), Qt::AlignCenter);
QCOMPARE(rawFormat.property(KoParagraphStyle::StyleId).toInt(), 1003);
//since we have not specified any NextStyle it should be the same as the current style
QCOMPARE(rawFormat.property(KoParagraphStyle::StyleId).toInt(), rawFormat.property(KoParagraphStyle::NextStyle).toInt());
QCOMPARE(format2.leftMargin(), 10.0);
QCOMPARE(format2.rightMargin(), 20.0);
style3.applyStyle(rawFormat);
KoParagraphStyle format3(rawFormat, rawFormat.toCharFormat());
QCOMPARE(rawFormat.properties().count(), 9);
QCOMPARE(rawFormat.property(KoParagraphStyle::LineSpacing).toReal(), 56.78);
QCOMPARE(format3.alignment(), Qt::AlignLeft | Qt::AlignAbsolute);
QCOMPARE(rawFormat.property(KoParagraphStyle::StyleId).toInt(), 1004);
//since we have not specified any NextStyle it should be the same as the current style
QCOMPARE(rawFormat.property(KoParagraphStyle::StyleId).toInt(), rawFormat.property(KoParagraphStyle::NextStyle).toInt());
QCOMPARE(format3.leftMargin(), 10.0);
QCOMPARE(format3.rightMargin(), 20.0);
}
/* Derivative of the dihedral potential. This shit gets quite involved.
*
* We want to compute
* Fi = -grad_ri V(r1, r2, r3, r4)
* Fi the force on the i'th particle and rj the position of the j'th
* particle.
*
* We can rather easily write V as a function of
* A = r12 x r23 = (r2 - r1) x (r3 - r2)
* and
* B = r23 x r34 = (r3 - r2) x (r4 - r3)
*
* Indeed, we have:
* V = DIHEDRAL_COUPLING * (1 - cos(phi - phi0))
* where
* phi = acos(A dot B / |A| * |B|)
*
* We can then write Fi as:
* Fi = -grad_ri Vi(A(r1, r2, r3), B(r2, r3, r4))
* and use the appropriate chain rule:
* Fi = - [J_ri(A)]^t * (grad_A V)
* - [J_ri(B)]^t * (grad_B V)
* where [J_ri(X)]^t is the transpose of the Jacobian matrix of the vector
* function X with regards to ri.
*
* After some calculus and algebra, we get
* grad_A V = DIHEDRAL_COUPLING
* * [ sin(phi0)/sin(phi) * (A dot B)/(|A|^2 |B|^2)
* - cos(phi0) / (|A||B|) ]
* * [B - A (A dot B) / (|B||A|)]
* and due to the symmetry in V of A and B, grad_B V is exactly the same,
* but with A and B interchanged.
* grad_B V = DIHEDRAL_COUPLING
* * [ sin(phi0)/sin(phi) * (A dot B)/(|A|^2 |B|^2)
* - cos(phi0) / (|A||B|) ]
* * [A - B (A dot B) / (|B||A|)]
*
*
* NOTE: when debugging the dihedral force for energy conservation, you
* also need to enable angle and bond interactions (which should be
* debugged first), otherwise you get a badly conditioned problem and you
* will experience blow up.
*/
static void Fdihedral(Particle *p1, Particle *p2, Particle *p3, Particle *p4,
DihedralCache phi0)
{
if (!interactions.enableDihedral)
return;
Vec3 r12 = nearestImageVector(p1->pos, p2->pos);
Vec3 r23 = nearestImageVector(p2->pos, p3->pos);
Vec3 r34 = nearestImageVector(p3->pos, p4->pos);
double sinPhi, cosPhi; //TODO find better algorithm to only compute sinPhi
sinCosDihedral(r12, r23, r34, &sinPhi, &cosPhi);
if (UNLIKELY(fabs(sinPhi) < 1e-5)) //TODO just check for ==0? -> saves an fabs!
return; /* (Unstable) equilibrium. */
double sinPhi0 = phi0.sinDihedral;
double cosPhi0 = phi0.cosDihedral;
Vec3 A = cross(r12, r23);
Vec3 B = cross(r23, r34);
double lAl2 = length2(A);
double lBl2 = length2(B);
double lAl2lBl2 = lAl2 * lBl2;
double lAllBl = sqrt(lAl2lBl2);
double AdB = dot(A, B);
double negGradPrefactor = DIHEDRAL_COUPLING * (cosPhi0 / lAllBl
- sinPhi0/sinPhi * AdB/lAl2lBl2);
/* -grad_A(V) */
Vec3 negGrad_AV = scale(
sub(B, scale(A, AdB/lAl2)),
negGradPrefactor);
/* -grad_B(V) */
Vec3 negGrad_BV = scale(
sub(A, scale(B, AdB/lBl2)),
negGradPrefactor);
/* F1 */
Mat3 J_r1A = crossProdTransposedJacobian(r12, r23, 1); /* [J_r1(A)]^t */
/* [J_r1(B)]^t = 0*/
Vec3 F1 = matApply(J_r1A, negGrad_AV);
/* F2 */
Mat3 J_r2A = crossProdTransposedJacobian(r12, r23, 2); /* [J_r2(A)]^t */
Mat3 J_r2B = crossProdTransposedJacobian(r23, r34, 1); /* [J_r2(B)]^t */
Vec3 F2 = add(matApply(J_r2A, negGrad_AV), matApply(J_r2B, negGrad_BV));
/* F4 (not F3 because that has two non-zero jacobi matrices,
* whereas F4 has only one non-zero jacobi matrix, so it's
* faster to compute)*/
/* [J_r4(A)]^t = 0*/
Mat3 J_r4B = crossProdTransposedJacobian(r23, r34, 3); /* [J_r4(B)]^t */
Vec3 F4 = matApply(J_r4B, negGrad_BV);
/* -F3 */
Vec3 negF3 = add(add(F1, F2), F4);
p1->F = add(p1->F, F1);
p2->F = add(p2->F, F2);
p3->F = sub(p3->F, negF3);
p4->F = add(p4->F, F4);
}
inline float length(const vec2& v) {
return sqrtf(length2(v));
}
