1e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen/* 2e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Copyright (C) 2011 The Android Open Source Project 3e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * 4e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Licensed under the Apache License, Version 2.0 (the "License"); 5e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * you may not use this file except in compliance with the License. 6e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * You may obtain a copy of the License at 7e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * 8e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * http://www.apache.org/licenses/LICENSE-2.0 9e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * 10e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * Unless required by applicable law or agreed to in writing, software 11e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * distributed under the License is distributed on an "AS IS" BASIS, 12e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * See the License for the specific language governing permissions and 14e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen * limitations under the License. 15e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen */ 16e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 17e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen// trsMatrix.cpp 18e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen// $Id: trsMatrix.cpp,v 1.9 2011/06/17 13:35:48 mbansal Exp $ 19e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 20e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#include "stdio.h" 21e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#include <math.h> 22e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen#include "trsMatrix.h" 23e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 24e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenvoid mult33d(double a[3][3], double b[3][3], double c[3][3]) 25e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 26e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a[0][0] = b[0][0]*c[0][0] + b[0][1]*c[1][0] + b[0][2]*c[2][0]; 27e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a[0][1] = b[0][0]*c[0][1] + b[0][1]*c[1][1] + b[0][2]*c[2][1]; 28e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a[0][2] = b[0][0]*c[0][2] + b[0][1]*c[1][2] + b[0][2]*c[2][2]; 29e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a[1][0] = b[1][0]*c[0][0] + b[1][1]*c[1][0] + b[1][2]*c[2][0]; 30e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a[1][1] = b[1][0]*c[0][1] + b[1][1]*c[1][1] + b[1][2]*c[2][1]; 31e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a[1][2] = b[1][0]*c[0][2] + b[1][1]*c[1][2] + b[1][2]*c[2][2]; 32e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a[2][0] = b[2][0]*c[0][0] + b[2][1]*c[1][0] + b[2][2]*c[2][0]; 33e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a[2][1] = b[2][0]*c[0][1] + b[2][1]*c[1][1] + b[2][2]*c[2][1]; 34e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen a[2][2] = b[2][0]*c[0][2] + b[2][1]*c[1][2] + b[2][2]*c[2][2]; 35e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 36e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 37e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 38e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen// normProjMat33d 39e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen// m = input matrix 40e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen// return: result if successful 41e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenint normProjMat33d(double m[3][3]) 42e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 43e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double m22; 44e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 45e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen if(m[2][2] == 0.0) 46e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen { 47e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen return 0; 48e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 49e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 50e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m[0][0] /= m[2][2]; 51e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m[0][1] /= m[2][2]; 52e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m[0][2] /= m[2][2]; 53e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m[1][0] /= m[2][2]; 54e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m[1][1] /= m[2][2]; 55e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m[1][2] /= m[2][2]; 56e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m[2][0] /= m[2][2]; 57e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m[2][1] /= m[2][2]; 58e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen m[2][2] = 1.0; 59e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 60e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen return 1; 61e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 62e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 63e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen// det33d 64e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen// m = input matrix 65e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen// returns: determinant 66e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chendouble det33d(const double m[3][3]) 67e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 68e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double result; 69e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 70e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen result = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]); 71e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen result += m[0][1] * (m[1][2] * m[2][0] - m[1][0] * m[2][2]); 72e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen result += m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]); 73e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 74e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen return result; 75e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 76e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 77e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen// inv33d 78e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen// 79e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chenvoid inv33d(const double m[3][3], double out[3][3]) 80e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen{ 81e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen double det = det33d(m); 82e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 83e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[0][0] = (m[1][1]*m[2][2] - m[1][2]*m[2][1]) / det; 84e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[1][0] = (m[1][2]*m[2][0] - m[1][0]*m[2][2]) / det; 85e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[2][0] = (m[1][0]*m[2][1] - m[1][1]*m[2][0]) / det; 86e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 87e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[0][1] = (m[0][2]*m[2][1] - m[0][1]*m[2][2]) / det; 88e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[1][1] = (m[0][0]*m[2][2] - m[0][2]*m[2][0]) / det; 89e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[2][1] = (m[0][1]*m[2][0] - m[0][0]*m[2][1]) / det; 90e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen 91e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[0][2] = (m[0][1]*m[1][2] - m[0][2]*m[1][1]) / det; 92e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[1][2] = (m[0][2]*m[1][0] - m[0][0]*m[1][2]) / det; 93e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen out[2][2] = (m[0][0]*m[1][1] - m[0][1]*m[1][0]) / det; 94e295e32b68cf04f0d99138bf4a6d25555f3aef99Wei-Ta Chen} 95