Task: Rodrigues’ rotation formula

C: #include <stdio.h>
#include <math.h>

typedef struct {
    double x, y, z;
} vector;

typedef struct {
    vector i, j, k;
} matrix;

double norm(vector v) {
    return sqrt(v.x*v.x + v.y*v.y + v.z*v.z);
}

vector normalize(vector v){
    double length = norm(v);
    vector n = {v.x / length, v.y / length, v.z / length};
    return n;
}

double dotProduct(vector v1, vector v2) {
    return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
}

vector crossProduct(vector v1, vector v2) {
    vector cp = {v1.y*v2.z - v1.z*v2.y, v1.z*v2.x - v1.x*v2.z, v1.x*v2.y - v1.y*v2.x};
    return cp;
}

double getAngle(vector v1, vector v2) {
    return acos(dotProduct(v1, v2) / (norm(v1)*norm(v2)));
}

vector matrixMultiply(matrix m ,vector v) {
    vector mm = {dotProduct(m.i, v), dotProduct(m.j, v), dotProduct(m.k, v)};
    return mm;
}

vector aRotate(vector p, vector v, double a) {
    double ca = cos(a), sa = sin(a);
    double t = 1.0 - ca;
    double x = v.x, y = v.y, z = v.z;
    matrix r = {
        {ca + x*x*t, x*y*t - z*sa, x*z*t + y*sa},
        {x*y*t + z*sa, ca + y*y*t, y*z*t - x*sa},
        {z*x*t - y*sa, z*y*t + x*sa, ca + z*z*t}
    };
    return matrixMultiply(r, p);
}

int main() {
    vector v1 = {5, -6, 4}, v2 = {8, 5, -30};
    double a = getAngle(v1, v2);
    vector cp = crossProduct(v1, v2);
    vector ncp = normalize(cp);
    vector np = aRotate(v1, ncp, a);
    printf("[%.13f, %.13f, %.13f]\n", np.x, np.y, np.z);
    return 0;
}





Java: 

class Vector{
  private double x, y, z;

  public Vector(double x1,double y1,double z1){
    x = x1;
    y = y1;
    z = z1;
  }
  
  void printVector(int x,int y){
    text("( " + this.x + " )  \u00ee + ( " + this.y + " ) + \u0135 ( " + this.z + ") \u006b\u0302",x,y);
  }

  public double norm() {
    return Math.sqrt(this.x*this.x + this.y*this.y + this.z*this.z);
  }
  
  public Vector normalize(){
    double length = this.norm();
    return new Vector(this.x / length, this.y / length, this.z / length);
  }
  
  public double dotProduct(Vector v2) {
    return this.x*v2.x + this.y*v2.y + this.z*v2.z;
  }
  
  public Vector crossProduct(Vector v2) {
    return new Vector(this.y*v2.z - this.z*v2.y, this.z*v2.x - this.x*v2.z, this.x*v2.y - this.y*v2.x);
  }
  
  public double getAngle(Vector v2) {
    return Math.acos(this.dotProduct(v2) / (this.norm()*v2.norm()));
  }
  
  public Vector aRotate(Vector v, double a) {
    double ca = Math.cos(a), sa = Math.sin(a);
    double t = 1.0 - ca;
    double x = v.x, y = v.y, z = v.z;
    Vector[] r = {
        new Vector(ca + x*x*t, x*y*t - z*sa, x*z*t + y*sa),
        new Vector(x*y*t + z*sa, ca + y*y*t, y*z*t - x*sa),
        new Vector(z*x*t - y*sa, z*y*t + x*sa, ca + z*z*t)
    };
    return new Vector(this.dotProduct(r[0]), this.dotProduct(r[1]), this.dotProduct(r[2]));
  }
}

void setup(){
  Vector v1 = new Vector(5d, -6d, 4d),v2 = new Vector(8d, 5d, -30d);
  double a = v1.getAngle(v2);
  Vector cp = v1.crossProduct(v2);
  Vector normCP = cp.normalize();
  Vector np = v1.aRotate(normCP,a);
  
  size(1200,600);
  fill(#000000);
  textSize(30);
  
  text("v1 = ",10,100);
  v1.printVector(60,100);
  text("v2 = ",10,150);
  v2.printVector(60,150);
  text("rV = ",10,200);
  np.printVector(60,200);
}