Embedded/common/src/b_TensorEm/Flt16Mat2D.c - platform/external/neven - Git at Google

 /*
  * Copyright (C) 2008 The Android Open Source Project
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  *
  *      http://www.apache.org/licenses/LICENSE-2.0
  *
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */

 /* ---- includes ----------------------------------------------------------- */

 #include "b_TensorEm/Flt16Mat2D.h"
 #include "b_TensorEm/Functions.h"
 #include "b_BasicEm/Math.h"

 /* ------------------------------------------------------------------------- */

 /* ========================================================================= */
 /*                                                                           */
 /* ---- \ghd{ auxiliary functions } ---------------------------------------- */
 /*                                                                           */
 /* ========================================================================= */

 /* ------------------------------------------------------------------------- */

 /* ========================================================================= */
 /*                                                                           */
 /* ---- \ghd{ constructor / destructor } ----------------------------------- */
 /*                                                                           */
 /* ========================================================================= */

 /* ------------------------------------------------------------------------- */

 void bts_Flt16Mat2D_init( struct bts_Flt16Mat2D* ptrA )
 {
 	ptrA->bbpE = 0;
 	ptrA->xxE = 0;
 	ptrA->xyE = 0;
 	ptrA->yxE = 0;
 	ptrA->yyE = 0;
 }

 /* ------------------------------------------------------------------------- */

 void bts_Flt16Mat2D_exit( struct bts_Flt16Mat2D* ptrA )
 {
 	ptrA->bbpE = 0;
 	ptrA->xxE = 0;
 	ptrA->xyE = 0;
 	ptrA->yxE = 0;
 	ptrA->yyE = 0;
 }

 /* ------------------------------------------------------------------------- */

 /* ========================================================================= */
 /*                                                                           */
 /* ---- \ghd{ operators } -------------------------------------------------- */
 /*                                                                           */
 /* ========================================================================= */

 /* ------------------------------------------------------------------------- */

 void bts_Flt16Mat2D_copy( struct bts_Flt16Mat2D* ptrA, const struct bts_Flt16Mat2D* srcPtrA )
 {
 	ptrA->bbpE = srcPtrA->bbpE;
 	ptrA->xxE = srcPtrA->xxE;
 	ptrA->xyE = srcPtrA->xyE;
 	ptrA->yxE = srcPtrA->yxE;
 	ptrA->yyE = srcPtrA->yyE;
 }

 /* ------------------------------------------------------------------------- */

 flag bts_Flt16Mat2D_equal( const struct bts_Flt16Mat2D* ptrA, const struct bts_Flt16Mat2D* srcPtrA )
 {
 	int32 bbpDiffL = ptrA->bbpE - srcPtrA->bbpE;
 	if( bbpDiffL == 0 )
 	{
 		if( ptrA->xxE != srcPtrA->xxE ) return FALSE;
 		if( ptrA->xyE != srcPtrA->xyE ) return FALSE;
 		if( ptrA->yxE != srcPtrA->yxE ) return FALSE;
 		if( ptrA->yyE != srcPtrA->yyE ) return FALSE;
 		return TRUE;
 	}

 	if( bbpDiffL > 0 )
 	{
 		int32 xxL = ( int32 ) srcPtrA->xxE << bbpDiffL;
 		int32 xyL = ( int32 ) srcPtrA->xyE << bbpDiffL;
 		int32 yxL = ( int32 ) srcPtrA->yxE << bbpDiffL;
 		int32 yyL = ( int32 ) srcPtrA->yyE << bbpDiffL;

 		if( ptrA->xxE != xxL ) return FALSE;
 		if( ptrA->xyE != xyL ) return FALSE;
 		if( ptrA->yxE != yxL ) return FALSE;
 		if( ptrA->yyE != yyL ) return FALSE;

 		/* check if bits were lost by the shifting */
 		if( srcPtrA->xxE != ( xxL >> bbpDiffL ) ) return FALSE;
 		if( srcPtrA->xyE != ( xyL >> bbpDiffL ) ) return FALSE;
 		if( srcPtrA->yxE != ( yxL >> bbpDiffL ) ) return FALSE;
 		if( srcPtrA->yyE != ( yyL >> bbpDiffL ) ) return FALSE;

 		return TRUE;
 	}

 	if( bbpDiffL < 0 )
 	{
 		int32 xxL = ( int32 ) ptrA->xxE << -bbpDiffL;
 		int32 xyL = ( int32 ) ptrA->xyE << -bbpDiffL;
 		int32 yxL = ( int32 ) ptrA->yxE << -bbpDiffL;
 		int32 yyL = ( int32 ) ptrA->yyE << -bbpDiffL;

 		if( xxL != srcPtrA->xxE ) return FALSE;
 		if( xyL != srcPtrA->xyE ) return FALSE;
 		if( yxL != srcPtrA->yxE ) return FALSE;
 		if( yyL != srcPtrA->yyE ) return FALSE;

 		/* check if bits were lost by the shifting */
 		if( ptrA->xxE != ( xxL >> -bbpDiffL ) ) return FALSE;
 		if( ptrA->xyE != ( xyL >> -bbpDiffL ) ) return FALSE;
 		if( ptrA->yxE != ( yxL >> -bbpDiffL ) ) return FALSE;
 		if( ptrA->yyE != ( yyL >> -bbpDiffL ) ) return FALSE;

 		return TRUE;
 	}

 	return TRUE;
 }

 /* ========================================================================= */
 /*                                                                           */
 /* ---- \ghd{ query functions } -------------------------------------------- */
 /*                                                                           */
 /* ========================================================================= */

 /* ------------------------------------------------------------------------- */

 /* ========================================================================= */
 /*                                                                           */
 /* ---- \ghd{ modify functions } ------------------------------------------- */
 /*                                                                           */
 /* ========================================================================= */

 /* ------------------------------------------------------------------------- */

 /* ========================================================================= */
 /*                                                                           */
 /* ---- \ghd{ I/O } -------------------------------------------------------- */
 /*                                                                           */
 /* ========================================================================= */

 /* ------------------------------------------------------------------------- */

 /* ========================================================================= */
 /*                                                                           */
 /* ---- \ghd{ exec functions } --------------------------------------------- */
 /*                                                                           */
 /* ========================================================================= */

 /* ------------------------------------------------------------------------- */

 uint32 bts_Flt16Mat2D_det( const struct bts_Flt16Mat2D* ptrA )
 {
 	/* This could be negativ, in theory. But almost always det > 0 for us,
 	   matrix is a rotation or scaling matrix.
 	   Then uint32 makes sure there is no overflow. */
 	uint32 detL = ( int32 ) ptrA->xxE * ptrA->yyE - ( int32 ) ptrA->xyE * ptrA->yxE;
 	return detL;
 }

 /* ------------------------------------------------------------------------- */

 struct bts_Flt16Mat2D bts_Flt16Mat2D_createIdentity()
 {
 	struct bts_Flt16Mat2D matL = { 1 << 14, 0, 0, 1 << 14, 14 };
 	return matL;
 }

 /* ------------------------------------------------------------------------- */

 struct bts_Flt16Mat2D bts_Flt16Mat2D_createRotation( phase16 angleA )
 {
 	int16 cL = bbs_cos16( angleA );
 	int16 sL = bbs_sin16( angleA );
 	struct bts_Flt16Mat2D matL;
 	matL.xxE =  cL;
 	matL.xyE = -sL;
 	matL.yxE =  sL;
 	matL.yyE =  cL;
 	matL.bbpE = 14;
 	return matL;
 }

 /* ------------------------------------------------------------------------- */

 struct bts_Flt16Mat2D bts_Flt16Mat2D_createScale( int32 scaleA, int32 scaleBbpA )
 {
 	struct bts_Flt16Mat2D matL = bts_Flt16Mat2D_createIdentity();
 	bts_Flt16Mat2D_scale( &matL, scaleA, scaleBbpA );
 	return matL;
 }

 /* ------------------------------------------------------------------------- */

 struct bts_Flt16Mat2D bts_Flt16Mat2D_createRigid( phase16 angleA, int32 scaleA, int32 scaleBbpA )
 {
 	struct bts_Flt16Mat2D matL = bts_Flt16Mat2D_createRotation( angleA );
 	bts_Flt16Mat2D_scale( &matL, scaleA, scaleBbpA );
 	return matL;
 }

 /* ------------------------------------------------------------------------- */

 struct bts_Flt16Mat2D bts_Flt16Mat2D_create16( int16 xxA, int16 xyA, int16 yxA, int16 yyA, int16 bbpA )
 {
 	struct bts_Flt16Mat2D matL;
 	matL.xxE = xxA;
 	matL.xyE = xyA;
 	matL.yxE = yxA;
 	matL.yyE = yyA;
 	matL.bbpE = bbpA;
 	return matL;
 }

 /* ------------------------------------------------------------------------- */

 struct bts_Flt16Mat2D bts_Flt16Mat2D_create32( int32 xxA, int32 xyA, int32 yxA, int32 yyA, int32 bbpA )
 {
 	struct bts_Flt16Mat2D matL;

 	if( ( xxA | xyA | yxA | yyA ) == 0 )
 	{
 		matL.xxE = 0;
 		matL.xyE = 0;
 		matL.yxE = 0;
 		matL.yyE = 0;
 		matL.bbpE = 0;
 	}
 	else
 	{
 		int32 shiftL = bts_maxAbsIntLog2Of4( xxA, xyA, yxA, yyA ) - 13;

 		if( shiftL > 0 )
 		{
 			int32 sh1L = shiftL - 1;
 			matL.xxE = ( ( xxA >> sh1L ) + 1 ) >> 1;
 			matL.xyE = ( ( xyA >> sh1L ) + 1 ) >> 1;
 			matL.yxE = ( ( yxA >> sh1L ) + 1 ) >> 1;
 			matL.yyE = ( ( yyA >> sh1L ) + 1 ) >> 1;
 		}
 		else
 		{
 			matL.xxE = xxA << -shiftL;
 			matL.xyE = xyA << -shiftL;
 			matL.yxE = yxA << -shiftL;
 			matL.yyE = yyA << -shiftL;
 		}

 		matL.bbpE = bbpA - shiftL;
 	}

 	return matL;
 }

 /* ------------------------------------------------------------------------- */

 void bts_Flt16Mat2D_scale( struct bts_Flt16Mat2D* ptrA, int32 scaleA, int32 scaleBbpA )
 {
 	/* fit scale in 15 bit */
 	uint32 scaleExpL = bts_absIntLog2( scaleA );
 	if( scaleExpL > 14 )
 	{
 		int32 shiftL = scaleExpL - 14;
 		scaleA = ( ( scaleA >> ( shiftL - 1 ) ) + 1 ) >> 1;
 		scaleBbpA -= shiftL;
 	}

 	*ptrA = bts_Flt16Mat2D_create32( (int32)ptrA->xxE * scaleA,
 									 (int32)ptrA->xyE * scaleA,
 									 (int32)ptrA->yxE * scaleA,
 									 (int32)ptrA->yyE * scaleA,
 									 ptrA->bbpE + scaleBbpA );
 }

 /* ------------------------------------------------------------------------- */

 struct bts_Int16Vec2D bts_Flt16Mat2D_map( const struct bts_Flt16Mat2D* matPtrA,
 								          const struct bts_Int16Vec2D* vecPtrA )
 {
 	struct bts_Int16Vec2D vecL;

 	int32 xL = ( int32 ) matPtrA->xxE * vecPtrA->xE + ( int32 ) matPtrA->xyE * vecPtrA->yE;
 	int32 yL = ( int32 ) matPtrA->yxE * vecPtrA->xE + ( int32 ) matPtrA->yyE * vecPtrA->yE;

 	if( matPtrA->bbpE > 0 )
 	{
 		int32 sh1L = matPtrA->bbpE - 1;
 		vecL.xE = ( ( xL >> sh1L ) + 1 ) >> 1;
 		vecL.yE = ( ( yL >> sh1L ) + 1 ) >> 1;
 	}
 	else
 	{
 		/* not overflow safe */
 		vecL.xE = xL << -matPtrA->bbpE;
 		vecL.yE = yL << -matPtrA->bbpE;
 	}

 	return vecL;
 }

 /* ------------------------------------------------------------------------- */

 struct bts_Flt16Vec2D bts_Flt16Mat2D_mapFlt( const struct bts_Flt16Mat2D* matPtrA,
 								             const struct bts_Flt16Vec2D* vecPtrA )
 {
 	int32 xL = ( int32 ) matPtrA->xxE * vecPtrA->xE + ( int32 ) matPtrA->xyE * vecPtrA->yE;
 	int32 yL = ( int32 ) matPtrA->yxE * vecPtrA->xE + ( int32 ) matPtrA->yyE * vecPtrA->yE;
 	int32 bbpL = matPtrA->bbpE + vecPtrA->bbpE;
 	return bts_Flt16Vec2D_create32( xL, yL, bbpL );
 }

 /* ------------------------------------------------------------------------- */

 struct bts_Flt16Mat2D bts_Flt16Mat2D_mul( const struct bts_Flt16Mat2D* mat1PtrA,
 								          const struct bts_Flt16Mat2D* mat2PtrA )
 {
 	return bts_Flt16Mat2D_create32( ( int32 ) mat1PtrA->xxE * mat2PtrA->xxE + ( int32 ) mat1PtrA->xyE * mat2PtrA->yxE,
 									( int32 ) mat1PtrA->xxE * mat2PtrA->xyE + ( int32 ) mat1PtrA->xyE * mat2PtrA->yyE,
 									( int32 ) mat1PtrA->yxE * mat2PtrA->xxE + ( int32 ) mat1PtrA->yyE * mat2PtrA->yxE,
 									( int32 ) mat1PtrA->yxE * mat2PtrA->xyE + ( int32 ) mat1PtrA->yyE * mat2PtrA->yyE,
 									mat1PtrA->bbpE + mat2PtrA->bbpE );
 }

 /* ------------------------------------------------------------------------- */

 struct bts_Flt16Mat2D* bts_Flt16Mat2D_mulTo( struct bts_Flt16Mat2D* mat1PtrA,
 				                             const struct bts_Flt16Mat2D* mat2PtrA )
 {
 	*mat1PtrA = bts_Flt16Mat2D_mul( mat1PtrA, mat2PtrA );
 	return mat1PtrA;
 }

 /* ------------------------------------------------------------------------- */

 void bts_Flt16Mat2D_invert( struct bts_Flt16Mat2D* ptrA )
 {
 	int32 detL = ( int32 ) ptrA->xxE * ptrA->yyE - ( int32 ) ptrA->xyE * ptrA->yxE;
 	int32 detExpL = bbs_intLog2( detL );
 	int32 dShrL = 0;
 	if( detExpL > 15 )
 	{
 		dShrL = detExpL - 15;
 		detL = ( ( detL >> ( dShrL - 1 ) ) + 1 ) >> 1;
 	}

 	if( detL == 0 )
 	{
 		ptrA->xxE = ptrA->yyE = ptrA->xyE = ptrA->yxE = 0;
 	}
 	else
 	{
 		/* bbp: bbpE + 16 - ( bbpE * 2 - dShrL ) = 16 + dShrL - bbpE */
 		int32 xxL = ( ( int32 )ptrA->xxE << 16 ) / detL;
 		int32 xyL = ( ( int32 )ptrA->xyE << 16 ) / detL;
 		int32 yxL = ( ( int32 )ptrA->yxE << 16 ) / detL;
 		int32 yyL = ( ( int32 )ptrA->yyE << 16 ) / detL;
 		*ptrA = bts_Flt16Mat2D_create32( xxL, -xyL, -yxL, yyL, 16 + dShrL - ptrA->bbpE );
 	}
 }

 /* ------------------------------------------------------------------------- */

 struct bts_Flt16Mat2D bts_Flt16Mat2D_inverted( const struct bts_Flt16Mat2D* ptrA )
 {
 	struct bts_Flt16Mat2D matL = *ptrA;
 	bts_Flt16Mat2D_invert( &matL );
 	return matL;
 }

 /* ------------------------------------------------------------------------- */

 /* ========================================================================= */
	/*
	* Copyright (C) 2008 The Android Open Source Project
	*
	* Licensed under the Apache License, Version 2.0 (the "License");
	* you may not use this file except in compliance with the License.
	* You may obtain a copy of the License at
	*
	* http://www.apache.org/licenses/LICENSE-2.0
	*
	* Unless required by applicable law or agreed to in writing, software
	* distributed under the License is distributed on an "AS IS" BASIS,
	* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
	* See the License for the specific language governing permissions and
	* limitations under the License.
	*/

	/* ---- includes ----------------------------------------------------------- */

	#include "b_TensorEm/Flt16Mat2D.h"
	#include "b_TensorEm/Functions.h"
	#include "b_BasicEm/Math.h"

	/* ------------------------------------------------------------------------- */

	/* ========================================================================= */
	/* */
	/* ---- \ghd{ auxiliary functions } ---------------------------------------- */
	/* */
	/* ========================================================================= */

	/* ------------------------------------------------------------------------- */

	/* ========================================================================= */
	/* */
	/* ---- \ghd{ constructor / destructor } ----------------------------------- */
	/* */
	/* ========================================================================= */

	/* ------------------------------------------------------------------------- */

	void bts_Flt16Mat2D_init( struct bts_Flt16Mat2D* ptrA )
	{
	ptrA->bbpE = 0;
	ptrA->xxE = 0;
	ptrA->xyE = 0;
	ptrA->yxE = 0;
	ptrA->yyE = 0;
	}

	/* ------------------------------------------------------------------------- */

	void bts_Flt16Mat2D_exit( struct bts_Flt16Mat2D* ptrA )
	{
	ptrA->bbpE = 0;
	ptrA->xxE = 0;
	ptrA->xyE = 0;
	ptrA->yxE = 0;
	ptrA->yyE = 0;
	}

	/* ------------------------------------------------------------------------- */

	/* ========================================================================= */
	/* */
	/* ---- \ghd{ operators } -------------------------------------------------- */
	/* */
	/* ========================================================================= */

	/* ------------------------------------------------------------------------- */

	void bts_Flt16Mat2D_copy( struct bts_Flt16Mat2D* ptrA, const struct bts_Flt16Mat2D* srcPtrA )
	{
	ptrA->bbpE = srcPtrA->bbpE;
	ptrA->xxE = srcPtrA->xxE;
	ptrA->xyE = srcPtrA->xyE;
	ptrA->yxE = srcPtrA->yxE;
	ptrA->yyE = srcPtrA->yyE;
	}

	/* ------------------------------------------------------------------------- */

	flag bts_Flt16Mat2D_equal( const struct bts_Flt16Mat2D* ptrA, const struct bts_Flt16Mat2D* srcPtrA )
	{
	int32 bbpDiffL = ptrA->bbpE - srcPtrA->bbpE;
	if( bbpDiffL == 0 )
	{
	if( ptrA->xxE != srcPtrA->xxE ) return FALSE;
	if( ptrA->xyE != srcPtrA->xyE ) return FALSE;
	if( ptrA->yxE != srcPtrA->yxE ) return FALSE;
	if( ptrA->yyE != srcPtrA->yyE ) return FALSE;
	return TRUE;
	}

	if( bbpDiffL > 0 )
	{
	int32 xxL = ( int32 ) srcPtrA->xxE << bbpDiffL;
	int32 xyL = ( int32 ) srcPtrA->xyE << bbpDiffL;
	int32 yxL = ( int32 ) srcPtrA->yxE << bbpDiffL;
	int32 yyL = ( int32 ) srcPtrA->yyE << bbpDiffL;

	if( ptrA->xxE != xxL ) return FALSE;
	if( ptrA->xyE != xyL ) return FALSE;
	if( ptrA->yxE != yxL ) return FALSE;
	if( ptrA->yyE != yyL ) return FALSE;

	/* check if bits were lost by the shifting */
	if( srcPtrA->xxE != ( xxL >> bbpDiffL ) ) return FALSE;
	if( srcPtrA->xyE != ( xyL >> bbpDiffL ) ) return FALSE;
	if( srcPtrA->yxE != ( yxL >> bbpDiffL ) ) return FALSE;
	if( srcPtrA->yyE != ( yyL >> bbpDiffL ) ) return FALSE;

	return TRUE;
	}

	if( bbpDiffL < 0 )
	{
	int32 xxL = ( int32 ) ptrA->xxE << -bbpDiffL;
	int32 xyL = ( int32 ) ptrA->xyE << -bbpDiffL;
	int32 yxL = ( int32 ) ptrA->yxE << -bbpDiffL;
	int32 yyL = ( int32 ) ptrA->yyE << -bbpDiffL;

	if( xxL != srcPtrA->xxE ) return FALSE;
	if( xyL != srcPtrA->xyE ) return FALSE;
	if( yxL != srcPtrA->yxE ) return FALSE;
	if( yyL != srcPtrA->yyE ) return FALSE;

	/* check if bits were lost by the shifting */
	if( ptrA->xxE != ( xxL >> -bbpDiffL ) ) return FALSE;
	if( ptrA->xyE != ( xyL >> -bbpDiffL ) ) return FALSE;
	if( ptrA->yxE != ( yxL >> -bbpDiffL ) ) return FALSE;
	if( ptrA->yyE != ( yyL >> -bbpDiffL ) ) return FALSE;

	return TRUE;
	}

	return TRUE;
	}

	/* ========================================================================= */
	/* */
	/* ---- \ghd{ query functions } -------------------------------------------- */
	/* */
	/* ========================================================================= */

	/* ------------------------------------------------------------------------- */

	/* ========================================================================= */
	/* */
	/* ---- \ghd{ modify functions } ------------------------------------------- */
	/* */
	/* ========================================================================= */

	/* ------------------------------------------------------------------------- */

	/* ========================================================================= */
	/* */
	/* ---- \ghd{ I/O } -------------------------------------------------------- */
	/* */
	/* ========================================================================= */

	/* ------------------------------------------------------------------------- */

	/* ========================================================================= */
	/* */
	/* ---- \ghd{ exec functions } --------------------------------------------- */
	/* */
	/* ========================================================================= */

	/* ------------------------------------------------------------------------- */

	uint32 bts_Flt16Mat2D_det( const struct bts_Flt16Mat2D* ptrA )
	{
	/* This could be negativ, in theory. But almost always det > 0 for us,
	matrix is a rotation or scaling matrix.
	Then uint32 makes sure there is no overflow. */
	uint32 detL = ( int32 ) ptrA->xxE * ptrA->yyE - ( int32 ) ptrA->xyE * ptrA->yxE;
	return detL;
	}

	/* ------------------------------------------------------------------------- */

	struct bts_Flt16Mat2D bts_Flt16Mat2D_createIdentity()
	{
	struct bts_Flt16Mat2D matL = { 1 << 14, 0, 0, 1 << 14, 14 };
	return matL;
	}

	/* ------------------------------------------------------------------------- */

	struct bts_Flt16Mat2D bts_Flt16Mat2D_createRotation( phase16 angleA )
	{
	int16 cL = bbs_cos16( angleA );
	int16 sL = bbs_sin16( angleA );
	struct bts_Flt16Mat2D matL;
	matL.xxE = cL;
	matL.xyE = -sL;
	matL.yxE = sL;
	matL.yyE = cL;
	matL.bbpE = 14;
	return matL;
	}

	/* ------------------------------------------------------------------------- */

	struct bts_Flt16Mat2D bts_Flt16Mat2D_createScale( int32 scaleA, int32 scaleBbpA )
	{
	struct bts_Flt16Mat2D matL = bts_Flt16Mat2D_createIdentity();
	bts_Flt16Mat2D_scale( &matL, scaleA, scaleBbpA );
	return matL;
	}

	/* ------------------------------------------------------------------------- */

	struct bts_Flt16Mat2D bts_Flt16Mat2D_createRigid( phase16 angleA, int32 scaleA, int32 scaleBbpA )
	{
	struct bts_Flt16Mat2D matL = bts_Flt16Mat2D_createRotation( angleA );
	bts_Flt16Mat2D_scale( &matL, scaleA, scaleBbpA );
	return matL;
	}

	/* ------------------------------------------------------------------------- */

	struct bts_Flt16Mat2D bts_Flt16Mat2D_create16( int16 xxA, int16 xyA, int16 yxA, int16 yyA, int16 bbpA )
	{
	struct bts_Flt16Mat2D matL;
	matL.xxE = xxA;
	matL.xyE = xyA;
	matL.yxE = yxA;
	matL.yyE = yyA;
	matL.bbpE = bbpA;
	return matL;
	}

	/* ------------------------------------------------------------------------- */

	struct bts_Flt16Mat2D bts_Flt16Mat2D_create32( int32 xxA, int32 xyA, int32 yxA, int32 yyA, int32 bbpA )
	{
	struct bts_Flt16Mat2D matL;

	if( ( xxA \| xyA \| yxA \| yyA ) == 0 )
	{
	matL.xxE = 0;
	matL.xyE = 0;
	matL.yxE = 0;
	matL.yyE = 0;
	matL.bbpE = 0;
	}
	else
	{
	int32 shiftL = bts_maxAbsIntLog2Of4( xxA, xyA, yxA, yyA ) - 13;

	if( shiftL > 0 )
	{
	int32 sh1L = shiftL - 1;
	matL.xxE = ( ( xxA >> sh1L ) + 1 ) >> 1;
	matL.xyE = ( ( xyA >> sh1L ) + 1 ) >> 1;
	matL.yxE = ( ( yxA >> sh1L ) + 1 ) >> 1;
	matL.yyE = ( ( yyA >> sh1L ) + 1 ) >> 1;
	}
	else
	{
	matL.xxE = xxA << -shiftL;
	matL.xyE = xyA << -shiftL;
	matL.yxE = yxA << -shiftL;
	matL.yyE = yyA << -shiftL;
	}

	matL.bbpE = bbpA - shiftL;
	}

	return matL;
	}

	/* ------------------------------------------------------------------------- */

	void bts_Flt16Mat2D_scale( struct bts_Flt16Mat2D* ptrA, int32 scaleA, int32 scaleBbpA )
	{
	/* fit scale in 15 bit */
	uint32 scaleExpL = bts_absIntLog2( scaleA );
	if( scaleExpL > 14 )
	{
	int32 shiftL = scaleExpL - 14;
	scaleA = ( ( scaleA >> ( shiftL - 1 ) ) + 1 ) >> 1;
	scaleBbpA -= shiftL;
	}

	ptrA = bts_Flt16Mat2D_create32( (int32)ptrA->xxE scaleA,
	(int32)ptrA->xyE * scaleA,
	(int32)ptrA->yxE * scaleA,
	(int32)ptrA->yyE * scaleA,
	ptrA->bbpE + scaleBbpA );
	}

	/* ------------------------------------------------------------------------- */

	struct bts_Int16Vec2D bts_Flt16Mat2D_map( const struct bts_Flt16Mat2D* matPtrA,
	const struct bts_Int16Vec2D* vecPtrA )
	{
	struct bts_Int16Vec2D vecL;

	int32 xL = ( int32 ) matPtrA->xxE * vecPtrA->xE + ( int32 ) matPtrA->xyE * vecPtrA->yE;
	int32 yL = ( int32 ) matPtrA->yxE * vecPtrA->xE + ( int32 ) matPtrA->yyE * vecPtrA->yE;

	if( matPtrA->bbpE > 0 )
	{
	int32 sh1L = matPtrA->bbpE - 1;
	vecL.xE = ( ( xL >> sh1L ) + 1 ) >> 1;
	vecL.yE = ( ( yL >> sh1L ) + 1 ) >> 1;
	}
	else
	{
	/* not overflow safe */
	vecL.xE = xL << -matPtrA->bbpE;
	vecL.yE = yL << -matPtrA->bbpE;
	}

	return vecL;
	}

	/* ------------------------------------------------------------------------- */

	struct bts_Flt16Vec2D bts_Flt16Mat2D_mapFlt( const struct bts_Flt16Mat2D* matPtrA,
	const struct bts_Flt16Vec2D* vecPtrA )
	{
	int32 xL = ( int32 ) matPtrA->xxE * vecPtrA->xE + ( int32 ) matPtrA->xyE * vecPtrA->yE;
	int32 yL = ( int32 ) matPtrA->yxE * vecPtrA->xE + ( int32 ) matPtrA->yyE * vecPtrA->yE;
	int32 bbpL = matPtrA->bbpE + vecPtrA->bbpE;
	return bts_Flt16Vec2D_create32( xL, yL, bbpL );
	}

	/* ------------------------------------------------------------------------- */

	struct bts_Flt16Mat2D bts_Flt16Mat2D_mul( const struct bts_Flt16Mat2D* mat1PtrA,
	const struct bts_Flt16Mat2D* mat2PtrA )
	{
	return bts_Flt16Mat2D_create32( ( int32 ) mat1PtrA->xxE * mat2PtrA->xxE + ( int32 ) mat1PtrA->xyE * mat2PtrA->yxE,
	( int32 ) mat1PtrA->xxE * mat2PtrA->xyE + ( int32 ) mat1PtrA->xyE * mat2PtrA->yyE,
	( int32 ) mat1PtrA->yxE * mat2PtrA->xxE + ( int32 ) mat1PtrA->yyE * mat2PtrA->yxE,
	( int32 ) mat1PtrA->yxE * mat2PtrA->xyE + ( int32 ) mat1PtrA->yyE * mat2PtrA->yyE,
	mat1PtrA->bbpE + mat2PtrA->bbpE );
	}

	/* ------------------------------------------------------------------------- */

	struct bts_Flt16Mat2D* bts_Flt16Mat2D_mulTo( struct bts_Flt16Mat2D* mat1PtrA,
	const struct bts_Flt16Mat2D* mat2PtrA )
	{
	*mat1PtrA = bts_Flt16Mat2D_mul( mat1PtrA, mat2PtrA );
	return mat1PtrA;
	}

	/* ------------------------------------------------------------------------- */

	void bts_Flt16Mat2D_invert( struct bts_Flt16Mat2D* ptrA )
	{
	int32 detL = ( int32 ) ptrA->xxE * ptrA->yyE - ( int32 ) ptrA->xyE * ptrA->yxE;
	int32 detExpL = bbs_intLog2( detL );
	int32 dShrL = 0;
	if( detExpL > 15 )
	{
	dShrL = detExpL - 15;
	detL = ( ( detL >> ( dShrL - 1 ) ) + 1 ) >> 1;
	}

	if( detL == 0 )
	{
	ptrA->xxE = ptrA->yyE = ptrA->xyE = ptrA->yxE = 0;
	}
	else
	{
	/* bbp: bbpE + 16 - ( bbpE * 2 - dShrL ) = 16 + dShrL - bbpE */
	int32 xxL = ( ( int32 )ptrA->xxE << 16 ) / detL;
	int32 xyL = ( ( int32 )ptrA->xyE << 16 ) / detL;
	int32 yxL = ( ( int32 )ptrA->yxE << 16 ) / detL;
	int32 yyL = ( ( int32 )ptrA->yyE << 16 ) / detL;
	*ptrA = bts_Flt16Mat2D_create32( xxL, -xyL, -yxL, yyL, 16 + dShrL - ptrA->bbpE );
	}
	}

	/* ------------------------------------------------------------------------- */

	struct bts_Flt16Mat2D bts_Flt16Mat2D_inverted( const struct bts_Flt16Mat2D* ptrA )
	{
	struct bts_Flt16Mat2D matL = *ptrA;
	bts_Flt16Mat2D_invert( &matL );
	return matL;
	}

	/* ------------------------------------------------------------------------- */

	/* ========================================================================= */