Math Notes

Vector(){ x = 0; y = 0; z = 0; }
Vector(float X, float Y, float Z){ x=X; y=Y; z=Z; }
~Vector(){ }

float valX(){ return x; }
float valY(){ return y; }
float valZ(){ return z; }

Vector inverse() { Vector I; I.x=-x; I.y=-y; I.z=-z; return I; }

float magnitude() { float m=sqrt(x*x + y*y + z*z); return m;}

float distance(Vector P2) { Vector P=(*this-P2); return P.magnitude(); }

Vector direction() {
  float mag;
  Vector unit;
  mag = magnitude(); // this->magnitude() works too...
  if (mag > 0) {
      unit.x = valX() / mag;
      unit.y = valY() / mag;
      unit.z = valZ() / mag; }
  else { unit.x = 0; unit.y = 0; unit.z =0;}
  return unit; }

Vector add(Vector offset) {
    Vector sum;
    sum.x = x + offset.x;
    sum.y = y + offset.y;
    sum.z = z + offset.z;
    return sum; }

Vector subtract(Vector offset) {
    Vector difference;
    difference.x = x - offset.x;
    difference.y = y - offset.y;
    difference.z = z - offset.z;
    return difference; }

Vector rotate(float &rads) {
    Vector rotated;
    float s, c;
    s=sin(rads); c=cos(rads);
    rotated.x = x*c - y*s;
    rotated.y= y*c + x*s;
    rotated.z= z;
    return rotated;        }

Vector rotate(float &rads, Vector &basept) {
    Vector rotated;
    float y2, s, c;
    rotated = this->subtract(basept);
    s=sin(rads); c=cos(rads);
    y2= rotated.y*c + rotated.x*s;
    rotated.x = rotated.x*c - rotated.y*s;
    rotated.y = y2;
    rotated.set2_add(basept);
    return rotated;              }

/*  rotation in XZ axis */
Vector tilt(float &rads) {
    Vector rotated;
    float s, c;
    s=sin(rads); c=cos(rads);
    rotated.x = x*c - y*s;
    rotated.z= z*c + z*s;
    rotated.y= y;
    return rotated;        }

Vector tilt(float &rads, Vector &basept) {
    Vector rotated;
    float z2, s, c;
    rotated = this->subtract(basept);
    s=sin(rads); c=cos(rads);
    z2= rotated.z*c + rotated.x*s;
    rotated.x = rotated.x*c - rotated.z*s;
    rotated.z = z2;
    rotated.set2_add(basept);
    return rotated;              }

Vector viewvector(Vector &VV) {
    if ((VV.x != 0) || (VV.y != 0)) {
        Vector T, Xplus(1,0,0);
        float r, t;
        T.x=VV.x; T.y=VV.y; T.z=0;
        r= ( T.y<0 ? T.angle(Xplus) : -T.angle(Xplus) );
        t = (M_PI/2) -  atan(VV.z / sqrt(VV.x * VV.x + VV.y * VV.y) );
        T = rotate(r);
        T.set2_tilt(t);
        r=-T.x; T.x=T.y; T.y=r; // turn 90deg- Z always points up
        return T;}
    else return *this; }

Vector perspective(float &lens, float &camdist) {
    Vector T; float tz;
    tz = camdist - z;
    if (tz) {
        T.x= (x * lens) / tz;
        T.y= (y * lens) / tz;}
    return T;  }

Vector scale(float &fact) {
    Vector scaled;
    scaled.x = x*fact;
    scaled.y= y*fact;
    scaled.z= z*fact;
    return scaled;        }

Vector scale(float &fact, Vector &basept) {
    Vector scaled;
    scaled = this->subtract(basept);
    scaled.x = scaled.x*fact;
    scaled.y = scaled.y*fact;
    scaled.z = scaled.z*fact;
    scaled.set2_add(basept);
    return scaled;              }

Vector scale(Vector &fact3d) {
    Vector scaled;
    scaled.x = x * fact3d.x;
    scaled.y = y * fact3d.y;
    scaled.z = z * fact3d.z;
    return scaled;        }

Vector scale(Vector &fact3d, Vector &basept) {
    Vector scaled;
    scaled = this->subtract(basept);
    scaled.x = scaled.x * fact3d.x;
    scaled.y = scaled.y * fact3d.y;
    scaled.z = scaled.z * fact3d.z;
    scaled.set2_add(basept);
    return scaled;              }

float dot(Vector &B) {
    float dotprod;
    dotprod = x * B.x + y * B.y + z * B.z;
    return dotprod; }

