If you are having to carry out loads of check, I would shy away from using square root functions: they are computationally expensive. for comparison purposes, just multiply everything by itself and you can bypass the square rooting:
magnitude of vector = length of vector
If vector is defined as float[3] length can be calculated as follows:
double magnitude = sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] );
However that is expensive computationally so I would use
double magnitudeSquared = a[0]*a[0] + a[1]*a[1] + a[2]*a[2];
Then modify any comparative calculations to use the squared version of the distance or magnitude and it will be more performant.
For the cross product, please forgive me if this maths is shaky, it has been a couple of years since I wrote functions for this (code re-use is great but terrible for remembering things):
double c[3];
c[0] = ( a[1]*b[2] - a[2]*b[1] );
c[1] = ( a[2]*b[0] - a[0]*b[2] );
c[2] = ( a[0]*b[1] - a[1]*b[0] );
To simplify it all I would put a vec3d in a class of its own, with a very simple representation being:
class vec3d
{
public:
float x, y, z;
vec3d crossProduct(vec3d secondVector)
{
vec3d retval;
retval.x = (this.y * secondVector.z)-(secondVector.y * this.z);
retval.y = -(this.x * secondVector.z)+(secondVector.x * this.z);
retval.z = (this.x * secondVector.y)-(this.y * secondVector.x);
return retval;
}
// to get the unit vector divide by a vectors length...
void normalise() // this will make the vector into a 1 unit long variant of itself, or a unit vector
{
if(fabs(x) > 0.0001){
x= x / this.magnitude();
}
if(fabs(y) > 0.0001){
y= y / this.magnitude();
}
if(fabs(z) > 0.0001){
z = / this.magnitude();
}
}
double magnitude()
{
return sqrt((x*x) + (y*y) + (z*z));
}
double magnitudeSquared()
{
return ((x*x) + (y*y) + (z*z));
}
};
A fuller implementation of a vec3d class can be had from one of my old 2nd year coding excercises: .h file and .cpp file.
And here is a minimalist 2d implementation (doing this off the top of my head so forgive the terse code please, and let me know if there are errors):
vec2d.h
#ifndef VEC2D_H
#define VEC2D_H
#include <iostream>
using namespace std;
class Vec2D {
private:
double x, y;
public:
Vec2D(); // default, takes no args
Vec2D(double, double); // user can specify init values
void setX(double);
void setY(double);
double getX() const;
double getY() const;
double getMagnitude() const;
double getMagnitudeSquared() const;
double getMagnitude2() const;
Vec2D normalize() const;
double crossProduct(Vec2D secondVector);
Vec2D crossProduct(Vec2D secondVector);
friend Vec2D operator+(const Vec2D&, const Vec2D&);
friend ostream &operator<<(ostream&, const Vec2D&);
};
double dotProduct(const Vec2D, const Vec2D);
#endif
vec2d.cpp
#include <iostream>
#include <cmath>
using namespace std;
#include "Vec2D.h"
// Constructors
Vec2D::Vec2D() { x = y = 0.0; }
Vec2D::Vec2D(double a, double b) { x = a; y = b; }
// Mutators
void Vec2D::setX(double a) { x = a; }
void Vec2D::setY(double a) { y = a; }
// Accessors
double Vec2D::getX() const { return x; }
double Vec2D::getY() const { return y; }
double Vec2D::getMagnitude() const { return sqrt((x*x) + (y*y)); }
double Vec2D::getMagnitudeSquared() const { return ((x*x) + (y*y)); }
double Vec2D::getMagnitude2 const { return getMagnitudeSquared(); }
double Vec2d::crossProduct(Vec2D secondVector) { return ((this.x * secondVector.getY())-(this.y * secondVector.getX()));}
Vec2D crossProduct(Vec2D secondVector) {return new Vec2D(this.y,-(this.x));}
Vec2D Vec2D::normalize() const { return Vec2D(x/getMagnitude(), y/getMagnitude());}
Vec2D operator+(const Vec2D& a, const Vec2D& b) { return Vec2D(a.x + b.x, a.y + b.y);}
ostream& operator<<(ostream& output, const Vec2D& a) { output << "(" << a.x << ", " << a.y << ")" << endl; return output;}
double dotProduct(const Vec2D a, const Vec2D b) { return a.getX() * b.getX() + a.getY() * b.getY();}
Check if a point is inside a triangle described by three vectors:
float calculateSign(Vec2D v1, Vec2D v2, Vec2D v3)
{
return (v1.getX() - v3.getX()) * (v2.getY() - v3.getY()) - (v2.getX() - v3.getX()) * (v1.getY() - v3.getY());
}
bool isPointInsideTriangle(Vec2D point2d, Vec2D v1, Vec2D v2, Vec2D v3)
{
bool b1, b2, b3;
// the < 0.0f is arbitrary, could have just as easily been > (would have flipped the results but would compare the same)
b1 = calculateSign(point2d, v1, v2) < 0.0f;
b2 = calculateSign(point2d, v2, v3) < 0.0f;
b3 = calculateSign(point2d, v3, v1) < 0.0f;
return ((b1 == b2) && (b2 == b3));
}
In the code above if calculateSign
is in the triangle you will get a true
returned :)
Hope this helps, let me know if you need more info or a fuller vec3d or 2d class and I can post:)
Addendum
I have added in a small 2d-vector class, to show the differences in the 2d and 3d ones.