VerdictVector vectorRotate(const double angle,
const VerdictVector &normalAxis,
const VerdictVector &referenceAxis)
{
// A new coordinate system is created with the xy plane corresponding
// to the plane normal to the normal axis, and the x axis corresponding to
// the projection of the reference axis onto the normal plane. The normal
// plane is the tangent plane at the root point. A unit vector is
// constructed along the local x axis and then rotated by the given
// ccw angle to form the new point. The new point, then is a unit
// distance from the global origin in the tangent plane.
double x, y;
// project a unit distance from root along reference axis
VerdictVector yAxis = normalAxis * referenceAxis;
VerdictVector xAxis = yAxis * normalAxis;
yAxis.normalize();
xAxis.normalize();
x = cos(angle);
y = sin(angle);
xAxis *= x;
yAxis *= y;
return VerdictVector(xAxis + yAxis);
}
/*!
moves and rotates the quad such that it enables us to
use components of ef's
*/
void localize_quad_for_ef( VerdictVector node_pos[4])
{
VerdictVector centroid(node_pos[0]);
centroid += node_pos[1];
centroid += node_pos[2];
centroid += node_pos[3];
centroid /= 4.0;
node_pos[0] -= centroid;
node_pos[1] -= centroid;
node_pos[2] -= centroid;
node_pos[3] -= centroid;
VerdictVector rotate = node_pos[1] + node_pos[2] - node_pos[3] - node_pos[0];
rotate.normalize();
double cosine = rotate.x();
double sine = rotate.y();
double xnew;
for (int i=0; i < 4; i++)
{
xnew = cosine * node_pos[i].x() + sine * node_pos[i].y();
node_pos[i].y( -sine * node_pos[i].x() + cosine * node_pos[i].y() );
node_pos[i].x(xnew);
}
}
double VerdictVector::vector_angle(const VerdictVector &vector1,
const VerdictVector &vector2) const
{
// This routine does not assume that any of the input vectors are of unit
// length. This routine does not normalize the input vectors.
// Special cases:
// If the normal vector is zero length:
// If a new one can be computed from vectors 1 & 2:
// the normal is replaced with the vector cross product
// else the two vectors are colinear and zero or 2PI is returned.
// If the normal is colinear with either (or both) vectors
// a new one is computed with the cross products
// (and checked again).
// Check for zero length normal vector
VerdictVector normal = *this;
double normal_lensq = normal.length_squared();
double len_tol = 0.0000001;
if( normal_lensq <= len_tol )
{
// null normal - make it the normal to the plane defined by vector1
// and vector2. If still null, the vectors are colinear so check
// for zero or 180 angle.
normal = vector1 * vector2;
normal_lensq = normal.length_squared();
if( normal_lensq <= len_tol )
{
double cosine = vector1 % vector2;
if( cosine > 0.0 ) return 0.0;
else return VERDICT_PI;
}
}
//Trap for normal vector colinear to one of the other vectors. If so,
//use a normal defined by the two vectors.
double dot_tol = 0.985;
double dot = vector1 % normal;
if( dot * dot >= vector1.length_squared() * normal_lensq * dot_tol )
{
normal = vector1 * vector2;
normal_lensq = normal.length_squared();
//Still problems if all three vectors were colinear
if( normal_lensq <= len_tol )
{
double cosine = vector1 % vector2;
if( cosine >= 0.0 ) return 0.0;
else return VERDICT_PI;
}
}
else
{
//The normal and vector1 are not colinear, now check for vector2
dot = vector2 % normal;
if( dot * dot >= vector2.length_squared() * normal_lensq * dot_tol )
{
normal = vector1 * vector2;
}
}
// Assume a plane such that the normal vector is the plane's normal.
// Create yAxis perpendicular to both the normal and vector1. yAxis is
// now in the plane. Create xAxis as the perpendicular to both yAxis and
// the normal. xAxis is in the plane and is the projection of vector1
// into the plane.
normal.normalize();
VerdictVector yAxis = normal;
yAxis *= vector1;
double yv = vector2 % yAxis;
// yAxis memory slot will now be used for xAxis
yAxis *= normal;
double xv = vector2 % yAxis;
// assert(x != 0.0 || y != 0.0);
if( xv == 0.0 && yv == 0.0 )
{
return 0.0;
}
double angle = atan2( yv, xv );
if (angle < 0.0)
{
angle += TWO_VERDICT_PI;
}
return angle;
}
