Processing用 Matrixクラス

ちょうどいいライブラリってなにかあるのかな。。。

4x4行列とベクトル

class Vector4 {
  float v0, v1, v2, v3;
  
  Vector4(float v0, float v1, float v2, float v3) {
    this.v0 = v0;
    this.v1 = v1;
    this.v2 = v2;
    this.v3 = v3;
  }
}

class Matrix4 {

  float m00, m01, m02, m03;
  float m10, m11, m12, m13;
  float m20, m21, m22, m23;
  float m30, m31, m32, m33;
  
  Matrix4() {
    this(
      1.0, 0.0, 0.0, 0.0,
      0.0, 1.0, 0.0, 0.0,
      0.0, 0.0, 1.0, 0.0,
      0.0, 0.0, 0.0, 1.0);
  }
  
  Matrix4(
    float m00, float m01, float m02, float m03,
    float m10, float m11, float m12, float m13,
    float m20, float m21, float m22, float m23,
    float m30, float m31, float m32, float m33
  ) {
    this.m00 = m00;
    this.m01 = m01;
    this.m02 = m02;
    this.m03 = m03;

    this.m10 = m10;
    this.m11 = m11;
    this.m12 = m12;
    this.m13 = m13;

    this.m20 = m20;
    this.m21 = m21;
    this.m22 = m22;
    this.m23 = m23;

    this.m30 = m30;
    this.m31 = m31;
    this.m32 = m32;
    this.m33 = m33;
  }
    
  void mul(Matrix4 m) {
    float m00 = this.m00*m.m00 + this.m01*m.m10 + this.m02*m.m20 + this.m03*m.m30;
    float m01 = this.m00*m.m01 + this.m01*m.m11 + this.m02*m.m21 + this.m03*m.m31;
    float m02 = this.m00*m.m02 + this.m01*m.m12 + this.m02*m.m22 + this.m03*m.m32;
    float m03 = this.m00*m.m03 + this.m01*m.m13 + this.m02*m.m23 + this.m03*m.m33;

    float m10 = this.m10*m.m00 + this.m11*m.m10 + this.m12*m.m20 + this.m13*m.m30;
    float m11 = this.m10*m.m01 + this.m11*m.m11 + this.m12*m.m21 + this.m13*m.m31;
    float m12 = this.m10*m.m02 + this.m11*m.m12 + this.m12*m.m22 + this.m13*m.m32;
    float m13 = this.m10*m.m03 + this.m11*m.m13 + this.m12*m.m23 + this.m13*m.m33;

    float m20 = this.m20*m.m00 + this.m21*m.m10 + this.m22*m.m20 + this.m23*m.m30;
    float m21 = this.m20*m.m01 + this.m21*m.m11 + this.m22*m.m21 + this.m23*m.m31;
    float m22 = this.m20*m.m02 + this.m21*m.m12 + this.m22*m.m22 + this.m23*m.m32;
    float m23 = this.m20*m.m03 + this.m21*m.m13 + this.m22*m.m23 + this.m23*m.m33;

    float m30 = this.m30*m.m00 + this.m31*m.m10 + this.m32*m.m20 + this.m33*m.m30;
    float m31 = this.m30*m.m01 + this.m31*m.m11 + this.m32*m.m21 + this.m33*m.m31;
    float m32 = this.m30*m.m02 + this.m31*m.m12 + this.m32*m.m22 + this.m33*m.m32;
    float m33 = this.m30*m.m03 + this.m31*m.m13 + this.m32*m.m23 + this.m33*m.m33;
    
    this.m00 = m00;
    this.m01 = m01;
    this.m02 = m02;
    this.m03 = m03;

    this.m10 = m10;
    this.m11 = m11;
    this.m12 = m12;
    this.m13 = m13;

    this.m20 = m20;
    this.m21 = m21;
    this.m22 = m22;
    this.m23 = m23;

    this.m30 = m30;
    this.m31 = m31;
    this.m32 = m32;
    this.m33 = m33;
  }
  
  Vector4 mul(Vector4 v) {
    float v0 = this.m00*v.v0 + this.m01*v.v1 + this.m02*v.v2 + this.m03*v.v3;
    float v1 = this.m10*v.v0 + this.m11*v.v1 + this.m12*v.v2 + this.m13*v.v3;
    float v2 = this.m20*v.v0 + this.m21*v.v1 + this.m22*v.v2 + this.m23*v.v3;
    float v3 = this.m30*v.v0 + this.m31*v.v1 + this.m32*v.v2 + this.m33*v.v3;
    return new Vector4(v0, v1, v2, v3);
  }
  
  void rotateX(float theta) {
    float s = sin(theta);
    float c = cos(theta);
    
    this.mul(new Matrix4(
      1, 0,  0, 0,
      0, c, -s, 0,
      0, s,  c, 0,
      0, 0,  0, 1
    ));
  }

  void rotateY(float theta) {
    float s = sin(theta);
    float c = cos(theta);
    
    this.mul(new Matrix4(
      c, 0, -s, 0,
      0, 1,  0, 0,
      s, 0,  c, 0,
      0, 0,  0, 1
    ));
  }

  void rotateZ(float theta) {
    float s = sin(theta);
    float c = cos(theta);
    
    this.mul(new Matrix4(
      c, -s, 0, 0,
      s,  c, 0, 0,
      0,  0, 1, 0,
      0,  0, 0, 1
    ));
  }
  
  void rotate_axis(float nx, float ny, float nz, float theta) {
    float s = sin(theta);
    float c = cos(theta);
    
    this.mul(new Matrix4(
      nx*nx*(1-c)+c,    nx*ny*(1-c)-nz*s, nz*nx*(1-c)+ny*s, 0,
      nx*ny*(1-c)+nz*s, ny*ny*(1-c)+c,    ny*nz*(1-c)-nx*s, 0,
      nz*nx*(1-c)-ny*s, ny*nz*(1-c)+nx*s, nz*nz*(1-c)+c,    0,
      0,                0,                0,                1
    ));
  }
  
  void apply() {
    applyMatrix(
      this.m00, this.m01, this.m02, this.m03,
      this.m10, this.m11, this.m12, this.m13,
      this.m20, this.m21, this.m22, this.m23,
      this.m30, this.m31, this.m32, this.m33
    );
  }
}