void CoreNode::look(const Vector& target, const Vector& up) {
Vector zaxis = (target - position_).unit();
Vector xaxis = (up.cross(zaxis)).unit();
Vector yaxis = (zaxis.cross(xaxis)).unit();
rotation(Quaternion(xaxis, yaxis, zaxis));
}
void Camera::SetPOVLookAt( const Vector& vCameraPosition, const Vector& vTarget, Vector vUp )
{
Vector vZBasis = (vCameraPosition - vTarget).normalized();
// if the zero vector was passed try to preserve the current camera up as much as possible
if ( vUp == Vector::zero() )
{
vUp = m_mtxPOV.yBasis;
}
else
{
vUp = vUp.normalized();
}
// if the z direction is parallel to the desired up direction
// we need to pick another up.
if ( IsNearZero(1.0f - fabs(vZBasis.dot(vUp))) )
{
// make the new up perpendicular to the plane of the new z axis and the old right direction.
// it's a decent guess.
vUp = vZBasis.cross( m_mtxPOV.xBasis ).normalized();
}
Vector vXBasis = vUp.cross(vZBasis).normalized();
Vector vYBasis = vZBasis.cross( vXBasis ).normalized();
m_mtxPOV.xBasis = vXBasis;
m_mtxPOV.yBasis = vYBasis;
m_mtxPOV.zBasis = vZBasis;
m_mtxPOV.origin = vCameraPosition;
updateOrbitDistance();
}
void Player::Wheel::constrainDirection(const Vector& z) {
if (!friction()) {
angle += angleStep;
return; // no normal force, no friction
}
/* Don't let upwards-moving platforms launch us into the air. */
Vector cv = contactVelocity;
if (cv.y > 0) {
cv.y = 0.f;
}
/* Cancel the sideways component of the velocity. */
Vector v = position - previousPosition - cv;
v -= z * z.dot(v);
previousPosition = position - v - cv;
/* Rotate the wheel. */
Vector forward = z.cross(contactNormal);
Vector vForward = forward * forward.dot(v);
float l = vForward.length();
if (vForward.cross(contactNormal).dot(z) > 0) {
l *= -1;
}
angleStep = 360.f * l / (M_PI * 2 * wheelRadius);
angle += angleStep;
}
// Pulled from Parallax
void Matrix::lookAt(const Vector & eye, const Vector & center, const Vector & upi) {
Vector forward = (center - eye).normalize();
// Side = forward x up
Vector side = (forward.cross(upi)).normalize();
// Recompute up as: up = side x forward
Vector up = side.cross(forward);
// Put the initial transformation in a tmp matrix
Matrix mult; mult.identity();
mult.matrix[0][0] = side.x;
mult.matrix[1][0] = side.y;
mult.matrix[2][0] = side.z;
mult.matrix[0][1] = up.x;
mult.matrix[1][1] = up.y;
mult.matrix[2][1] = up.z;
mult.matrix[0][2] = -forward.x;
mult.matrix[1][2] = -forward.y;
mult.matrix[2][2] = -forward.z;
// Translate us
translate(-eye);
// And finish recomputing
*this = (*this) * mult;
}
Color Ray::glossyReflectionShade(Ray& reflectedRay, const Vector& normal, const ObjektConstPtr& object, const double glossy, const int glossySamples, const std::vector<ObjektConstPtr>& objects, const std::vector<LightConstPtr>& lights, const Color& background, const Color& globalAmbient)
{
static std::default_random_engine generator;
static uniform_real_distribution<double> distribution(0, 1);
static auto random = std::bind(distribution, generator);
// glossy reflection
const Vector reflZ = reflectedRay.getDirection();
const Vector reflX = reflZ.cross(normal).normalize();
const Vector reflY = reflZ.cross(reflX).normalize();
AccumulatedColor accMirrColor;
for (int i = 0; i < glossySamples; i++)
{
const double angle = random() * 2 * M_PI;
const double radius = random() * glossy;
const Vector r = reflZ
.vadd(reflX.svmpy(radius*cos(angle)))
.vadd(reflY.svmpy(radius*sin(angle)));
// if reflected ray intersects with object, skip it
if (normal.dot(r) < 0)
continue;
reflectedRay.setDirection(r.normalize());
const Color clr = reflectedRay.shade(objects, lights, background, globalAmbient);
accMirrColor.addcolor(clr);
}
return accMirrColor.getColor();
}
void Camera::rotate( float x, float y )
{
Vector po = getVSpherePos((float) m_LastMouseX, m_LastMouseY);
Vector pn = getVSpherePos(x, y);
if((po-pn).lengthSquared() < 0.0001f )
return;
float cosangle = po.dot(pn);
if(cosangle>1.0f) cosangle=1.0f;
if(cosangle<-1.0f) cosangle=-1.0f;
const float angle = acos(cosangle);
Vector RotAxis = pn.cross(po);
RotAxis.normalize();
//Vector Diff = m_Position-m_Target;
Vector Diff(0,0,(m_Position-m_Target).length());
Vector RotDiff = rotateAxisAngle(Diff, RotAxis, angle);
Vector cdir = m_Target-m_Position;
cdir.normalize();
Vector cup = m_Up;
Vector cright = cdir.cross(cup);
cright.normalize();
cup = cright.cross(cdir);
Vector RotDiffW;
RotDiffW.X = cright.X * RotDiff.X + cup.X * RotDiff.Y + -cdir.X * RotDiff.Z;
RotDiffW.Y = cright.Y * RotDiff.X + cup.Y * RotDiff.Y + -cdir.Y * RotDiff.Z;
RotDiffW.Z = cright.Z * RotDiff.X + cup.Z * RotDiff.Y + -cdir.Z * RotDiff.Z;
m_Rotation = RotDiffW - (m_Position-m_Target);
}
Matrix Scene::getPerspectiveMatrix(float x, float y, float z, Vector direction, Vector up, float fovy, float aspect, float near, float far)
{
// first we set camera params
direction = direction.normalized();
Vector normal = direction.cross(up).normalized();
Vector u = normal.cross(direction).normalized();
direction = Vector(-direction.getX(), -direction.getY(), -direction.getZ());
Vector eye(x,y,z);
Matrix camera(
normal.getX(), normal.getY(), normal.getZ(), -normal.dot(eye),
u.getX(), u.getY(), u.getZ(), -u.dot(eye),
direction.getX(), direction.getY(), direction.getZ(), -direction.dot(eye),
0, 0, 0, 1
);
float f = 1.0 / tan(fovy / 2);
float d = far - near;
Matrix perspective(
f/aspect, 0.0, 0, 0,
0.0, f, 0.0, 0.0,
0.0, 0.0, -(near + far)/d, -2*near*far/d,
0.0, 0.0, -1, 0.0
);
Matrix result = perspective.mult(camera);
return result;
}
// Source: gluLookAt
Matrix Matrix::lookAt(const Vector &eye, const Vector &front, const Vector &up) {
Vector s = front.cross(up);
Vector u = s.cross(front);
Matrix result;
Float32 *dest = result.data;
dest[0] = s.x;
dest[4] = s.y;
dest[8] = s.z;
dest[12] = 0.0f;
dest[1] = u.x;
dest[5] = u.y;
dest[9] = u.z;
dest[13] = 0.0f;
dest[2] = -front.x;
dest[6] = -front.y;
dest[10] = -front.z;
dest[14] = 0.0f;
dest[3] = 0.0f;
dest[7] = 0.0f;
dest[11] = 0.0f;
dest[15] = 1.0f;
return result * translate(-eye);
}
void Camera::generateRay(Sample& sample, Ray* ray){
/*float normalX = sample.x / myWidth;
float normalY = sample.y / myHeight;
float remapX = 2 * normalX - 1;
float remapY = 2 * normalY - 1;*/
//account for aspect ratio and dof; not sure if needed
/*
float ar = myWidth / myHeight;
float alpha = 2 * atan(abs(myUL.y) / abs(myUL.z));
float cameraX = (2 * remapX - 1) * ar * tan(alpha/2);
float cameraY = (1 - 2 * remapY) * tan(alpha/2);
*/
// Calculate ray with camera data given by input file, not four corners /////////////////////
Vector dir = lookat-pos;
dir.normalize();
//cout << n.x << " " << n.y << " " << n.z << endl; //0 0 -1
Vector u = dir.cross(up);
u.normalize();
//cout << u.x << " " << u.y << " " << u.z << endl; //1 0 0
Vector v = u.cross(dir);
v.normalize();
//cout << v.x << " " << v.y << " " << v.z << endl; //0 1 0
Vector xinc = u*2*tan(fov*M_PI/180/2)/pixWidth * pixWidth/pixHeight;
//cout << xinc.x << " " << xinc.y << " " << xinc.z << endl;
Vector yinc = v*2*tan(fov*M_PI/180/2)/pixHeight;
//cout << yinc.x << " " << yinc.y << " " << yinc.z << endl;
Vector vec = dir + yinc*0.5*(2*sample.y+1-pixHeight) + xinc*0.5*(2*sample.x+1-pixWidth);
//cout << vec.x << " " << vec.y << " " << vec.z << endl;
/*if (pixWidth > pixHeight)
{
vec.x = vec.x*((float)pixWidth/pixHeight);
}
else if(pixHeight > pixWidth)
{
vec.y = vec.y*((float)pixHeight/pixWidth);
}*/
//vec.x = vec.x*pixWidth/pixHeight;
vec.normalize();
//cout << vec.x << " " << vec.y << " " << vec.z << endl;
*ray = Ray(pos, vec, 1, 100);
///////////////////////////////////////////////////////////////////////////////////////////
/*Point camSpace = Point(remapX, remapY, -3);
Vector dir = camSpace - myPos;
dir.normalize();
*ray = Ray(myPos, dir, 1, 100);*/
}
//******************************************************************************
Matrix Matrix::buildRotation( const Vector & rkFrom, const Vector & rkTo )
{
// compute dot product between From and To
Real fCosAngle = rkFrom.dot( rkTo );
Real fEpsilon = 10e-4;
if ( Abs( fCosAngle - 1 ) < fEpsilon ) /// dot product close to 1.0
{
// rotate by an angle of zero with an arbitrary direction
return buildRotation( 0.0, 1 ); // use y axis
}
else
if ( Abs( fCosAngle + 1.0 ) < fEpsilon ) // dot product close to 0.0
{
// find an orthogonal direction to create axis
Vector kOrtho;
if ( Abs( rkFrom.x() ) < Abs( rkFrom.y() ) )
{
if ( Abs( rkFrom.x() ) < Abs( rkFrom.z() ) )
{
kOrtho = Vector( 1.0, 0.0, 0.0 ); // use x axis.
}
else
{
kOrtho = Vector( 0.0, 0.0, 1.0 ); // use z axis
}
}
else
if ( Abs( rkFrom.y() ) < Abs( rkFrom.z() ) )
{
kOrtho = Vector( 0.0, 1.0, 0.0 ); // use y axis
}
else
{
kOrtho = Vector( 0.0, 0.0, 1.0 ); // use z axis
}
// find orthogonal axis to use for rotation
Vector kAxis( rkFrom.cross( kOrtho ) ); // Axis = From x Ortho (cross prod)
kAxis.normalize();
Real fAngle = ArcCosine( fCosAngle ); // find rotation angle
// create new rotation matrix about new axis
return buildRotation( fAngle, kAxis );
}
else
{
// find orthogonal axis to use for rotation
Vector kAxis( rkFrom.cross( rkTo ) ); // Axis = From x To (cross prod)
Real fAngle = ArcCosine( fCosAngle ); // find rotation angle
// create new rotation matrix about new axis
return buildRotation( fAngle, kAxis );
}
}
void Camera::pan( float dx, float dy)
{
// calculate panning-plane
Vector aDir = m_Target-m_Position;
aDir.normalize();
Vector aRight = aDir.cross(m_Up);
aRight.normalize();
Vector aUp = aDir.cross(aRight);
m_Panning = aRight * dx + aUp * dy;
}
bool Tracer::scatterDiffuse(const IntersectDescr& node, Ray& ray) const
{
const Material& material = node.shape_->getMaterial();
const double max_comp = std::max(
material.getColor()[ 0 ],
std::max( material.getColor()[ 1 ], material.getColor()[ 2 ] ) );
if( max_comp <= 1e-4 )
{
return false;
}
const Vector w_vec = node.normal_;
Vector u_vec = std::abs( w_vec[ 0 ] ) > .1 ? Vector( 0., 1., 0. )
: Vector( 1., 0., 0. );
u_vec = u_vec.cross( w_vec ).normalized();
const Vector v_vec = w_vec.cross( u_vec ).normalized();
const double theta = 2. * ::PI * uniform_distrib_( rand_gen_ );
const double radius = uniform_distrib_( rand_gen_ );
const double radius_sqr = std::sqrt( radius );
ray = Ray( node.point_,
( u_vec * std::cos( theta ) * radius_sqr +
v_vec * std::sin( theta ) * radius_sqr +
w_vec * std::sqrt( 1. - radius ) )
.normalized() );
return true;
}
Matrix Matrix::look(Vector const& pos, Vector const& at, Vector const& sky) {
Vector forward = (pos - at).unit();
Vector right = sky.unit().cross(forward);
Vector up = forward.cross(right);
return Matrix::translate(pos) * Matrix::rotate(right, up, forward);
/*
//INVERSE:
Vector forward = (pos - at).unit();
Vector right = sky.unit().cross(forward);
Vector up = forward.cross(right);
Matrix data;
data[0] = right.x;
data[1] = up.x;
data[2] = forward.x;
data[3] = 0;
data[4] = right.y;
data[5] = up.y;
data[6] = forward.y;
data[7] = 0;
data[8] = right.z;
data[9] = up.z;
data[10] = forward.z;
data[11] = 0;
return data * Matrix::translate(-pos);*/
}
Matrix Matrix::look(Vector const& direction) {
Vector forward = direction.unit();
Vector up = forward.orthogonal();
Vector right = up.cross(forward);
return Matrix::rotate(right, up, forward);
}
/*
====================
Tube::intersect
Computes intersection between the Tube and the ray, and returns itself if
it is hit or NULL if it is not along with the point of intersection
====================
*/
SceneObject* Tube::intersect(Ray* r, Point &intersect) {
Vector dist = this->origin - r->start;
//norm(dist);
//norm(r->dir);
//double a = dot3(r->dir,r->dir);
//double b = 2 * dot3(r->start - this->origin,r->dir);
//double c = dot3(r->start - this->origin,r->start - this->origin) - this->radius * this->radius;
//double disc = discrim(a,b,c);
////Parallel to tube, does not intersect
//if(dot3(r->dir,this->up) == 0) return false;
//else if(disc >= 0) { //Find closest intersection
// double discSqrt = sqrt(disc);
// double quad;
// if (b < 0) quad = (-b - discSqrt)/2.f;
// else quad = (-b + discSqrt)/2.f;
// double t0 = quad/a;
// double t1 = c/quad;
// if(t0 > t1) swap(t0,t1);
// double t;
// if(t0 < 0 && t1 < 0) return false;
// if(t0 < 0) t = t1;
// else t = t0;
// intersect = r->start + t * r->dir;
// return this;
//}
//return NULL;
/*
Ray: O + V * t
Cylinder: [this->origin,this->up,this->radius]
A = this->origin
O = r->start
V = r->dir
*/
Vector AB = this->up;
Vector AO = r->start - this->origin;
Vector AOxAB = AO.cross(AB);
Vector VxAB = r->dir.cross(AB);
double ab2 = AB.dot(AB);
double a = VxAB.dot(VxAB);
double b = 2 * VxAB.dot(AOxAB);
double c = AOxAB.dot(AOxAB) - this->radius*this->radius * ab2;
double t = quadratic(a,b,c);
if(t < 0) return NULL;
intersect = r->start + t * r->dir;
return this;
}
// Function to compute the unit normal vector
// Remember to output a normalised vector!
Vector Plane::normal(Vector pos, Vector src)
{
Vector bMinusA = b - a;
Vector cMinusA = c - a;
Vector n = bMinusA.cross(cMinusA);
n.normalise();
return n;
}
const AxisAngle AxisAngle::fromTwoVectors(const Vector& u, const Vector& v)
{
AxisAngle a = u.cross(v);
float norm = sqrt(u.normSquare() * v.normSquare());
if(norm != 0.0) a /= norm; // Avoid singularity if u or v is null
float sinAngle = a.norm();
float ratio = asin(sinAngle) / sinAngle;
if(ratio != ratio) ratio = 0.0; // Avoid ratio singularity
return a * ratio;
}
void
dmz::StarfighterPluginTargets::_new_ori (
const Float64 DeltaTime,
const Vector &Dir,
TargetStruct &obj,
Matrix &ori) {
Vector hvec (Dir);
hvec.set_y (0.0);
hvec.normalize_in_place ();
Float64 heading = Forward.get_angle (hvec);
Vector hcross = Forward.cross (hvec).normalize ();
if (hcross.get_y () < 0.0) { heading = TwoPi64 - heading; }
if (heading > Pi64) { heading = heading - TwoPi64; }
else if (heading < -Pi64) { heading = heading + TwoPi64; }
Float64 pitch = Dir.get_angle (hvec);
Vector pcross = Dir.cross (hvec).normalize ();
Vector ncross = hvec.cross (pcross);
if (ncross.get_y () < 0.0) { pitch = TwoPi64 - pitch; }
obj.heading = _rotate (DeltaTime, obj.heading, heading);
obj.pitch = _rotate (DeltaTime, obj.pitch, pitch);
if (is_zero64 (pitch - obj.pitch) && is_zero64 (heading - obj.heading)) {
obj.onTarget = true;
}
Matrix hm (Up, obj.heading);
Matrix pm (Right, obj.pitch);
ori = hm * pm;
}
Matrix Matrix::fromForwardVector(Vector const& forward) {
Vector const zaxis = forward.unit();
Vector const xaxis = forward.orthogonal().cross(zaxis).unit();
Vector const yaxis = zaxis.cross(xaxis).unit();
return Matrix(
xaxis.x, xaxis.y, xaxis.z, 0,
yaxis.x, yaxis.y, yaxis.z, 0,
zaxis.x, zaxis.y, zaxis.z, 0,
0, 0, 0, 1
);
}
void GetOrthonormalBasisFromNormal(const Vector<float>& normal,
Vector<float>& u, Vector<float>& v,
Vector<float>& w) {
// u-v-w - Coordinate system - w is along normal
// determine major direction:
w = normal;
int axis = 0;
float mag = fabs(w.x());
if (fabs(w.y()) > mag) {
axis = 1;
mag = fabs(w.y());
}
if (fabs(w.z()) > mag) {
axis = 2;
mag = fabs(w.z());
}
w.normalize();
if (axis != 0) {
v = w.cross(Vector<float>(1, 0, 0));
v.normalize();
u = v.cross(w);
} else if (axis != 1) {
v = w.cross(Vector<float>(0, 1, 0));
v.normalize();
u = v.cross(w);
} else {
v = w.cross(Vector<float>(0, 0, 1));
v.normalize();
u = v.cross(w);
}
}
Quaternion Vector::quat_to(const Vector r) const{
const double dot = r.normalise().dot(this->normalise());
if(dot >= 1){
return Quaternion(1,0,0,0);
} else if(dot <= -1){
return Quaternion(0,0,0,1);
} else {
const Vector axis = r.cross(*this).normalise();
const a_t theta = acos(dot);
const float sinv = sin(theta/2);
return Quaternion(cos(theta/2),sinv*axis.x,sinv*axis.y,sinv*axis.z);
}
}
void Camera<Scalar>::orbitDown(Scalar rad)
{
//first update camera position
Vector<Scalar,3> camera_direction = focus_position_ - camera_position_;
Vector<Scalar,3> axis = camera_direction.cross(camera_up_);
axis.normalize();
Quaternion<Scalar> quat(axis,rad);
camera_position_ = quat.rotate(camera_position_);
//then update up direction
camera_direction = focus_position_ - camera_position_;
camera_up_ = axis.cross(camera_direction);
camera_up_.normalize();
}
Vector Ant::getSteepestDown()
{
Vector normal = GridManager::interpolateNormal(AgentManager::getFace(agentID), position);
if(normal.x == 0 && normal.x == 0) // tangent plane is a horizontal plane
{
return Vector(0,0,0);
}
if(normal.y == 0)
{
Vector tangent = Vector(0,1,0)
}
else
{
float a = -(normal.x/normal.y);
Vector tangent = Vector(1,a,0);
}
Vector steepest = normal.cross(tangent) / normal.cross(tangent).magnitude();
// z component must be between -1 and 1
if(steepest.z > 0)
steepest = steepest * -1;
return steepest;
}
//Function to test if an input point is within the quad.
bool Plane::isInside(Vector q)
{
Vector n = normal(q);
Vector ua = b-a, ub = c-b, uc = d-c, ud = a-d;
Vector va = q-a, vb = q-b, vc = q-c, vd = q-d;
if (((ua.cross(va)).dot(n) >0) &&
((ub.cross(vb)).dot(n) >0) &&
((uc.cross(vc)).dot(n) >0) &&
((ud.cross(vd)).dot(n) >0)) return true;
return false;
//Complete this function
}
// determine the transform from view coordinates to world coordinates.
void Transform::view(const Point& eye, const Point&at, const Vector& up){
Vector ncoor = eye - at;
Vector ucoor = up.cross(ncoor);
Vector vcoor = ncoor.cross(ucoor);
ncoor.normalize();
ucoor.normalize();
vcoor.normalize();
Vector tmp = eye - Point(0.0, 0.0, 0.0, 1.0f);
float dx = -tmp.dot(ucoor);
float dy = -tmp.dot(vcoor);
float dz = -tmp.dot(ncoor);
mat_[0][0] = ucoor.x();
mat_[0][1] = ucoor.y();
mat_[0][2] = ucoor.z();
mat_[0][3] = dx;
mat_[1][0] = vcoor.x();
mat_[1][1] = vcoor.y();
mat_[1][2] = vcoor.z();
mat_[1][3] = dy;
mat_[2][0] = ncoor.x();
mat_[2][1] = ncoor.y();
mat_[2][2] = ncoor.z();
mat_[2][3] = dz;
mat_[3][0] = 0.0;
mat_[3][1] = 0.0;
mat_[3][2] = 0.0;
mat_[3][3] = 1.0f;
}
void Matrix4x4::makeViewRotation(const Vector& ahead)
{
static Vector worldUp(0, 1, 0);
Vector backward = -ahead.normalize();
Vector right = backward.cross(worldUp).normalize();
Vector up = right.cross(backward);
m[0][0] = right.x;
m[0][1] = right.y;
m[0][2] = right.z;
m[0][3] = 0;
m[1][0] = up.x;
m[1][1] = up.y;
m[1][2] = up.z;
m[1][3] = 0;
m[2][0] = backward.x;
m[2][1] = backward.y;
m[2][2] = backward.z;
m[2][3] = 0;
m[3][0] = 0;
m[3][1] = 0;
m[3][2] = 0;
m[3][3] = 1;
}
void Matrix4x4::makeView(const Vector& eye, const Vector& ahead)
{
static Vector worldUp(0, 1, 0);
Vector unitAhead = ahead.normalize();
Vector right = unitAhead.cross(worldUp).normalize();
Vector up = right.cross(unitAhead);
m[0][0] = right.x;
m[0][1] = right.y;
m[0][2] = right.z;
m[0][3] = eye.dot(Vector(right.x, up.x, unitAhead.x));
m[1][0] = up.x;
m[1][1] = up.y;
m[1][2] = up.z;
m[1][3] = eye.dot(Vector(right.y, up.y, unitAhead.y));
m[2][0] = unitAhead.x;
m[2][1] = unitAhead.y;
m[2][2] = unitAhead.z;
m[2][3] = eye.dot(Vector(right.y, up.y, unitAhead.y));
m[3][0] = 0;
m[3][1] = 0;
m[3][2] = 0;
m[3][3] = 1;
}
Vector Ant::getClimbMountainForce(){
Vector normal = GridManager::interpolateNormal(AgentManager::getFace(agentID), position);
if(normal.x==0 && normal.y==0) // tangent plane is a horizontal plane
{
return Vector(0,0,0);
}
if(normal.y == 0)
{
Vector tangent = Vector(0,1,0)
}
else
{
float a = -(normal.x/normal.y);
Vector tangent = Vector(1,a,0);
}
Vector steepest = normal.cross(tangent) / normal.cross(tangent).magnitude();
// z component must be between -1 and 1
if(steepest.z < 0)
steepest = steepest * -1;
// z component between 0 and 1, the closer to 1, the harder it should be to get up
steepest = steepest * 5* pow(steepest.z -1,2);
return steepest;
}
double angleBetween(const Vector &v1, const Vector &v2, Vector *axis) {
double squared_length1 = v1.squaredNorm();
double squared_length2 = v2.squaredNorm();
double tmp = sqrt(squared_length1 * squared_length2);
double angle = acos(v1.dot(v2) / tmp);
if (axis != NULL) {
// special case 0 and 180 degrees where the axis is ambiguous
// Note that acos returns a value in [0, M_PI] so angle is in that range
if ((angle <= EPSILON) || (M_PI - angle <= EPSILON)) {
// find an axis that isn't zero and switch it with another one
// make sure to keep the length intact
Vector dummy;
if (v1(0) > EPSILON) {
dummy = Vector(v1.y(), -v1.x(), v1.z());
} else {
dummy = Vector(v1.x(), v1.z(), -v1.y());
}
*axis = dummy.cross(v2) / tmp;
} else {
*axis = v1.cross(v2) / tmp;
}
}
return angle;
}
void Transform::reorthonormalize(void)
{
Vector k = getLocalFrameK_p();
Vector j = getLocalFrameJ_p();
if (!k.normalize())
{
DEBUG_FATAL(true, ("could not normalize frame k"));
k = Vector::unitZ; //lint !e527 // Unreachable
}
if (!j.normalize())
{
DEBUG_FATAL(true, ("could not normalize frame j"));
j = Vector::unitY; //lint !e527 // Unreachable
}
// build the remaining vector with the cross product
Vector i = j.cross(k);
// use that result to rebuild the
j = k.cross(i);
// copy the results back into the transform
matrix[0][0] = i.x;
matrix[1][0] = i.y;
matrix[2][0] = i.z;
matrix[0][1] = j.x;
matrix[1][1] = j.y;
matrix[2][1] = j.z;
matrix[0][2] = k.x;
matrix[1][2] = k.y;
matrix[2][2] = k.z;
}