float angle(Vector &B) {
    float ang;
    ang = magnitude() * B.magnitude();
    if ( ang == 0 ) ang = 0;
    else ang = acos( dot(B) / ang ) ;
    if (!(ang == ang)) return 0; /* nan check!!!
    else return ang; }

Vector midpoint(Vector &B) {
    return (((*this - B) * .5) + B); }

float angle(Vector B, Vector basept) {
    set2_subtract(basept); B.set2_subtract(basept);
    return angle(B); }

Vector cross(Vector &B) {
    Vector C;
    C.x = y * B.z - z * B.y;
    C.y = z * B.x - x * B.z;
    C.z = x * B.y - y * B.x;
    return C;  }

/* Input: */
/* A= matrix of dimension x by y. */
/* B= matrix of dimension y by z. */
/* Output: */
/* C= matrix of dimension x by z
/* where C[i][j] is dot product of ith row of A and jth column of B */

for (i=1;i<=x;i++)
    for (j=1;j<=z;j++) {
        C[i][j]= 0;
        for (k=1;k<=y;k++)
            C[i][j]+= A[i][k] * B[k][j];
    }

#!/usr/bin/python3
from math import *

class matrix:

    def __init__(self, value):
        self.value = value
        self.dimx = len(value)
        self.dimy = len(value[0])
        if value == [[]]:
            self.dimx = 0

    def zero(self, dimx, dimy):
        assert dimx >= 1 or dimy >= 1, "Invalid size of matrix"
        self.dimx = dimx
        self.dimy = dimy
        self.value = [[0 for row in range(dimy)] for col in range(dimx)]

    def identity(self, dim):
        assert dim >= 1, "Invalid size of matrix"
        self.dimx = dim
        self.dimy = dim
        self.value = [[0 for row in range(dim)] for col in range(dim)]
        for i in range(dim):
            self.value[i][i] = 1

    def show(self):
        for i in range(self.dimx):
            print(self.value[i])
        print(' ')

    def __add__(self, other):
        assert self.dimx == other.dimx or self.dimy == other.dimy, \
               "Matrices must be of equal dimensions to add"
        res = matrix([[]])
        res.zero(self.dimx, self.dimy)
        for i in range(self.dimx):
            for j in range(self.dimy):
                res.value[i][j] = self.value[i][j] + other.value[i][j]
        return res

    def __sub__(self, other):
        assert self.dimx == other.dimx or self.dimy == other.dimy, \
               "Matrices must be of equal dimensions to subtract"
        res = matrix([[]])
        res.zero(self.dimx, self.dimy)
        for i in range(self.dimx):
            for j in range(self.dimy):
                res.value[i][j] = self.value[i][j] - other.value[i][j]
        return res

   def __mul__(self, other):
        assert self.dimy == other.dimx, \
             "Matrices must be m*n and n*p to multiply"
        res = matrix([[]])
        res.zero(self.dimx, other.dimy)
        for i in range(self.dimx):
            for j in range(other.dimy):
                for k in range(self.dimy):
                    res.value[i][j] += self.value[i][k] * other.value[k][j]
        return res

    def transpose(self):
        res = matrix([[]])
        res.zero(self.dimy, self.dimx)
        for i in range(self.dimx):
            for j in range(self.dimy):
                res.value[j][i] = self.value[i][j]
        return res

    def Cholesky(self, ztol=1.0e-5):
        # Computes the upper triangular Cholesky factorization of
        # a positive definite matrix.
        res = matrix([[]])
        res.zero(self.dimx, self.dimx)
        for i in range(self.dimx):
            S = sum([(res.value[k][i])**2 for k in range(i)])
            d = self.value[i][i] - S
            if abs(d) < ztol:
                res.value[i][i] = 0.0
            else:
                assert d >= 0.0, "Matrix not positive-definite"
                res.value[i][i] = sqrt(d)
            for j in range(i+1, self.dimx):
                S = sum([res.value[k][i] * res.value[k][j] for k in range(self.dimx)])
                if abs(S) < ztol:
                    S = 0.0
                res.value[i][j] = (self.value[i][j] - S)/res.value[i][i]
        return res

    def CholeskyInverse(self):
        # Computes inverse of matrix given its Cholesky upper Triangular
        # decomposition of matrix.
        res = matrix([[]])
        res.zero(self.dimx, self.dimx)
        # Backward step for inverse.
        for j in reversed(range(self.dimx)):
            tjj = self.value[j][j]
            S = sum([self.value[j][k]*res.value[j][k] for k in range(j+1, self.dimx)])
            res.value[j][j] = 1.0/tjj**2 - S/tjj
            for i in reversed(range(j)):
                res.value[j][i] = res.value[i][j] = -sum([self.value[i][k]*res.value[k][j] for k in range(i+1, self.dimx)])/self.value[i][i]
        return res

    def inverse(self):
        aux = self.Cholesky()
        res = aux.CholeskyInverse()
        return res

    def __repr__(self):
        return repr(self.value)