C++ Vector Math Functions

float Length(Vec3 v)
{
    return v.y;
}

void SetPrecicion3Decimal(float &f) {
    f = (int) (f*1000 + 0.0f);
    f = f /1000.0f;
}

Vec3 Setprecicion2Decimal(Vec3 v) {
    v.x = (int) (v.x*100 + 0.5f);
    v.x = v.x /100.0f;
    v.y = (int) (v.y*100 + 0.5f);
    v.y = v.y /100.0f;
    v.z = (int) (v.z*100 + 0.5f);
    v.z = v.z /100.0f;
    return v;
}

float Magnitude(Vec3 a) {
    return Sqrtf( a.x*a.x + a.y*a.y + a.z*a.z);
}
float Dot(Vec3 a, Vec3 b) {
    return a.x*b.x + a.y*b.y + a.z*b.z;
}
Vec3 Project(Vec3 a, Vec3 b) {
    return b * ( Dot(a,b) / Dot(b,b) );
}
Vec3 Normal(Vec3 v) {
    float m = Magnitude(v);
    Vec3 normal = v/m;
    return normal;
}

Vec3 NormalDir(Vec3 a, Vec3 b) {
    Vec3 d( b.x-a.x, b.y-a.y, b.z-a.z );
    return Normal( d );
}

Vec3 Cross(Vec3 a, Vec3 b) {
    Vec3 cross( a.y* b.z - b.y* a.z,/**/ b.x* a.z - a.x* b.z,/**/  a.x* b.y - b.x* a.y );
    return cross;
}
Vec3 Difference(Vec3 v1, Vec3 v2) {
   Vec3 d(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
   return d;
}

Vec3 AlignToUp(Vec3 grav) {
    Vec3 upo( 0.0f, 1.0f, 0.0f );
    Vec3 ng = Normal(grav);
    Vec3 up( upo.x/ng.x, upo.y/ng.y, upo.z/ng.z );//a = a / np.linalg.norm(a) # normalize b  NOT USED
    Vec3 v = Cross(grav, up);
    float c = Dot(grav, up);
    //R = I + v + ( v*v * h); //Identity Matrix/*Vec3 I1( 1, 0, 0);Vec3 I2(0, 1, 0);Vec3 I3(0, 0, 1);*/
    Vec3 v1(0, -v.z, v.y);
    Vec3 v2(v.z, 0, -v.x);
    Vec3 v3(-v.y, v.x, 0);
    float h = 1 / (1 + c);
    //I+v
    Vec3 Iv1(v1.x+1, v1.y, v1.z);
    Vec3 Iv2(v2.x, v2.y+1, v2.z);
    Vec3 Iv3(v3.x, v3.y, v3.z+1);
    //V*V //Vec3 dv( Dot(v1,v1), Dot(v2,v2), Dot(v3,v3) );
    Vec3 vPow1(v1.x*v1.x + v1.y*v2.x + v1.z*v3.x, /**/v1.x*v1.y + v1.y*v2.y + v1.z*v3.y,/**/ v1.x*v1.z + v1.y*v2.z + v1.z*v3.z);
    Vec3 vPow2(v2.x*v1.x + v2.y*v2.x + v2.z*v3.x, /**/v2.x*v1.y + v2.y*v2.y + v2.z*v3.y,/**/ v2.x*v1.z + v2.y*v2.z + v2.z*v3.z);
    Vec3 vPow3(v3.x*v3.x + v3.y*v2.x + v3.z*v3.x, /**/v3.x*v1.y + v3.y*v2.y + v3.z*v3.y,/**/ v3.x*v1.z + v3.y*v2.z + v3.z*v3.z);
    //vPow * h
    Vec3 prodVH1(vPow1.x*h, /**/vPow1.y*h,/**/ vPow1.z*h); Vec3 prodVH2(vPow2.x*h, /**/vPow2.y*h,/**/ vPow2.z*h); Vec3 prodVH3(vPow3.x*h, /**/vPow3.y*h,/**/ vPow3.z*h);
    //I+vPow
    Vec3 R1(Iv1.x+vPow1.x, /**/Iv1.y+vPow1.y,/**/Iv1.z+vPow1.z);
    Vec3 R2(Iv2.x+vPow2.x, /**/Iv2.y+vPow2.y,/**/Iv2.z+vPow2.z);
    Vec3 R3(Iv3.x+vPow3.x, /**/Iv3.y+vPow3.y,/**/Iv3.z+vPow3.z);
    Vec3 alignedGrav(R1.x*grav.x+R1.y*grav.y+R1.z*grav.z,/**/ R2.x*grav.x+R2.y*grav.y+R2.z*grav.z,/**/ R3.x*grav.x+R3.y*grav.y+R3.z*grav.z);
    return alignedGrav;//GravVector * Rmatrix
}