ShakeForFood2.zip_ShakeForFood2_face Recognize_face opencv_openc

/* Copyright (c) 2008, Luke Benstead. All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ /** * @file mat4.c */ #include <memory.h> #include <assert.h> #include <stdlib.h> #include "kazmath/utility.h" #include "kazmath/vec3.h" #include "kazmath/mat4.h" #include "kazmath/mat3.h" #include "kazmath/quaternion.h" #include "kazmath/plane.h" #include "kazmath/neon_matrix_impl.h" /** * Fills a kmMat4 structure with the values from a 16 * element array of floats * @Params pOut - A pointer to the destination matrix * pMat - A 16 element array of floats * @Return Returns pOut so that the call can be nested */ kmMat4* const kmMat4Fill(kmMat4* pOut, const kmScalar* pMat) { memcpy(pOut->mat, pMat, sizeof(kmScalar) * 16); return pOut; } /** * Sets pOut to an identity matrix returns pOut * @Params pOut - A pointer to the matrix to set to identity * @Return Returns pOut so that the call can be nested */ kmMat4* const kmMat4Identity(kmMat4* pOut) { memset(pOut->mat, 0, sizeof(float) * 16); pOut->mat[0] = pOut->mat[5] = pOut->mat[10] = pOut->mat[15] = 1.0f; return pOut; } float get(const kmMat4 * pIn, int row, int col) { return pIn->mat[row + 4*col]; } void set(kmMat4 * pIn, int row, int col, float value) { pIn->mat[row + 4*col] = value; } void swap(kmMat4 * pIn, int r1, int c1, int r2, int c2) { float tmp = get(pIn,r1,c1); set(pIn,r1,c1,get(pIn,r2,c2)); set(pIn,r2,c2, tmp); } //Returns an upper and a lower triangular matrix which are L and R in the Gauss algorithm int gaussj(kmMat4 *a, kmMat4 *b) { int i, icol = 0, irow = 0, j, k, l, ll, n = 4, m = 4; float big, dum, pivinv; int indxc[4] = {0}; int indxr[4] = {0}; int ipiv[4] = {0}; for (j = 0; j < n; j++) { ipiv[j] = 0; } for (i = 0; i < n; i++) { big = 0.0f; for (j = 0; j < n; j++) { if (ipiv[j] != 1) { for (k = 0; k < n; k++) { if (ipiv[k] == 0) { if (abs(get(a,j, k)) >= big) { big = abs(get(a,j, k)); irow = j; icol = k; } } } } } ++(ipiv[icol]); if (irow != icol) { for (l = 0; l < n; l++) { swap(a,irow, l, icol, l); } for (l = 0; l < m; l++) { swap(b,irow, l, icol, l); } } indxr[i] = irow; indxc[i] = icol; if (get(a,icol, icol) == 0.0) { return KM_FALSE; } pivinv = 1.0f / get(a,icol, icol); set(a,icol, icol, 1.0f); for (l = 0; l < n; l++) { set(a,icol, l, get(a,icol, l) * pivinv); } for (l = 0; l < m; l++) { set(b,icol, l, get(b,icol, l) * pivinv); } for (ll = 0; ll < n; ll++) { if (ll != icol) { dum = get(a,ll, icol); set(a,ll, icol, 0.0f); for (l = 0; l < n; l++) { set(a,ll, l, get(a,ll, l) - get(a,icol, l) * dum); } for (l = 0; l < m; l++) { set(b,ll, l, get(a,ll, l) - get(b,icol, l) * dum); } } } } // This is the end of the main loop over columns of the reduction. It only remains to unscram- // ble the solution in view of the column interchanges. We do this by interchanging pairs of // columns in the reverse order that the permutation was built up. for (l = n - 1; l >= 0; l--) { if (indxr[l] != indxc[l]) { for (k = 0; k < n; k++) { swap(a,k, indxr[l], k, indxc[l]); } } } return KM_TRUE; } /** * Calculates the inverse of pM and stores the result in * pOut. * @Return Returns NULL if there is no inverse, else pOut */ kmMat4* const kmMat4Inverse(kmMat4* pOut, const kmMat4* pM) { kmMat4 inv; kmMat4 tmp; kmMat4Assign(&inv, pM); kmMat4Identity(&tmp); if(gaussj(&inv, &tmp) == KM_FALSE) { return NULL; } kmMat4Assign(pOut, &inv); return pOut; } /** * Returns KM_TRUE if pIn is an identity matrix * KM_FALSE otherwise */ const int kmMat4IsIdentity(const kmMat4* pIn) { static const float identity [] = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; return (memcmp(identity, pIn->mat, sizeof(float) * 16) == 0); } /** * Sets pOut to the transpose of pIn, returns pOut */ kmMat4* const kmMat4Transpose(kmMat4* pOut, const kmMat4* pIn) { int x, z; for (z = 0; z < 4; ++z) { for (x = 0; x < 4; ++x) { pOut->mat[(z * 4) + x] = pIn->mat[(x * 4) + z]; } } return pOut; } /** * Multiplies pM1 with pM2, stores the result in pOut, returns pOut */ kmMat4* const kmMat4Multiply(kmMat4* pOut, const kmMat4* pM1, const kmMat4* pM2) { #if defined(__ARM_NEON__) float mat[16]; // Invert column-order with row-order NEON_Matrix4Mul( &pM2->mat[0], &pM1->mat[0], &mat[0] ); #else float mat[16]; const float *m1 = pM1->mat, *m2 = pM2->mat; mat[0] = m1[0] * m2[0] + m1[4] * m2[1] + m1[8] * m2[2] + m1[12] * m2[3]; mat[1] = m1[1] * m2[0] + m1[5] * m2[1] + m1[9] * m2[2] + m1[13] * m2[3]; mat[2] = m1[2] * m2[0] + m1[6] * m2[1] + m1[10] * m2[2] + m1[14] * m2[3]; mat[3] = m1[3] * m2[0] + m1[7] * m2[1] + m1[11] * m2[2] + m1[15] * m2[3]; mat[4] = m1[0] * m2[4] + m1[4] * m2[5] + m1[8] * m2[6] + m1[12] * m2[7]; mat[5] = m1[1] * m2[4] + m1[5] * m2[5] + m1[9] * m2[6] + m1[13] * m2[7]; mat[6] = m1[2] * m2[4] + m1[6] * m2[5] + m1[10] * m2[6] + m1[14] * m2[7]; mat[7] = m1[3] * m2[4] + m1[7] * m2[5] + m1[11] * m2[6] + m1[15] * m2[7]; mat[8] = m1[0] * m2[8] + m1[4] * m2[9] + m1[8] * m2[10] + m1[12] * m2[11]; mat[9] = m1[1] * m2[8] + m1[5] * m2[9] + m1[9] * m2[10] + m1[13] * m2[11]; mat[10] = m1[2] * m2[8] + m1[6] * m2[9] + m1[10] * m2[10] + m1[14] * m2[11]; mat[11] = m1[3] * m2[8] + m1[7] * m2[9] + m1[11] * m2[10] + m1[15] * m2[11]; mat[12] = m1[0] * m2[12] + m1[4] * m2[13] + m1[8] * m2[14] + m1[12] * m2[15]; mat[13] = m1[1] * m2[12] + m1[5] * m2[13] + m1[9] * m2[14] + m1[13] * m2[15]; mat[14] = m1[2] * m2[12] + m1[6] * m2[13] + m1[10] * m2[14] + m1[14] * m2[15]; mat[15] = m1[3] * m2[12] + m1[7] * m2[13] + m1[11] * m2[14] + m1[15] * m2[15]; #endif memcpy(pOut->mat, mat, sizeof(float)*16); return pOut; } /** * Assigns