#################################
### COPYRIGHT: Anders Lyhagen ###
#################################


module CAUL_FAQS
module Matrix

  #return the transformation that aligns the normalized vector a
  #with the normalized vector b
  def self.getAlignmentMatrix(a, b)
    v = a.cross(b)
    sin = v.length
    cos = a.dot(b)
    
    return Geom::Transformation.new if sin == 0
    
    vm = getZM()
    vm[1] = v.z;  vm[2] = -v.y
    vm[4] = -v.z; vm[6] = v.x 
    vm[8] = v.y;  vm[9] = -v.x
   
    vm2 = mulM(vm, vm)
    vms = scalarMulM(vm2, (1 - cos) / (sin * sin))
    tm = addM(vm, vms)
    resm = addM(getIM, tm)
    return Geom::Transformation.new(resm)
  end

  ############ MATRIX MANIPULATION ##############
  #The matrix is a 1D array representing a 4x4 array. 
  #The first 4 numbers is the first column.  
  #######################################################
  
  def self.getIM
    return [1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]
  end
  
  def self.getZM
    return Array::new(16, 0)
  end
  
  def self.mulM(m0, m1)
    m = getZM
    (0..3).each { |row|
      (0..3).each { |col| 
        (0..3).each { |i| m[col * 4 + row] += m0[row + i * 4] * m1[col * 4 + i]
    }}}
    return m
  end
    
  def self.scalarMulM(m0, s)
    m = getZM
    (0..3).each { |row|
      (0..3).each { |col| 
        m[col * 4 + row] = m0[col * 4 + row] * s
    }}
    return m
  end
  
  def self.addM(m0, m1)
    m = getZM
    (0..3).each { |row|
      (0..3).each { |col| 
        m[col * 4 + row] = m0[col * 4 + row] + m1[col * 4 + row]
    }}
    return m
  end
end
end
