Matrix.cpp 139 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464246524662467246824692470247124722473247424752476247724782479248024812482248324842485248624872488248924902491249224932494249524962497249824992500250125022503250425052506250725082509251025112512251325142515251625172518251925202521252225232524252525262527252825292530253125322533253425352536253725382539254025412542254325442545254625472548254925502551255225532554255525562557255825592560256125622563256425652566256725682569257025712572257325742575257625772578257925802581258225832584258525862587258825892590259125922593259425952596259725982599260026012602260326042605260626072608260926102611261226132614261526162617261826192620262126222623262426252626262726282629263026312632263326342635263626372638263926402641264226432644264526462647264826492650265126522653265426552656265726582659266026612662266326642665266626672668266926702671267226732674267526762677267826792680268126822683268426852686268726882689269026912692269326942695269626972698269927002701270227032704270527062707270827092710271127122713271427152716271727182719272027212722272327242725272627272728272927302731273227332734273527362737273827392740274127422743274427452746274727482749275027512752275327542755275627572758275927602761276227632764276527662767276827692770277127722773277427752776277727782779278027812782278327842785278627872788278927902791279227932794279527962797279827992800280128022803280428052806280728082809281028112812281328142815281628172818281928202821282228232824282528262827282828292830283128322833283428352836283728382839284028412842284328442845284628472848284928502851285228532854285528562857285828592860286128622863286428652866286728682869287028712872287328742875287628772878287928802881288228832884288528862887288828892890289128922893289428952896289728982899290029012902290329042905290629072908290929102911291229132914291529162917
  1. /*
  2. ===========================================================================
  3. Doom 3 BFG Edition GPL Source Code
  4. Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company.
  5. This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").
  6. Doom 3 BFG Edition Source Code is free software: you can redistribute it and/or modify
  7. it under the terms of the GNU General Public License as published by
  8. the Free Software Foundation, either version 3 of the License, or
  9. (at your option) any later version.
  10. Doom 3 BFG Edition Source Code is distributed in the hope that it will be useful,
  11. but WITHOUT ANY WARRANTY; without even the implied warranty of
  12. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  13. GNU General Public License for more details.
  14. You should have received a copy of the GNU General Public License
  15. along with Doom 3 BFG Edition Source Code. If not, see <http://www.gnu.org/licenses/>.
  16. In addition, the Doom 3 BFG Edition Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 BFG Edition Source Code. If not, please request a copy in writing from id Software at the address below.
  17. If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
  18. ===========================================================================
  19. */
  20. #pragma hdrstop
  21. #include "../precompiled.h"
  22. //===============================================================
  23. //
  24. // idMat2
  25. //
  26. //===============================================================
  27. idMat2 mat2_zero( idVec2( 0, 0 ), idVec2( 0, 0 ) );
  28. idMat2 mat2_identity( idVec2( 1, 0 ), idVec2( 0, 1 ) );
  29. /*
  30. ============
  31. idMat2::InverseSelf
  32. ============
  33. */
  34. bool idMat2::InverseSelf() {
  35. // 2+4 = 6 multiplications
  36. // 1 division
  37. double det, invDet, a;
  38. det = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  39. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  40. return false;
  41. }
  42. invDet = 1.0f / det;
  43. a = mat[0][0];
  44. mat[0][0] = mat[1][1] * invDet;
  45. mat[0][1] = - mat[0][1] * invDet;
  46. mat[1][0] = - mat[1][0] * invDet;
  47. mat[1][1] = a * invDet;
  48. return true;
  49. }
  50. /*
  51. ============
  52. idMat2::InverseFastSelf
  53. ============
  54. */
  55. bool idMat2::InverseFastSelf() {
  56. #if 1
  57. // 2+4 = 6 multiplications
  58. // 1 division
  59. double det, invDet, a;
  60. det = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  61. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  62. return false;
  63. }
  64. invDet = 1.0f / det;
  65. a = mat[0][0];
  66. mat[0][0] = mat[1][1] * invDet;
  67. mat[0][1] = - mat[0][1] * invDet;
  68. mat[1][0] = - mat[1][0] * invDet;
  69. mat[1][1] = a * invDet;
  70. return true;
  71. #else
  72. // 2*4 = 8 multiplications
  73. // 2 division
  74. float *mat = reinterpret_cast<float *>(this);
  75. double d, di;
  76. float s;
  77. di = mat[0];
  78. s = di;
  79. mat[0*2+0] = d = 1.0f / di;
  80. mat[0*2+1] *= d;
  81. d = -d;
  82. mat[1*2+0] *= d;
  83. d = mat[1*2+0] * di;
  84. mat[1*2+1] += mat[0*2+1] * d;
  85. di = mat[1*2+1];
  86. s *= di;
  87. mat[1*2+1] = d = 1.0f / di;
  88. mat[1*2+0] *= d;
  89. d = -d;
  90. mat[0*2+1] *= d;
  91. d = mat[0*2+1] * di;
  92. mat[0*2+0] += mat[1*2+0] * d;
  93. return ( s != 0.0f && !IEEE_FLT_IS_NAN( s ) );
  94. #endif
  95. }
  96. /*
  97. =============
  98. idMat2::ToString
  99. =============
  100. */
  101. const char *idMat2::ToString( int precision ) const {
  102. return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
  103. }
  104. //===============================================================
  105. //
  106. // idMat3
  107. //
  108. //===============================================================
  109. idMat3 mat3_zero( idVec3( 0, 0, 0 ), idVec3( 0, 0, 0 ), idVec3( 0, 0, 0 ) );
  110. idMat3 mat3_identity( idVec3( 1, 0, 0 ), idVec3( 0, 1, 0 ), idVec3( 0, 0, 1 ) );
  111. /*
  112. ========================
  113. idMat3::ToAngles
  114. returns the pitch/yaw/roll each in the range [-180, 180] degrees
  115. ========================
  116. */
  117. idAngles idMat3::ToAngles() const {
  118. idAngles angles;
  119. float s = idMath::Sqrt( mat[0][0] * mat[0][0] + mat[0][1] * mat[0][1] );
  120. if ( s > idMath::FLT_EPSILON ) {
  121. angles.pitch = RAD2DEG( - idMath::ATan( mat[0][2], s ) );
  122. angles.yaw = RAD2DEG( idMath::ATan( mat[0][1], mat[0][0] ) );
  123. angles.roll = RAD2DEG( idMath::ATan( mat[1][2], mat[2][2] ) );
  124. } else {
  125. angles.pitch = mat[0][2] < 0.0f ? 90.0f : -90.0f;
  126. angles.yaw = RAD2DEG( - idMath::ATan( mat[1][0], mat[1][1] ) );
  127. angles.roll = 0.0f;
  128. }
  129. return angles;
  130. }
  131. /*
  132. ============
  133. idMat3::ToQuat
  134. ============
  135. */
  136. idQuat idMat3::ToQuat() const {
  137. idQuat q;
  138. float trace;
  139. float s;
  140. float t;
  141. int i;
  142. int j;
  143. int k;
  144. static int next[ 3 ] = { 1, 2, 0 };
  145. trace = mat[ 0 ][ 0 ] + mat[ 1 ][ 1 ] + mat[ 2 ][ 2 ];
  146. if ( trace > 0.0f ) {
  147. t = trace + 1.0f;
  148. s = idMath::InvSqrt( t ) * 0.5f;
  149. q[3] = s * t;
  150. q[0] = ( mat[ 2 ][ 1 ] - mat[ 1 ][ 2 ] ) * s;
  151. q[1] = ( mat[ 0 ][ 2 ] - mat[ 2 ][ 0 ] ) * s;
  152. q[2] = ( mat[ 1 ][ 0 ] - mat[ 0 ][ 1 ] ) * s;
  153. } else {
  154. i = 0;
  155. if ( mat[ 1 ][ 1 ] > mat[ 0 ][ 0 ] ) {
  156. i = 1;
  157. }
  158. if ( mat[ 2 ][ 2 ] > mat[ i ][ i ] ) {
  159. i = 2;
  160. }
  161. j = next[ i ];
  162. k = next[ j ];
  163. t = ( mat[ i ][ i ] - ( mat[ j ][ j ] + mat[ k ][ k ] ) ) + 1.0f;
  164. s = idMath::InvSqrt( t ) * 0.5f;
  165. q[i] = s * t;
  166. q[3] = ( mat[ k ][ j ] - mat[ j ][ k ] ) * s;
  167. q[j] = ( mat[ j ][ i ] + mat[ i ][ j ] ) * s;
  168. q[k] = ( mat[ k ][ i ] + mat[ i ][ k ] ) * s;
  169. }
  170. return q;
  171. }
  172. /*
  173. ============
  174. idMat3::ToCQuat
  175. ============
  176. */
  177. idCQuat idMat3::ToCQuat() const {
  178. idQuat q = ToQuat();
  179. if ( q.w < 0.0f ) {
  180. return idCQuat( -q.x, -q.y, -q.z );
  181. }
  182. return idCQuat( q.x, q.y, q.z );
  183. }
  184. /*
  185. ============
  186. idMat3::ToRotation
  187. ============
  188. */
  189. idRotation idMat3::ToRotation() const {
  190. idRotation r;
  191. float trace;
  192. float s;
  193. float t;
  194. int i;
  195. int j;
  196. int k;
  197. static int next[ 3 ] = { 1, 2, 0 };
  198. trace = mat[ 0 ][ 0 ] + mat[ 1 ][ 1 ] + mat[ 2 ][ 2 ];
  199. if ( trace > 0.0f ) {
  200. t = trace + 1.0f;
  201. s = idMath::InvSqrt( t ) * 0.5f;
  202. r.angle = s * t;
  203. r.vec[0] = ( mat[ 2 ][ 1 ] - mat[ 1 ][ 2 ] ) * s;
  204. r.vec[1] = ( mat[ 0 ][ 2 ] - mat[ 2 ][ 0 ] ) * s;
  205. r.vec[2] = ( mat[ 1 ][ 0 ] - mat[ 0 ][ 1 ] ) * s;
  206. } else {
  207. i = 0;
  208. if ( mat[ 1 ][ 1 ] > mat[ 0 ][ 0 ] ) {
  209. i = 1;
  210. }
  211. if ( mat[ 2 ][ 2 ] > mat[ i ][ i ] ) {
  212. i = 2;
  213. }
  214. j = next[ i ];
  215. k = next[ j ];
  216. t = ( mat[ i ][ i ] - ( mat[ j ][ j ] + mat[ k ][ k ] ) ) + 1.0f;
  217. s = idMath::InvSqrt( t ) * 0.5f;
  218. r.vec[i] = s * t;
  219. r.angle = ( mat[ k ][ j ] - mat[ j ][ k ] ) * s;
  220. r.vec[j] = ( mat[ j ][ i ] + mat[ i ][ j ] ) * s;
  221. r.vec[k] = ( mat[ k ][ i ] + mat[ i ][ k ] ) * s;
  222. }
  223. r.angle = idMath::ACos( r.angle );
  224. float lengthSqr = r.vec.LengthSqr();
  225. if ( ( idMath::Fabs( r.angle ) < 1e-10f ) || ( lengthSqr < 1e-10f ) ) {
  226. r.vec.Set( 0.0f, 0.0f, 1.0f );
  227. r.angle = 0.0f;
  228. } else {
  229. r.vec *= idMath::InvSqrt( lengthSqr );
  230. r.angle *= 2.0f * idMath::M_RAD2DEG;
  231. }
  232. r.origin.Zero();
  233. r.axis = *this;
  234. r.axisValid = true;
  235. return r;
  236. }
  237. /*
  238. =================
  239. idMat3::ToAngularVelocity
  240. =================
  241. */
  242. idVec3 idMat3::ToAngularVelocity() const {
  243. idRotation rotation = ToRotation();
  244. return rotation.GetVec() * DEG2RAD( rotation.GetAngle() );
  245. }
  246. /*
  247. ============
  248. idMat3::Determinant
  249. ============
  250. */
  251. float idMat3::Determinant() const {
  252. float det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
  253. float det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0];
  254. float det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
  255. return mat[0][0] * det2_12_12 - mat[0][1] * det2_12_02 + mat[0][2] * det2_12_01;
  256. }
  257. /*
  258. ============
  259. idMat3::InverseSelf
  260. ============
  261. */
  262. bool idMat3::InverseSelf() {
  263. // 18+3+9 = 30 multiplications
  264. // 1 division
  265. idMat3 inverse;
  266. double det, invDet;
  267. inverse[0][0] = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
  268. inverse[1][0] = mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2];
  269. inverse[2][0] = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
  270. det = mat[0][0] * inverse[0][0] + mat[0][1] * inverse[1][0] + mat[0][2] * inverse[2][0];
  271. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  272. return false;
  273. }
  274. invDet = 1.0f / det;
  275. inverse[0][1] = mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2];
  276. inverse[0][2] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
  277. inverse[1][1] = mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0];
  278. inverse[1][2] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2];
  279. inverse[2][1] = mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1];
  280. inverse[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  281. mat[0][0] = inverse[0][0] * invDet;
  282. mat[0][1] = inverse[0][1] * invDet;
  283. mat[0][2] = inverse[0][2] * invDet;
  284. mat[1][0] = inverse[1][0] * invDet;
  285. mat[1][1] = inverse[1][1] * invDet;
  286. mat[1][2] = inverse[1][2] * invDet;
  287. mat[2][0] = inverse[2][0] * invDet;
  288. mat[2][1] = inverse[2][1] * invDet;
  289. mat[2][2] = inverse[2][2] * invDet;
  290. return true;
  291. }
  292. /*
  293. ============
  294. idMat3::InverseFastSelf
  295. ============
  296. */
  297. bool idMat3::InverseFastSelf() {
  298. #if 1
  299. // 18+3+9 = 30 multiplications
  300. // 1 division
  301. idMat3 inverse;
  302. double det, invDet;
  303. inverse[0][0] = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
  304. inverse[1][0] = mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2];
  305. inverse[2][0] = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
  306. det = mat[0][0] * inverse[0][0] + mat[0][1] * inverse[1][0] + mat[0][2] * inverse[2][0];
  307. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  308. return false;
  309. }
  310. invDet = 1.0f / det;
  311. inverse[0][1] = mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2];
  312. inverse[0][2] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
  313. inverse[1][1] = mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0];
  314. inverse[1][2] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2];
  315. inverse[2][1] = mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1];
  316. inverse[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  317. mat[0][0] = inverse[0][0] * invDet;
  318. mat[0][1] = inverse[0][1] * invDet;
  319. mat[0][2] = inverse[0][2] * invDet;
  320. mat[1][0] = inverse[1][0] * invDet;
  321. mat[1][1] = inverse[1][1] * invDet;
  322. mat[1][2] = inverse[1][2] * invDet;
  323. mat[2][0] = inverse[2][0] * invDet;
  324. mat[2][1] = inverse[2][1] * invDet;
  325. mat[2][2] = inverse[2][2] * invDet;
  326. return true;
  327. #elif 0
  328. // 3*10 = 30 multiplications
  329. // 3 divisions
  330. float *mat = reinterpret_cast<float *>(this);
  331. float s;
  332. double d, di;
  333. di = mat[0];
  334. s = di;
  335. mat[0] = d = 1.0f / di;
  336. mat[1] *= d;
  337. mat[2] *= d;
  338. d = -d;
  339. mat[3] *= d;
  340. mat[6] *= d;
  341. d = mat[3] * di;
  342. mat[4] += mat[1] * d;
  343. mat[5] += mat[2] * d;
  344. d = mat[6] * di;
  345. mat[7] += mat[1] * d;
  346. mat[8] += mat[2] * d;
  347. di = mat[4];
  348. s *= di;
  349. mat[4] = d = 1.0f / di;
  350. mat[3] *= d;
  351. mat[5] *= d;
  352. d = -d;
  353. mat[1] *= d;
  354. mat[7] *= d;
  355. d = mat[1] * di;
  356. mat[0] += mat[3] * d;
  357. mat[2] += mat[5] * d;
  358. d = mat[7] * di;
  359. mat[6] += mat[3] * d;
  360. mat[8] += mat[5] * d;
  361. di = mat[8];
  362. s *= di;
  363. mat[8] = d = 1.0f / di;
  364. mat[6] *= d;
  365. mat[7] *= d;
  366. d = -d;
  367. mat[2] *= d;
  368. mat[5] *= d;
  369. d = mat[2] * di;
  370. mat[0] += mat[6] * d;
  371. mat[1] += mat[7] * d;
  372. d = mat[5] * di;
  373. mat[3] += mat[6] * d;
  374. mat[4] += mat[7] * d;
  375. return ( s != 0.0f && !IEEE_FLT_IS_NAN( s ) );
  376. #else
  377. // 4*2+4*4 = 24 multiplications
  378. // 2*1 = 2 divisions
  379. idMat2 r0;
  380. float r1[2], r2[2], r3;
  381. float det, invDet;
  382. float *mat = reinterpret_cast<float *>(this);
  383. // r0 = m0.Inverse(); // 2x2
  384. det = mat[0*3+0] * mat[1*3+1] - mat[0*3+1] * mat[1*3+0];
  385. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  386. return false;
  387. }
  388. invDet = 1.0f / det;
  389. r0[0][0] = mat[1*3+1] * invDet;
  390. r0[0][1] = - mat[0*3+1] * invDet;
  391. r0[1][0] = - mat[1*3+0] * invDet;
  392. r0[1][1] = mat[0*3+0] * invDet;
  393. // r1 = r0 * m1; // 2x1 = 2x2 * 2x1
  394. r1[0] = r0[0][0] * mat[0*3+2] + r0[0][1] * mat[1*3+2];
  395. r1[1] = r0[1][0] * mat[0*3+2] + r0[1][1] * mat[1*3+2];
  396. // r2 = m2 * r1; // 1x1 = 1x2 * 2x1
  397. r2[0] = mat[2*3+0] * r1[0] + mat[2*3+1] * r1[1];
  398. // r3 = r2 - m3; // 1x1 = 1x1 - 1x1
  399. r3 = r2[0] - mat[2*3+2];
  400. // r3.InverseSelf();
  401. if ( idMath::Fabs( r3 ) < MATRIX_INVERSE_EPSILON ) {
  402. return false;
  403. }
  404. r3 = 1.0f / r3;
  405. // r2 = m2 * r0; // 1x2 = 1x2 * 2x2
  406. r2[0] = mat[2*3+0] * r0[0][0] + mat[2*3+1] * r0[1][0];
  407. r2[1] = mat[2*3+0] * r0[0][1] + mat[2*3+1] * r0[1][1];
  408. // m2 = r3 * r2; // 1x2 = 1x1 * 1x2
  409. mat[2*3+0] = r3 * r2[0];
  410. mat[2*3+1] = r3 * r2[1];
  411. // m0 = r0 - r1 * m2; // 2x2 - 2x1 * 1x2
  412. mat[0*3+0] = r0[0][0] - r1[0] * mat[2*3+0];
  413. mat[0*3+1] = r0[0][1] - r1[0] * mat[2*3+1];
  414. mat[1*3+0] = r0[1][0] - r1[1] * mat[2*3+0];
  415. mat[1*3+1] = r0[1][1] - r1[1] * mat[2*3+1];
  416. // m1 = r1 * r3; // 2x1 = 2x1 * 1x1
  417. mat[0*3+2] = r1[0] * r3;
  418. mat[1*3+2] = r1[1] * r3;
  419. // m3 = -r3;
  420. mat[2*3+2] = -r3;
  421. return true;
  422. #endif
  423. }
  424. /*
  425. ============
  426. idMat3::InertiaTranslate
  427. ============
  428. */
  429. idMat3 idMat3::InertiaTranslate( const float mass, const idVec3 &centerOfMass, const idVec3 &translation ) const {
  430. idMat3 m;
  431. idVec3 newCenter;
  432. newCenter = centerOfMass + translation;
  433. m[0][0] = mass * ( ( centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2] )
  434. - ( newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2] ) );
  435. m[1][1] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2] )
  436. - ( newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2] ) );
  437. m[2][2] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1] )
  438. - ( newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1] ) );
  439. m[0][1] = m[1][0] = mass * ( newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1] );
  440. m[1][2] = m[2][1] = mass * ( newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2] );
  441. m[0][2] = m[2][0] = mass * ( newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2] );
  442. return (*this) + m;
  443. }
  444. /*
  445. ============
  446. idMat3::InertiaTranslateSelf
  447. ============
  448. */
  449. idMat3 &idMat3::InertiaTranslateSelf( const float mass, const idVec3 &centerOfMass, const idVec3 &translation ) {
  450. idMat3 m;
  451. idVec3 newCenter;
  452. newCenter = centerOfMass + translation;
  453. m[0][0] = mass * ( ( centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2] )
  454. - ( newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2] ) );
  455. m[1][1] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2] )
  456. - ( newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2] ) );
  457. m[2][2] = mass * ( ( centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1] )
  458. - ( newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1] ) );
  459. m[0][1] = m[1][0] = mass * ( newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1] );
  460. m[1][2] = m[2][1] = mass * ( newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2] );
  461. m[0][2] = m[2][0] = mass * ( newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2] );
  462. (*this) += m;
  463. return (*this);
  464. }
  465. /*
  466. ============
  467. idMat3::InertiaRotate
  468. ============
  469. */
  470. idMat3 idMat3::InertiaRotate( const idMat3 &rotation ) const {
  471. // NOTE: the rotation matrix is stored column-major
  472. return rotation.Transpose() * (*this) * rotation;
  473. }
  474. /*
  475. ============
  476. idMat3::InertiaRotateSelf
  477. ============
  478. */
  479. idMat3 &idMat3::InertiaRotateSelf( const idMat3 &rotation ) {
  480. // NOTE: the rotation matrix is stored column-major
  481. *this = rotation.Transpose() * (*this) * rotation;
  482. return *this;
  483. }
  484. /*
  485. =============
  486. idMat3::ToString
  487. =============
  488. */
  489. const char *idMat3::ToString( int precision ) const {
  490. return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
  491. }
  492. //===============================================================
  493. //
  494. // idMat4
  495. //
  496. //===============================================================
  497. idMat4 mat4_zero( idVec4( 0, 0, 0, 0 ), idVec4( 0, 0, 0, 0 ), idVec4( 0, 0, 0, 0 ), idVec4( 0, 0, 0, 0 ) );
  498. idMat4 mat4_identity( idVec4( 1, 0, 0, 0 ), idVec4( 0, 1, 0, 0 ), idVec4( 0, 0, 1, 0 ), idVec4( 0, 0, 0, 1 ) );
  499. /*
  500. ============
  501. idMat4::Transpose
  502. ============
  503. */
  504. idMat4 idMat4::Transpose() const {
  505. idMat4 transpose;
  506. int i, j;
  507. for( i = 0; i < 4; i++ ) {
  508. for( j = 0; j < 4; j++ ) {
  509. transpose[ i ][ j ] = mat[ j ][ i ];
  510. }
  511. }
  512. return transpose;
  513. }
  514. /*
  515. ============
  516. idMat4::TransposeSelf
  517. ============
  518. */
  519. idMat4 &idMat4::TransposeSelf() {
  520. float temp;
  521. int i, j;
  522. for( i = 0; i < 4; i++ ) {
  523. for( j = i + 1; j < 4; j++ ) {
  524. temp = mat[ i ][ j ];
  525. mat[ i ][ j ] = mat[ j ][ i ];
  526. mat[ j ][ i ] = temp;
  527. }
  528. }
  529. return *this;
  530. }
  531. /*
  532. ============
  533. idMat4::Determinant
  534. ============
  535. */
  536. float idMat4::Determinant() const {
  537. // 2x2 sub-determinants
  538. float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  539. float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
  540. float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
  541. float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
  542. float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
  543. float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];
  544. // 3x3 sub-determinants
  545. float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
  546. float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
  547. float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
  548. float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;
  549. return ( - det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3] );
  550. }
  551. /*
  552. ============
  553. idMat4::InverseSelf
  554. ============
  555. */
  556. bool idMat4::InverseSelf() {
  557. // 84+4+16 = 104 multiplications
  558. // 1 division
  559. double det, invDet;
  560. // 2x2 sub-determinants required to calculate 4x4 determinant
  561. float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  562. float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
  563. float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
  564. float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
  565. float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
  566. float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];
  567. // 3x3 sub-determinants required to calculate 4x4 determinant
  568. float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
  569. float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
  570. float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
  571. float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;
  572. det = ( - det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3] );
  573. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  574. return false;
  575. }
  576. invDet = 1.0f / det;
  577. // remaining 2x2 sub-determinants
  578. float det2_03_01 = mat[0][0] * mat[3][1] - mat[0][1] * mat[3][0];
  579. float det2_03_02 = mat[0][0] * mat[3][2] - mat[0][2] * mat[3][0];
  580. float det2_03_03 = mat[0][0] * mat[3][3] - mat[0][3] * mat[3][0];
  581. float det2_03_12 = mat[0][1] * mat[3][2] - mat[0][2] * mat[3][1];
  582. float det2_03_13 = mat[0][1] * mat[3][3] - mat[0][3] * mat[3][1];
  583. float det2_03_23 = mat[0][2] * mat[3][3] - mat[0][3] * mat[3][2];
  584. float det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
  585. float det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0];
  586. float det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
  587. float det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1];
  588. float det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
  589. float det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2];
  590. // remaining 3x3 sub-determinants
  591. float det3_203_012 = mat[2][0] * det2_03_12 - mat[2][1] * det2_03_02 + mat[2][2] * det2_03_01;
  592. float det3_203_013 = mat[2][0] * det2_03_13 - mat[2][1] * det2_03_03 + mat[2][3] * det2_03_01;
  593. float det3_203_023 = mat[2][0] * det2_03_23 - mat[2][2] * det2_03_03 + mat[2][3] * det2_03_02;
  594. float det3_203_123 = mat[2][1] * det2_03_23 - mat[2][2] * det2_03_13 + mat[2][3] * det2_03_12;
  595. float det3_213_012 = mat[2][0] * det2_13_12 - mat[2][1] * det2_13_02 + mat[2][2] * det2_13_01;
  596. float det3_213_013 = mat[2][0] * det2_13_13 - mat[2][1] * det2_13_03 + mat[2][3] * det2_13_01;
  597. float det3_213_023 = mat[2][0] * det2_13_23 - mat[2][2] * det2_13_03 + mat[2][3] * det2_13_02;
  598. float det3_213_123 = mat[2][1] * det2_13_23 - mat[2][2] * det2_13_13 + mat[2][3] * det2_13_12;
  599. float det3_301_012 = mat[3][0] * det2_01_12 - mat[3][1] * det2_01_02 + mat[3][2] * det2_01_01;
  600. float det3_301_013 = mat[3][0] * det2_01_13 - mat[3][1] * det2_01_03 + mat[3][3] * det2_01_01;
  601. float det3_301_023 = mat[3][0] * det2_01_23 - mat[3][2] * det2_01_03 + mat[3][3] * det2_01_02;
  602. float det3_301_123 = mat[3][1] * det2_01_23 - mat[3][2] * det2_01_13 + mat[3][3] * det2_01_12;
  603. mat[0][0] = - det3_213_123 * invDet;
  604. mat[1][0] = + det3_213_023 * invDet;
  605. mat[2][0] = - det3_213_013 * invDet;
  606. mat[3][0] = + det3_213_012 * invDet;
  607. mat[0][1] = + det3_203_123 * invDet;
  608. mat[1][1] = - det3_203_023 * invDet;
  609. mat[2][1] = + det3_203_013 * invDet;
  610. mat[3][1] = - det3_203_012 * invDet;
  611. mat[0][2] = + det3_301_123 * invDet;
  612. mat[1][2] = - det3_301_023 * invDet;
  613. mat[2][2] = + det3_301_013 * invDet;
  614. mat[3][2] = - det3_301_012 * invDet;
  615. mat[0][3] = - det3_201_123 * invDet;
  616. mat[1][3] = + det3_201_023 * invDet;
  617. mat[2][3] = - det3_201_013 * invDet;
  618. mat[3][3] = + det3_201_012 * invDet;
  619. return true;
  620. }
  621. /*
  622. ============
  623. idMat4::InverseFastSelf
  624. ============
  625. */
  626. bool idMat4::InverseFastSelf() {
  627. #if 0
  628. // 84+4+16 = 104 multiplications
  629. // 1 division
  630. double det, invDet;
  631. // 2x2 sub-determinants required to calculate 4x4 determinant
  632. float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
  633. float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
  634. float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
  635. float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
  636. float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
  637. float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];
  638. // 3x3 sub-determinants required to calculate 4x4 determinant
  639. float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
  640. float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
  641. float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
  642. float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;
  643. det = ( - det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3] );
  644. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  645. return false;
  646. }
  647. invDet = 1.0f / det;
  648. // remaining 2x2 sub-determinants
  649. float det2_03_01 = mat[0][0] * mat[3][1] - mat[0][1] * mat[3][0];
  650. float det2_03_02 = mat[0][0] * mat[3][2] - mat[0][2] * mat[3][0];
  651. float det2_03_03 = mat[0][0] * mat[3][3] - mat[0][3] * mat[3][0];
  652. float det2_03_12 = mat[0][1] * mat[3][2] - mat[0][2] * mat[3][1];
  653. float det2_03_13 = mat[0][1] * mat[3][3] - mat[0][3] * mat[3][1];
  654. float det2_03_23 = mat[0][2] * mat[3][3] - mat[0][3] * mat[3][2];
  655. float det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
  656. float det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0];
  657. float det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
  658. float det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1];
  659. float det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
  660. float det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2];
  661. // remaining 3x3 sub-determinants
  662. float det3_203_012 = mat[2][0] * det2_03_12 - mat[2][1] * det2_03_02 + mat[2][2] * det2_03_01;
  663. float det3_203_013 = mat[2][0] * det2_03_13 - mat[2][1] * det2_03_03 + mat[2][3] * det2_03_01;
  664. float det3_203_023 = mat[2][0] * det2_03_23 - mat[2][2] * det2_03_03 + mat[2][3] * det2_03_02;
  665. float det3_203_123 = mat[2][1] * det2_03_23 - mat[2][2] * det2_03_13 + mat[2][3] * det2_03_12;
  666. float det3_213_012 = mat[2][0] * det2_13_12 - mat[2][1] * det2_13_02 + mat[2][2] * det2_13_01;
  667. float det3_213_013 = mat[2][0] * det2_13_13 - mat[2][1] * det2_13_03 + mat[2][3] * det2_13_01;
  668. float det3_213_023 = mat[2][0] * det2_13_23 - mat[2][2] * det2_13_03 + mat[2][3] * det2_13_02;
  669. float det3_213_123 = mat[2][1] * det2_13_23 - mat[2][2] * det2_13_13 + mat[2][3] * det2_13_12;
  670. float det3_301_012 = mat[3][0] * det2_01_12 - mat[3][1] * det2_01_02 + mat[3][2] * det2_01_01;
  671. float det3_301_013 = mat[3][0] * det2_01_13 - mat[3][1] * det2_01_03 + mat[3][3] * det2_01_01;
  672. float det3_301_023 = mat[3][0] * det2_01_23 - mat[3][2] * det2_01_03 + mat[3][3] * det2_01_02;
  673. float det3_301_123 = mat[3][1] * det2_01_23 - mat[3][2] * det2_01_13 + mat[3][3] * det2_01_12;
  674. mat[0][0] = - det3_213_123 * invDet;
  675. mat[1][0] = + det3_213_023 * invDet;
  676. mat[2][0] = - det3_213_013 * invDet;
  677. mat[3][0] = + det3_213_012 * invDet;
  678. mat[0][1] = + det3_203_123 * invDet;
  679. mat[1][1] = - det3_203_023 * invDet;
  680. mat[2][1] = + det3_203_013 * invDet;
  681. mat[3][1] = - det3_203_012 * invDet;
  682. mat[0][2] = + det3_301_123 * invDet;
  683. mat[1][2] = - det3_301_023 * invDet;
  684. mat[2][2] = + det3_301_013 * invDet;
  685. mat[3][2] = - det3_301_012 * invDet;
  686. mat[0][3] = - det3_201_123 * invDet;
  687. mat[1][3] = + det3_201_023 * invDet;
  688. mat[2][3] = - det3_201_013 * invDet;
  689. mat[3][3] = + det3_201_012 * invDet;
  690. return true;
  691. #elif 0
  692. // 4*18 = 72 multiplications
  693. // 4 divisions
  694. float *mat = reinterpret_cast<float *>(this);
  695. float s;
  696. double d, di;
  697. di = mat[0];
  698. s = di;
  699. mat[0] = d = 1.0f / di;
  700. mat[1] *= d;
  701. mat[2] *= d;
  702. mat[3] *= d;
  703. d = -d;
  704. mat[4] *= d;
  705. mat[8] *= d;
  706. mat[12] *= d;
  707. d = mat[4] * di;
  708. mat[5] += mat[1] * d;
  709. mat[6] += mat[2] * d;
  710. mat[7] += mat[3] * d;
  711. d = mat[8] * di;
  712. mat[9] += mat[1] * d;
  713. mat[10] += mat[2] * d;
  714. mat[11] += mat[3] * d;
  715. d = mat[12] * di;
  716. mat[13] += mat[1] * d;
  717. mat[14] += mat[2] * d;
  718. mat[15] += mat[3] * d;
  719. di = mat[5];
  720. s *= di;
  721. mat[5] = d = 1.0f / di;
  722. mat[4] *= d;
  723. mat[6] *= d;
  724. mat[7] *= d;
  725. d = -d;
  726. mat[1] *= d;
  727. mat[9] *= d;
  728. mat[13] *= d;
  729. d = mat[1] * di;
  730. mat[0] += mat[4] * d;
  731. mat[2] += mat[6] * d;
  732. mat[3] += mat[7] * d;
  733. d = mat[9] * di;
  734. mat[8] += mat[4] * d;
  735. mat[10] += mat[6] * d;
  736. mat[11] += mat[7] * d;
  737. d = mat[13] * di;
  738. mat[12] += mat[4] * d;
  739. mat[14] += mat[6] * d;
  740. mat[15] += mat[7] * d;
  741. di = mat[10];
  742. s *= di;
  743. mat[10] = d = 1.0f / di;
  744. mat[8] *= d;
  745. mat[9] *= d;
  746. mat[11] *= d;
  747. d = -d;
  748. mat[2] *= d;
  749. mat[6] *= d;
  750. mat[14] *= d;
  751. d = mat[2] * di;
  752. mat[0] += mat[8] * d;
  753. mat[1] += mat[9] * d;
  754. mat[3] += mat[11] * d;
  755. d = mat[6] * di;
  756. mat[4] += mat[8] * d;
  757. mat[5] += mat[9] * d;
  758. mat[7] += mat[11] * d;
  759. d = mat[14] * di;
  760. mat[12] += mat[8] * d;
  761. mat[13] += mat[9] * d;
  762. mat[15] += mat[11] * d;
  763. di = mat[15];
  764. s *= di;
  765. mat[15] = d = 1.0f / di;
  766. mat[12] *= d;
  767. mat[13] *= d;
  768. mat[14] *= d;
  769. d = -d;
  770. mat[3] *= d;
  771. mat[7] *= d;
  772. mat[11] *= d;
  773. d = mat[3] * di;
  774. mat[0] += mat[12] * d;
  775. mat[1] += mat[13] * d;
  776. mat[2] += mat[14] * d;
  777. d = mat[7] * di;
  778. mat[4] += mat[12] * d;
  779. mat[5] += mat[13] * d;
  780. mat[6] += mat[14] * d;
  781. d = mat[11] * di;
  782. mat[8] += mat[12] * d;
  783. mat[9] += mat[13] * d;
  784. mat[10] += mat[14] * d;
  785. return ( s != 0.0f && !IEEE_FLT_IS_NAN( s ) );
  786. #else
  787. // 6*8+2*6 = 60 multiplications
  788. // 2*1 = 2 divisions
  789. idMat2 r0, r1, r2, r3;
  790. float a, det, invDet;
  791. float *mat = reinterpret_cast<float *>(this);
  792. // r0 = m0.Inverse();
  793. det = mat[0*4+0] * mat[1*4+1] - mat[0*4+1] * mat[1*4+0];
  794. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  795. return false;
  796. }
  797. invDet = 1.0f / det;
  798. r0[0][0] = mat[1*4+1] * invDet;
  799. r0[0][1] = - mat[0*4+1] * invDet;
  800. r0[1][0] = - mat[1*4+0] * invDet;
  801. r0[1][1] = mat[0*4+0] * invDet;
  802. // r1 = r0 * m1;
  803. r1[0][0] = r0[0][0] * mat[0*4+2] + r0[0][1] * mat[1*4+2];
  804. r1[0][1] = r0[0][0] * mat[0*4+3] + r0[0][1] * mat[1*4+3];
  805. r1[1][0] = r0[1][0] * mat[0*4+2] + r0[1][1] * mat[1*4+2];
  806. r1[1][1] = r0[1][0] * mat[0*4+3] + r0[1][1] * mat[1*4+3];
  807. // r2 = m2 * r1;
  808. r2[0][0] = mat[2*4+0] * r1[0][0] + mat[2*4+1] * r1[1][0];
  809. r2[0][1] = mat[2*4+0] * r1[0][1] + mat[2*4+1] * r1[1][1];
  810. r2[1][0] = mat[3*4+0] * r1[0][0] + mat[3*4+1] * r1[1][0];
  811. r2[1][1] = mat[3*4+0] * r1[0][1] + mat[3*4+1] * r1[1][1];
  812. // r3 = r2 - m3;
  813. r3[0][0] = r2[0][0] - mat[2*4+2];
  814. r3[0][1] = r2[0][1] - mat[2*4+3];
  815. r3[1][0] = r2[1][0] - mat[3*4+2];
  816. r3[1][1] = r2[1][1] - mat[3*4+3];
  817. // r3.InverseSelf();
  818. det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];
  819. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  820. return false;
  821. }
  822. invDet = 1.0f / det;
  823. a = r3[0][0];
  824. r3[0][0] = r3[1][1] * invDet;
  825. r3[0][1] = - r3[0][1] * invDet;
  826. r3[1][0] = - r3[1][0] * invDet;
  827. r3[1][1] = a * invDet;
  828. // r2 = m2 * r0;
  829. r2[0][0] = mat[2*4+0] * r0[0][0] + mat[2*4+1] * r0[1][0];
  830. r2[0][1] = mat[2*4+0] * r0[0][1] + mat[2*4+1] * r0[1][1];
  831. r2[1][0] = mat[3*4+0] * r0[0][0] + mat[3*4+1] * r0[1][0];
  832. r2[1][1] = mat[3*4+0] * r0[0][1] + mat[3*4+1] * r0[1][1];
  833. // m2 = r3 * r2;
  834. mat[2*4+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0];
  835. mat[2*4+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1];
  836. mat[3*4+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0];
  837. mat[3*4+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1];
  838. // m0 = r0 - r1 * m2;
  839. mat[0*4+0] = r0[0][0] - r1[0][0] * mat[2*4+0] - r1[0][1] * mat[3*4+0];
  840. mat[0*4+1] = r0[0][1] - r1[0][0] * mat[2*4+1] - r1[0][1] * mat[3*4+1];
  841. mat[1*4+0] = r0[1][0] - r1[1][0] * mat[2*4+0] - r1[1][1] * mat[3*4+0];
  842. mat[1*4+1] = r0[1][1] - r1[1][0] * mat[2*4+1] - r1[1][1] * mat[3*4+1];
  843. // m1 = r1 * r3;
  844. mat[0*4+2] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0];
  845. mat[0*4+3] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1];
  846. mat[1*4+2] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0];
  847. mat[1*4+3] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1];
  848. // m3 = -r3;
  849. mat[2*4+2] = -r3[0][0];
  850. mat[2*4+3] = -r3[0][1];
  851. mat[3*4+2] = -r3[1][0];
  852. mat[3*4+3] = -r3[1][1];
  853. return true;
  854. #endif
  855. }
  856. /*
  857. =============
  858. idMat4::ToString
  859. =============
  860. */
  861. const char *idMat4::ToString( int precision ) const {
  862. return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
  863. }
  864. //===============================================================
  865. //
  866. // idMat5
  867. //
  868. //===============================================================
  869. idMat5 mat5_zero( idVec5( 0, 0, 0, 0, 0 ), idVec5( 0, 0, 0, 0, 0 ), idVec5( 0, 0, 0, 0, 0 ), idVec5( 0, 0, 0, 0, 0 ), idVec5( 0, 0, 0, 0, 0 ) );
  870. idMat5 mat5_identity( idVec5( 1, 0, 0, 0, 0 ), idVec5( 0, 1, 0, 0, 0 ), idVec5( 0, 0, 1, 0, 0 ), idVec5( 0, 0, 0, 1, 0 ), idVec5( 0, 0, 0, 0, 1 ) );
  871. /*
  872. ============
  873. idMat5::Transpose
  874. ============
  875. */
  876. idMat5 idMat5::Transpose() const {
  877. idMat5 transpose;
  878. int i, j;
  879. for( i = 0; i < 5; i++ ) {
  880. for( j = 0; j < 5; j++ ) {
  881. transpose[ i ][ j ] = mat[ j ][ i ];
  882. }
  883. }
  884. return transpose;
  885. }
  886. /*
  887. ============
  888. idMat5::TransposeSelf
  889. ============
  890. */
  891. idMat5 &idMat5::TransposeSelf() {
  892. float temp;
  893. int i, j;
  894. for( i = 0; i < 5; i++ ) {
  895. for( j = i + 1; j < 5; j++ ) {
  896. temp = mat[ i ][ j ];
  897. mat[ i ][ j ] = mat[ j ][ i ];
  898. mat[ j ][ i ] = temp;
  899. }
  900. }
  901. return *this;
  902. }
  903. /*
  904. ============
  905. idMat5::Determinant
  906. ============
  907. */
  908. float idMat5::Determinant() const {
  909. // 2x2 sub-determinants required to calculate 5x5 determinant
  910. float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
  911. float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
  912. float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
  913. float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
  914. float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
  915. float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
  916. float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
  917. float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
  918. float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
  919. float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
  920. // 3x3 sub-determinants required to calculate 5x5 determinant
  921. float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
  922. float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
  923. float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
  924. float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
  925. float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
  926. float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
  927. float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
  928. float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
  929. float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
  930. float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
  931. // 4x4 sub-determinants required to calculate 5x5 determinant
  932. float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
  933. float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
  934. float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
  935. float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
  936. float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
  937. // determinant of 5x5 matrix
  938. return mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
  939. }
  940. /*
  941. ============
  942. idMat5::InverseSelf
  943. ============
  944. */
  945. bool idMat5::InverseSelf() {
  946. // 280+5+25 = 310 multiplications
  947. // 1 division
  948. double det, invDet;
  949. // 2x2 sub-determinants required to calculate 5x5 determinant
  950. float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
  951. float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
  952. float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
  953. float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
  954. float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
  955. float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
  956. float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
  957. float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
  958. float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
  959. float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
  960. // 3x3 sub-determinants required to calculate 5x5 determinant
  961. float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
  962. float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
  963. float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
  964. float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
  965. float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
  966. float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
  967. float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
  968. float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
  969. float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
  970. float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
  971. // 4x4 sub-determinants required to calculate 5x5 determinant
  972. float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
  973. float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
  974. float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
  975. float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
  976. float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
  977. // determinant of 5x5 matrix
  978. det = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
  979. if( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  980. return false;
  981. }
  982. invDet = 1.0f / det;
  983. // remaining 2x2 sub-determinants
  984. float det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0];
  985. float det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0];
  986. float det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0];
  987. float det2_23_04 = mat[2][0] * mat[3][4] - mat[2][4] * mat[3][0];
  988. float det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1];
  989. float det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];
  990. float det2_23_14 = mat[2][1] * mat[3][4] - mat[2][4] * mat[3][1];
  991. float det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];
  992. float det2_23_24 = mat[2][2] * mat[3][4] - mat[2][4] * mat[3][2];
  993. float det2_23_34 = mat[2][3] * mat[3][4] - mat[2][4] * mat[3][3];
  994. float det2_24_01 = mat[2][0] * mat[4][1] - mat[2][1] * mat[4][0];
  995. float det2_24_02 = mat[2][0] * mat[4][2] - mat[2][2] * mat[4][0];
  996. float det2_24_03 = mat[2][0] * mat[4][3] - mat[2][3] * mat[4][0];
  997. float det2_24_04 = mat[2][0] * mat[4][4] - mat[2][4] * mat[4][0];
  998. float det2_24_12 = mat[2][1] * mat[4][2] - mat[2][2] * mat[4][1];
  999. float det2_24_13 = mat[2][1] * mat[4][3] - mat[2][3] * mat[4][1];
  1000. float det2_24_14 = mat[2][1] * mat[4][4] - mat[2][4] * mat[4][1];
  1001. float det2_24_23 = mat[2][2] * mat[4][3] - mat[2][3] * mat[4][2];
  1002. float det2_24_24 = mat[2][2] * mat[4][4] - mat[2][4] * mat[4][2];
  1003. float det2_24_34 = mat[2][3] * mat[4][4] - mat[2][4] * mat[4][3];
  1004. // remaining 3x3 sub-determinants
  1005. float det3_123_012 = mat[1][0] * det2_23_12 - mat[1][1] * det2_23_02 + mat[1][2] * det2_23_01;
  1006. float det3_123_013 = mat[1][0] * det2_23_13 - mat[1][1] * det2_23_03 + mat[1][3] * det2_23_01;
  1007. float det3_123_014 = mat[1][0] * det2_23_14 - mat[1][1] * det2_23_04 + mat[1][4] * det2_23_01;
  1008. float det3_123_023 = mat[1][0] * det2_23_23 - mat[1][2] * det2_23_03 + mat[1][3] * det2_23_02;
  1009. float det3_123_024 = mat[1][0] * det2_23_24 - mat[1][2] * det2_23_04 + mat[1][4] * det2_23_02;
  1010. float det3_123_034 = mat[1][0] * det2_23_34 - mat[1][3] * det2_23_04 + mat[1][4] * det2_23_03;
  1011. float det3_123_123 = mat[1][1] * det2_23_23 - mat[1][2] * det2_23_13 + mat[1][3] * det2_23_12;
  1012. float det3_123_124 = mat[1][1] * det2_23_24 - mat[1][2] * det2_23_14 + mat[1][4] * det2_23_12;
  1013. float det3_123_134 = mat[1][1] * det2_23_34 - mat[1][3] * det2_23_14 + mat[1][4] * det2_23_13;
  1014. float det3_123_234 = mat[1][2] * det2_23_34 - mat[1][3] * det2_23_24 + mat[1][4] * det2_23_23;
  1015. float det3_124_012 = mat[1][0] * det2_24_12 - mat[1][1] * det2_24_02 + mat[1][2] * det2_24_01;
  1016. float det3_124_013 = mat[1][0] * det2_24_13 - mat[1][1] * det2_24_03 + mat[1][3] * det2_24_01;
  1017. float det3_124_014 = mat[1][0] * det2_24_14 - mat[1][1] * det2_24_04 + mat[1][4] * det2_24_01;
  1018. float det3_124_023 = mat[1][0] * det2_24_23 - mat[1][2] * det2_24_03 + mat[1][3] * det2_24_02;
  1019. float det3_124_024 = mat[1][0] * det2_24_24 - mat[1][2] * det2_24_04 + mat[1][4] * det2_24_02;
  1020. float det3_124_034 = mat[1][0] * det2_24_34 - mat[1][3] * det2_24_04 + mat[1][4] * det2_24_03;
  1021. float det3_124_123 = mat[1][1] * det2_24_23 - mat[1][2] * det2_24_13 + mat[1][3] * det2_24_12;
  1022. float det3_124_124 = mat[1][1] * det2_24_24 - mat[1][2] * det2_24_14 + mat[1][4] * det2_24_12;
  1023. float det3_124_134 = mat[1][1] * det2_24_34 - mat[1][3] * det2_24_14 + mat[1][4] * det2_24_13;
  1024. float det3_124_234 = mat[1][2] * det2_24_34 - mat[1][3] * det2_24_24 + mat[1][4] * det2_24_23;
  1025. float det3_134_012 = mat[1][0] * det2_34_12 - mat[1][1] * det2_34_02 + mat[1][2] * det2_34_01;
  1026. float det3_134_013 = mat[1][0] * det2_34_13 - mat[1][1] * det2_34_03 + mat[1][3] * det2_34_01;
  1027. float det3_134_014 = mat[1][0] * det2_34_14 - mat[1][1] * det2_34_04 + mat[1][4] * det2_34_01;
  1028. float det3_134_023 = mat[1][0] * det2_34_23 - mat[1][2] * det2_34_03 + mat[1][3] * det2_34_02;
  1029. float det3_134_024 = mat[1][0] * det2_34_24 - mat[1][2] * det2_34_04 + mat[1][4] * det2_34_02;
  1030. float det3_134_034 = mat[1][0] * det2_34_34 - mat[1][3] * det2_34_04 + mat[1][4] * det2_34_03;
  1031. float det3_134_123 = mat[1][1] * det2_34_23 - mat[1][2] * det2_34_13 + mat[1][3] * det2_34_12;
  1032. float det3_134_124 = mat[1][1] * det2_34_24 - mat[1][2] * det2_34_14 + mat[1][4] * det2_34_12;
  1033. float det3_134_134 = mat[1][1] * det2_34_34 - mat[1][3] * det2_34_14 + mat[1][4] * det2_34_13;
  1034. float det3_134_234 = mat[1][2] * det2_34_34 - mat[1][3] * det2_34_24 + mat[1][4] * det2_34_23;
  1035. // remaining 4x4 sub-determinants
  1036. float det4_0123_0123 = mat[0][0] * det3_123_123 - mat[0][1] * det3_123_023 + mat[0][2] * det3_123_013 - mat[0][3] * det3_123_012;
  1037. float det4_0123_0124 = mat[0][0] * det3_123_124 - mat[0][1] * det3_123_024 + mat[0][2] * det3_123_014 - mat[0][4] * det3_123_012;
  1038. float det4_0123_0134 = mat[0][0] * det3_123_134 - mat[0][1] * det3_123_034 + mat[0][3] * det3_123_014 - mat[0][4] * det3_123_013;
  1039. float det4_0123_0234 = mat[0][0] * det3_123_234 - mat[0][2] * det3_123_034 + mat[0][3] * det3_123_024 - mat[0][4] * det3_123_023;
  1040. float det4_0123_1234 = mat[0][1] * det3_123_234 - mat[0][2] * det3_123_134 + mat[0][3] * det3_123_124 - mat[0][4] * det3_123_123;
  1041. float det4_0124_0123 = mat[0][0] * det3_124_123 - mat[0][1] * det3_124_023 + mat[0][2] * det3_124_013 - mat[0][3] * det3_124_012;
  1042. float det4_0124_0124 = mat[0][0] * det3_124_124 - mat[0][1] * det3_124_024 + mat[0][2] * det3_124_014 - mat[0][4] * det3_124_012;
  1043. float det4_0124_0134 = mat[0][0] * det3_124_134 - mat[0][1] * det3_124_034 + mat[0][3] * det3_124_014 - mat[0][4] * det3_124_013;
  1044. float det4_0124_0234 = mat[0][0] * det3_124_234 - mat[0][2] * det3_124_034 + mat[0][3] * det3_124_024 - mat[0][4] * det3_124_023;
  1045. float det4_0124_1234 = mat[0][1] * det3_124_234 - mat[0][2] * det3_124_134 + mat[0][3] * det3_124_124 - mat[0][4] * det3_124_123;
  1046. float det4_0134_0123 = mat[0][0] * det3_134_123 - mat[0][1] * det3_134_023 + mat[0][2] * det3_134_013 - mat[0][3] * det3_134_012;
  1047. float det4_0134_0124 = mat[0][0] * det3_134_124 - mat[0][1] * det3_134_024 + mat[0][2] * det3_134_014 - mat[0][4] * det3_134_012;
  1048. float det4_0134_0134 = mat[0][0] * det3_134_134 - mat[0][1] * det3_134_034 + mat[0][3] * det3_134_014 - mat[0][4] * det3_134_013;
  1049. float det4_0134_0234 = mat[0][0] * det3_134_234 - mat[0][2] * det3_134_034 + mat[0][3] * det3_134_024 - mat[0][4] * det3_134_023;
  1050. float det4_0134_1234 = mat[0][1] * det3_134_234 - mat[0][2] * det3_134_134 + mat[0][3] * det3_134_124 - mat[0][4] * det3_134_123;
  1051. float det4_0234_0123 = mat[0][0] * det3_234_123 - mat[0][1] * det3_234_023 + mat[0][2] * det3_234_013 - mat[0][3] * det3_234_012;
  1052. float det4_0234_0124 = mat[0][0] * det3_234_124 - mat[0][1] * det3_234_024 + mat[0][2] * det3_234_014 - mat[0][4] * det3_234_012;
  1053. float det4_0234_0134 = mat[0][0] * det3_234_134 - mat[0][1] * det3_234_034 + mat[0][3] * det3_234_014 - mat[0][4] * det3_234_013;
  1054. float det4_0234_0234 = mat[0][0] * det3_234_234 - mat[0][2] * det3_234_034 + mat[0][3] * det3_234_024 - mat[0][4] * det3_234_023;
  1055. float det4_0234_1234 = mat[0][1] * det3_234_234 - mat[0][2] * det3_234_134 + mat[0][3] * det3_234_124 - mat[0][4] * det3_234_123;
  1056. mat[0][0] = det4_1234_1234 * invDet;
  1057. mat[0][1] = -det4_0234_1234 * invDet;
  1058. mat[0][2] = det4_0134_1234 * invDet;
  1059. mat[0][3] = -det4_0124_1234 * invDet;
  1060. mat[0][4] = det4_0123_1234 * invDet;
  1061. mat[1][0] = -det4_1234_0234 * invDet;
  1062. mat[1][1] = det4_0234_0234 * invDet;
  1063. mat[1][2] = -det4_0134_0234 * invDet;
  1064. mat[1][3] = det4_0124_0234 * invDet;
  1065. mat[1][4] = -det4_0123_0234 * invDet;
  1066. mat[2][0] = det4_1234_0134 * invDet;
  1067. mat[2][1] = -det4_0234_0134 * invDet;
  1068. mat[2][2] = det4_0134_0134 * invDet;
  1069. mat[2][3] = -det4_0124_0134 * invDet;
  1070. mat[2][4] = det4_0123_0134 * invDet;
  1071. mat[3][0] = -det4_1234_0124 * invDet;
  1072. mat[3][1] = det4_0234_0124 * invDet;
  1073. mat[3][2] = -det4_0134_0124 * invDet;
  1074. mat[3][3] = det4_0124_0124 * invDet;
  1075. mat[3][4] = -det4_0123_0124 * invDet;
  1076. mat[4][0] = det4_1234_0123 * invDet;
  1077. mat[4][1] = -det4_0234_0123 * invDet;
  1078. mat[4][2] = det4_0134_0123 * invDet;
  1079. mat[4][3] = -det4_0124_0123 * invDet;
  1080. mat[4][4] = det4_0123_0123 * invDet;
  1081. return true;
  1082. }
  1083. /*
  1084. ============
  1085. idMat5::InverseFastSelf
  1086. ============
  1087. */
  1088. bool idMat5::InverseFastSelf() {
  1089. #if 0
  1090. // 280+5+25 = 310 multiplications
  1091. // 1 division
  1092. double det, invDet;
  1093. // 2x2 sub-determinants required to calculate 5x5 determinant
  1094. float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
  1095. float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
  1096. float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
  1097. float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
  1098. float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
  1099. float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
  1100. float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
  1101. float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
  1102. float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
  1103. float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
  1104. // 3x3 sub-determinants required to calculate 5x5 determinant
  1105. float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
  1106. float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
  1107. float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
  1108. float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
  1109. float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
  1110. float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
  1111. float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
  1112. float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
  1113. float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
  1114. float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
  1115. // 4x4 sub-determinants required to calculate 5x5 determinant
  1116. float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
  1117. float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
  1118. float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
  1119. float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
  1120. float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
  1121. // determinant of 5x5 matrix
  1122. det = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
  1123. if( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  1124. return false;
  1125. }
  1126. invDet = 1.0f / det;
  1127. // remaining 2x2 sub-determinants
  1128. float det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0];
  1129. float det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0];
  1130. float det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0];
  1131. float det2_23_04 = mat[2][0] * mat[3][4] - mat[2][4] * mat[3][0];
  1132. float det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1];
  1133. float det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];
  1134. float det2_23_14 = mat[2][1] * mat[3][4] - mat[2][4] * mat[3][1];
  1135. float det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];
  1136. float det2_23_24 = mat[2][2] * mat[3][4] - mat[2][4] * mat[3][2];
  1137. float det2_23_34 = mat[2][3] * mat[3][4] - mat[2][4] * mat[3][3];
  1138. float det2_24_01 = mat[2][0] * mat[4][1] - mat[2][1] * mat[4][0];
  1139. float det2_24_02 = mat[2][0] * mat[4][2] - mat[2][2] * mat[4][0];
  1140. float det2_24_03 = mat[2][0] * mat[4][3] - mat[2][3] * mat[4][0];
  1141. float det2_24_04 = mat[2][0] * mat[4][4] - mat[2][4] * mat[4][0];
  1142. float det2_24_12 = mat[2][1] * mat[4][2] - mat[2][2] * mat[4][1];
  1143. float det2_24_13 = mat[2][1] * mat[4][3] - mat[2][3] * mat[4][1];
  1144. float det2_24_14 = mat[2][1] * mat[4][4] - mat[2][4] * mat[4][1];
  1145. float det2_24_23 = mat[2][2] * mat[4][3] - mat[2][3] * mat[4][2];
  1146. float det2_24_24 = mat[2][2] * mat[4][4] - mat[2][4] * mat[4][2];
  1147. float det2_24_34 = mat[2][3] * mat[4][4] - mat[2][4] * mat[4][3];
  1148. // remaining 3x3 sub-determinants
  1149. float det3_123_012 = mat[1][0] * det2_23_12 - mat[1][1] * det2_23_02 + mat[1][2] * det2_23_01;
  1150. float det3_123_013 = mat[1][0] * det2_23_13 - mat[1][1] * det2_23_03 + mat[1][3] * det2_23_01;
  1151. float det3_123_014 = mat[1][0] * det2_23_14 - mat[1][1] * det2_23_04 + mat[1][4] * det2_23_01;
  1152. float det3_123_023 = mat[1][0] * det2_23_23 - mat[1][2] * det2_23_03 + mat[1][3] * det2_23_02;
  1153. float det3_123_024 = mat[1][0] * det2_23_24 - mat[1][2] * det2_23_04 + mat[1][4] * det2_23_02;
  1154. float det3_123_034 = mat[1][0] * det2_23_34 - mat[1][3] * det2_23_04 + mat[1][4] * det2_23_03;
  1155. float det3_123_123 = mat[1][1] * det2_23_23 - mat[1][2] * det2_23_13 + mat[1][3] * det2_23_12;
  1156. float det3_123_124 = mat[1][1] * det2_23_24 - mat[1][2] * det2_23_14 + mat[1][4] * det2_23_12;
  1157. float det3_123_134 = mat[1][1] * det2_23_34 - mat[1][3] * det2_23_14 + mat[1][4] * det2_23_13;
  1158. float det3_123_234 = mat[1][2] * det2_23_34 - mat[1][3] * det2_23_24 + mat[1][4] * det2_23_23;
  1159. float det3_124_012 = mat[1][0] * det2_24_12 - mat[1][1] * det2_24_02 + mat[1][2] * det2_24_01;
  1160. float det3_124_013 = mat[1][0] * det2_24_13 - mat[1][1] * det2_24_03 + mat[1][3] * det2_24_01;
  1161. float det3_124_014 = mat[1][0] * det2_24_14 - mat[1][1] * det2_24_04 + mat[1][4] * det2_24_01;
  1162. float det3_124_023 = mat[1][0] * det2_24_23 - mat[1][2] * det2_24_03 + mat[1][3] * det2_24_02;
  1163. float det3_124_024 = mat[1][0] * det2_24_24 - mat[1][2] * det2_24_04 + mat[1][4] * det2_24_02;
  1164. float det3_124_034 = mat[1][0] * det2_24_34 - mat[1][3] * det2_24_04 + mat[1][4] * det2_24_03;
  1165. float det3_124_123 = mat[1][1] * det2_24_23 - mat[1][2] * det2_24_13 + mat[1][3] * det2_24_12;
  1166. float det3_124_124 = mat[1][1] * det2_24_24 - mat[1][2] * det2_24_14 + mat[1][4] * det2_24_12;
  1167. float det3_124_134 = mat[1][1] * det2_24_34 - mat[1][3] * det2_24_14 + mat[1][4] * det2_24_13;
  1168. float det3_124_234 = mat[1][2] * det2_24_34 - mat[1][3] * det2_24_24 + mat[1][4] * det2_24_23;
  1169. float det3_134_012 = mat[1][0] * det2_34_12 - mat[1][1] * det2_34_02 + mat[1][2] * det2_34_01;
  1170. float det3_134_013 = mat[1][0] * det2_34_13 - mat[1][1] * det2_34_03 + mat[1][3] * det2_34_01;
  1171. float det3_134_014 = mat[1][0] * det2_34_14 - mat[1][1] * det2_34_04 + mat[1][4] * det2_34_01;
  1172. float det3_134_023 = mat[1][0] * det2_34_23 - mat[1][2] * det2_34_03 + mat[1][3] * det2_34_02;
  1173. float det3_134_024 = mat[1][0] * det2_34_24 - mat[1][2] * det2_34_04 + mat[1][4] * det2_34_02;
  1174. float det3_134_034 = mat[1][0] * det2_34_34 - mat[1][3] * det2_34_04 + mat[1][4] * det2_34_03;
  1175. float det3_134_123 = mat[1][1] * det2_34_23 - mat[1][2] * det2_34_13 + mat[1][3] * det2_34_12;
  1176. float det3_134_124 = mat[1][1] * det2_34_24 - mat[1][2] * det2_34_14 + mat[1][4] * det2_34_12;
  1177. float det3_134_134 = mat[1][1] * det2_34_34 - mat[1][3] * det2_34_14 + mat[1][4] * det2_34_13;
  1178. float det3_134_234 = mat[1][2] * det2_34_34 - mat[1][3] * det2_34_24 + mat[1][4] * det2_34_23;
  1179. // remaining 4x4 sub-determinants
  1180. float det4_0123_0123 = mat[0][0] * det3_123_123 - mat[0][1] * det3_123_023 + mat[0][2] * det3_123_013 - mat[0][3] * det3_123_012;
  1181. float det4_0123_0124 = mat[0][0] * det3_123_124 - mat[0][1] * det3_123_024 + mat[0][2] * det3_123_014 - mat[0][4] * det3_123_012;
  1182. float det4_0123_0134 = mat[0][0] * det3_123_134 - mat[0][1] * det3_123_034 + mat[0][3] * det3_123_014 - mat[0][4] * det3_123_013;
  1183. float det4_0123_0234 = mat[0][0] * det3_123_234 - mat[0][2] * det3_123_034 + mat[0][3] * det3_123_024 - mat[0][4] * det3_123_023;
  1184. float det4_0123_1234 = mat[0][1] * det3_123_234 - mat[0][2] * det3_123_134 + mat[0][3] * det3_123_124 - mat[0][4] * det3_123_123;
  1185. float det4_0124_0123 = mat[0][0] * det3_124_123 - mat[0][1] * det3_124_023 + mat[0][2] * det3_124_013 - mat[0][3] * det3_124_012;
  1186. float det4_0124_0124 = mat[0][0] * det3_124_124 - mat[0][1] * det3_124_024 + mat[0][2] * det3_124_014 - mat[0][4] * det3_124_012;
  1187. float det4_0124_0134 = mat[0][0] * det3_124_134 - mat[0][1] * det3_124_034 + mat[0][3] * det3_124_014 - mat[0][4] * det3_124_013;
  1188. float det4_0124_0234 = mat[0][0] * det3_124_234 - mat[0][2] * det3_124_034 + mat[0][3] * det3_124_024 - mat[0][4] * det3_124_023;
  1189. float det4_0124_1234 = mat[0][1] * det3_124_234 - mat[0][2] * det3_124_134 + mat[0][3] * det3_124_124 - mat[0][4] * det3_124_123;
  1190. float det4_0134_0123 = mat[0][0] * det3_134_123 - mat[0][1] * det3_134_023 + mat[0][2] * det3_134_013 - mat[0][3] * det3_134_012;
  1191. float det4_0134_0124 = mat[0][0] * det3_134_124 - mat[0][1] * det3_134_024 + mat[0][2] * det3_134_014 - mat[0][4] * det3_134_012;
  1192. float det4_0134_0134 = mat[0][0] * det3_134_134 - mat[0][1] * det3_134_034 + mat[0][3] * det3_134_014 - mat[0][4] * det3_134_013;
  1193. float det4_0134_0234 = mat[0][0] * det3_134_234 - mat[0][2] * det3_134_034 + mat[0][3] * det3_134_024 - mat[0][4] * det3_134_023;
  1194. float det4_0134_1234 = mat[0][1] * det3_134_234 - mat[0][2] * det3_134_134 + mat[0][3] * det3_134_124 - mat[0][4] * det3_134_123;
  1195. float det4_0234_0123 = mat[0][0] * det3_234_123 - mat[0][1] * det3_234_023 + mat[0][2] * det3_234_013 - mat[0][3] * det3_234_012;
  1196. float det4_0234_0124 = mat[0][0] * det3_234_124 - mat[0][1] * det3_234_024 + mat[0][2] * det3_234_014 - mat[0][4] * det3_234_012;
  1197. float det4_0234_0134 = mat[0][0] * det3_234_134 - mat[0][1] * det3_234_034 + mat[0][3] * det3_234_014 - mat[0][4] * det3_234_013;
  1198. float det4_0234_0234 = mat[0][0] * det3_234_234 - mat[0][2] * det3_234_034 + mat[0][3] * det3_234_024 - mat[0][4] * det3_234_023;
  1199. float det4_0234_1234 = mat[0][1] * det3_234_234 - mat[0][2] * det3_234_134 + mat[0][3] * det3_234_124 - mat[0][4] * det3_234_123;
  1200. mat[0][0] = det4_1234_1234 * invDet;
  1201. mat[0][1] = -det4_0234_1234 * invDet;
  1202. mat[0][2] = det4_0134_1234 * invDet;
  1203. mat[0][3] = -det4_0124_1234 * invDet;
  1204. mat[0][4] = det4_0123_1234 * invDet;
  1205. mat[1][0] = -det4_1234_0234 * invDet;
  1206. mat[1][1] = det4_0234_0234 * invDet;
  1207. mat[1][2] = -det4_0134_0234 * invDet;
  1208. mat[1][3] = det4_0124_0234 * invDet;
  1209. mat[1][4] = -det4_0123_0234 * invDet;
  1210. mat[2][0] = det4_1234_0134 * invDet;
  1211. mat[2][1] = -det4_0234_0134 * invDet;
  1212. mat[2][2] = det4_0134_0134 * invDet;
  1213. mat[2][3] = -det4_0124_0134 * invDet;
  1214. mat[2][4] = det4_0123_0134 * invDet;
  1215. mat[3][0] = -det4_1234_0124 * invDet;
  1216. mat[3][1] = det4_0234_0124 * invDet;
  1217. mat[3][2] = -det4_0134_0124 * invDet;
  1218. mat[3][3] = det4_0124_0124 * invDet;
  1219. mat[3][4] = -det4_0123_0124 * invDet;
  1220. mat[4][0] = det4_1234_0123 * invDet;
  1221. mat[4][1] = -det4_0234_0123 * invDet;
  1222. mat[4][2] = det4_0134_0123 * invDet;
  1223. mat[4][3] = -det4_0124_0123 * invDet;
  1224. mat[4][4] = det4_0123_0123 * invDet;
  1225. return true;
  1226. #elif 0
  1227. // 5*28 = 140 multiplications
  1228. // 5 divisions
  1229. float *mat = reinterpret_cast<float *>(this);
  1230. float s;
  1231. double d, di;
  1232. di = mat[0];
  1233. s = di;
  1234. mat[0] = d = 1.0f / di;
  1235. mat[1] *= d;
  1236. mat[2] *= d;
  1237. mat[3] *= d;
  1238. mat[4] *= d;
  1239. d = -d;
  1240. mat[5] *= d;
  1241. mat[10] *= d;
  1242. mat[15] *= d;
  1243. mat[20] *= d;
  1244. d = mat[5] * di;
  1245. mat[6] += mat[1] * d;
  1246. mat[7] += mat[2] * d;
  1247. mat[8] += mat[3] * d;
  1248. mat[9] += mat[4] * d;
  1249. d = mat[10] * di;
  1250. mat[11] += mat[1] * d;
  1251. mat[12] += mat[2] * d;
  1252. mat[13] += mat[3] * d;
  1253. mat[14] += mat[4] * d;
  1254. d = mat[15] * di;
  1255. mat[16] += mat[1] * d;
  1256. mat[17] += mat[2] * d;
  1257. mat[18] += mat[3] * d;
  1258. mat[19] += mat[4] * d;
  1259. d = mat[20] * di;
  1260. mat[21] += mat[1] * d;
  1261. mat[22] += mat[2] * d;
  1262. mat[23] += mat[3] * d;
  1263. mat[24] += mat[4] * d;
  1264. di = mat[6];
  1265. s *= di;
  1266. mat[6] = d = 1.0f / di;
  1267. mat[5] *= d;
  1268. mat[7] *= d;
  1269. mat[8] *= d;
  1270. mat[9] *= d;
  1271. d = -d;
  1272. mat[1] *= d;
  1273. mat[11] *= d;
  1274. mat[16] *= d;
  1275. mat[21] *= d;
  1276. d = mat[1] * di;
  1277. mat[0] += mat[5] * d;
  1278. mat[2] += mat[7] * d;
  1279. mat[3] += mat[8] * d;
  1280. mat[4] += mat[9] * d;
  1281. d = mat[11] * di;
  1282. mat[10] += mat[5] * d;
  1283. mat[12] += mat[7] * d;
  1284. mat[13] += mat[8] * d;
  1285. mat[14] += mat[9] * d;
  1286. d = mat[16] * di;
  1287. mat[15] += mat[5] * d;
  1288. mat[17] += mat[7] * d;
  1289. mat[18] += mat[8] * d;
  1290. mat[19] += mat[9] * d;
  1291. d = mat[21] * di;
  1292. mat[20] += mat[5] * d;
  1293. mat[22] += mat[7] * d;
  1294. mat[23] += mat[8] * d;
  1295. mat[24] += mat[9] * d;
  1296. di = mat[12];
  1297. s *= di;
  1298. mat[12] = d = 1.0f / di;
  1299. mat[10] *= d;
  1300. mat[11] *= d;
  1301. mat[13] *= d;
  1302. mat[14] *= d;
  1303. d = -d;
  1304. mat[2] *= d;
  1305. mat[7] *= d;
  1306. mat[17] *= d;
  1307. mat[22] *= d;
  1308. d = mat[2] * di;
  1309. mat[0] += mat[10] * d;
  1310. mat[1] += mat[11] * d;
  1311. mat[3] += mat[13] * d;
  1312. mat[4] += mat[14] * d;
  1313. d = mat[7] * di;
  1314. mat[5] += mat[10] * d;
  1315. mat[6] += mat[11] * d;
  1316. mat[8] += mat[13] * d;
  1317. mat[9] += mat[14] * d;
  1318. d = mat[17] * di;
  1319. mat[15] += mat[10] * d;
  1320. mat[16] += mat[11] * d;
  1321. mat[18] += mat[13] * d;
  1322. mat[19] += mat[14] * d;
  1323. d = mat[22] * di;
  1324. mat[20] += mat[10] * d;
  1325. mat[21] += mat[11] * d;
  1326. mat[23] += mat[13] * d;
  1327. mat[24] += mat[14] * d;
  1328. di = mat[18];
  1329. s *= di;
  1330. mat[18] = d = 1.0f / di;
  1331. mat[15] *= d;
  1332. mat[16] *= d;
  1333. mat[17] *= d;
  1334. mat[19] *= d;
  1335. d = -d;
  1336. mat[3] *= d;
  1337. mat[8] *= d;
  1338. mat[13] *= d;
  1339. mat[23] *= d;
  1340. d = mat[3] * di;
  1341. mat[0] += mat[15] * d;
  1342. mat[1] += mat[16] * d;
  1343. mat[2] += mat[17] * d;
  1344. mat[4] += mat[19] * d;
  1345. d = mat[8] * di;
  1346. mat[5] += mat[15] * d;
  1347. mat[6] += mat[16] * d;
  1348. mat[7] += mat[17] * d;
  1349. mat[9] += mat[19] * d;
  1350. d = mat[13] * di;
  1351. mat[10] += mat[15] * d;
  1352. mat[11] += mat[16] * d;
  1353. mat[12] += mat[17] * d;
  1354. mat[14] += mat[19] * d;
  1355. d = mat[23] * di;
  1356. mat[20] += mat[15] * d;
  1357. mat[21] += mat[16] * d;
  1358. mat[22] += mat[17] * d;
  1359. mat[24] += mat[19] * d;
  1360. di = mat[24];
  1361. s *= di;
  1362. mat[24] = d = 1.0f / di;
  1363. mat[20] *= d;
  1364. mat[21] *= d;
  1365. mat[22] *= d;
  1366. mat[23] *= d;
  1367. d = -d;
  1368. mat[4] *= d;
  1369. mat[9] *= d;
  1370. mat[14] *= d;
  1371. mat[19] *= d;
  1372. d = mat[4] * di;
  1373. mat[0] += mat[20] * d;
  1374. mat[1] += mat[21] * d;
  1375. mat[2] += mat[22] * d;
  1376. mat[3] += mat[23] * d;
  1377. d = mat[9] * di;
  1378. mat[5] += mat[20] * d;
  1379. mat[6] += mat[21] * d;
  1380. mat[7] += mat[22] * d;
  1381. mat[8] += mat[23] * d;
  1382. d = mat[14] * di;
  1383. mat[10] += mat[20] * d;
  1384. mat[11] += mat[21] * d;
  1385. mat[12] += mat[22] * d;
  1386. mat[13] += mat[23] * d;
  1387. d = mat[19] * di;
  1388. mat[15] += mat[20] * d;
  1389. mat[16] += mat[21] * d;
  1390. mat[17] += mat[22] * d;
  1391. mat[18] += mat[23] * d;
  1392. return ( s != 0.0f && !IEEE_FLT_IS_NAN( s ) );
  1393. #else
  1394. // 86+30+6 = 122 multiplications
  1395. // 2*1 = 2 divisions
  1396. idMat3 r0, r1, r2, r3;
  1397. float c0, c1, c2, det, invDet;
  1398. float *mat = reinterpret_cast<float *>(this);
  1399. // r0 = m0.Inverse(); // 3x3
  1400. c0 = mat[1*5+1] * mat[2*5+2] - mat[1*5+2] * mat[2*5+1];
  1401. c1 = mat[1*5+2] * mat[2*5+0] - mat[1*5+0] * mat[2*5+2];
  1402. c2 = mat[1*5+0] * mat[2*5+1] - mat[1*5+1] * mat[2*5+0];
  1403. det = mat[0*5+0] * c0 + mat[0*5+1] * c1 + mat[0*5+2] * c2;
  1404. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  1405. return false;
  1406. }
  1407. invDet = 1.0f / det;
  1408. r0[0][0] = c0 * invDet;
  1409. r0[0][1] = ( mat[0*5+2] * mat[2*5+1] - mat[0*5+1] * mat[2*5+2] ) * invDet;
  1410. r0[0][2] = ( mat[0*5+1] * mat[1*5+2] - mat[0*5+2] * mat[1*5+1] ) * invDet;
  1411. r0[1][0] = c1 * invDet;
  1412. r0[1][1] = ( mat[0*5+0] * mat[2*5+2] - mat[0*5+2] * mat[2*5+0] ) * invDet;
  1413. r0[1][2] = ( mat[0*5+2] * mat[1*5+0] - mat[0*5+0] * mat[1*5+2] ) * invDet;
  1414. r0[2][0] = c2 * invDet;
  1415. r0[2][1] = ( mat[0*5+1] * mat[2*5+0] - mat[0*5+0] * mat[2*5+1] ) * invDet;
  1416. r0[2][2] = ( mat[0*5+0] * mat[1*5+1] - mat[0*5+1] * mat[1*5+0] ) * invDet;
  1417. // r1 = r0 * m1; // 3x2 = 3x3 * 3x2
  1418. r1[0][0] = r0[0][0] * mat[0*5+3] + r0[0][1] * mat[1*5+3] + r0[0][2] * mat[2*5+3];
  1419. r1[0][1] = r0[0][0] * mat[0*5+4] + r0[0][1] * mat[1*5+4] + r0[0][2] * mat[2*5+4];
  1420. r1[1][0] = r0[1][0] * mat[0*5+3] + r0[1][1] * mat[1*5+3] + r0[1][2] * mat[2*5+3];
  1421. r1[1][1] = r0[1][0] * mat[0*5+4] + r0[1][1] * mat[1*5+4] + r0[1][2] * mat[2*5+4];
  1422. r1[2][0] = r0[2][0] * mat[0*5+3] + r0[2][1] * mat[1*5+3] + r0[2][2] * mat[2*5+3];
  1423. r1[2][1] = r0[2][0] * mat[0*5+4] + r0[2][1] * mat[1*5+4] + r0[2][2] * mat[2*5+4];
  1424. // r2 = m2 * r1; // 2x2 = 2x3 * 3x2
  1425. r2[0][0] = mat[3*5+0] * r1[0][0] + mat[3*5+1] * r1[1][0] + mat[3*5+2] * r1[2][0];
  1426. r2[0][1] = mat[3*5+0] * r1[0][1] + mat[3*5+1] * r1[1][1] + mat[3*5+2] * r1[2][1];
  1427. r2[1][0] = mat[4*5+0] * r1[0][0] + mat[4*5+1] * r1[1][0] + mat[4*5+2] * r1[2][0];
  1428. r2[1][1] = mat[4*5+0] * r1[0][1] + mat[4*5+1] * r1[1][1] + mat[4*5+2] * r1[2][1];
  1429. // r3 = r2 - m3; // 2x2 = 2x2 - 2x2
  1430. r3[0][0] = r2[0][0] - mat[3*5+3];
  1431. r3[0][1] = r2[0][1] - mat[3*5+4];
  1432. r3[1][0] = r2[1][0] - mat[4*5+3];
  1433. r3[1][1] = r2[1][1] - mat[4*5+4];
  1434. // r3.InverseSelf(); // 2x2
  1435. det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];
  1436. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  1437. return false;
  1438. }
  1439. invDet = 1.0f / det;
  1440. c0 = r3[0][0];
  1441. r3[0][0] = r3[1][1] * invDet;
  1442. r3[0][1] = - r3[0][1] * invDet;
  1443. r3[1][0] = - r3[1][0] * invDet;
  1444. r3[1][1] = c0 * invDet;
  1445. // r2 = m2 * r0; // 2x3 = 2x3 * 3x3
  1446. r2[0][0] = mat[3*5+0] * r0[0][0] + mat[3*5+1] * r0[1][0] + mat[3*5+2] * r0[2][0];
  1447. r2[0][1] = mat[3*5+0] * r0[0][1] + mat[3*5+1] * r0[1][1] + mat[3*5+2] * r0[2][1];
  1448. r2[0][2] = mat[3*5+0] * r0[0][2] + mat[3*5+1] * r0[1][2] + mat[3*5+2] * r0[2][2];
  1449. r2[1][0] = mat[4*5+0] * r0[0][0] + mat[4*5+1] * r0[1][0] + mat[4*5+2] * r0[2][0];
  1450. r2[1][1] = mat[4*5+0] * r0[0][1] + mat[4*5+1] * r0[1][1] + mat[4*5+2] * r0[2][1];
  1451. r2[1][2] = mat[4*5+0] * r0[0][2] + mat[4*5+1] * r0[1][2] + mat[4*5+2] * r0[2][2];
  1452. // m2 = r3 * r2; // 2x3 = 2x2 * 2x3
  1453. mat[3*5+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0];
  1454. mat[3*5+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1];
  1455. mat[3*5+2] = r3[0][0] * r2[0][2] + r3[0][1] * r2[1][2];
  1456. mat[4*5+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0];
  1457. mat[4*5+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1];
  1458. mat[4*5+2] = r3[1][0] * r2[0][2] + r3[1][1] * r2[1][2];
  1459. // m0 = r0 - r1 * m2; // 3x3 = 3x3 - 3x2 * 2x3
  1460. mat[0*5+0] = r0[0][0] - r1[0][0] * mat[3*5+0] - r1[0][1] * mat[4*5+0];
  1461. mat[0*5+1] = r0[0][1] - r1[0][0] * mat[3*5+1] - r1[0][1] * mat[4*5+1];
  1462. mat[0*5+2] = r0[0][2] - r1[0][0] * mat[3*5+2] - r1[0][1] * mat[4*5+2];
  1463. mat[1*5+0] = r0[1][0] - r1[1][0] * mat[3*5+0] - r1[1][1] * mat[4*5+0];
  1464. mat[1*5+1] = r0[1][1] - r1[1][0] * mat[3*5+1] - r1[1][1] * mat[4*5+1];
  1465. mat[1*5+2] = r0[1][2] - r1[1][0] * mat[3*5+2] - r1[1][1] * mat[4*5+2];
  1466. mat[2*5+0] = r0[2][0] - r1[2][0] * mat[3*5+0] - r1[2][1] * mat[4*5+0];
  1467. mat[2*5+1] = r0[2][1] - r1[2][0] * mat[3*5+1] - r1[2][1] * mat[4*5+1];
  1468. mat[2*5+2] = r0[2][2] - r1[2][0] * mat[3*5+2] - r1[2][1] * mat[4*5+2];
  1469. // m1 = r1 * r3; // 3x2 = 3x2 * 2x2
  1470. mat[0*5+3] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0];
  1471. mat[0*5+4] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1];
  1472. mat[1*5+3] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0];
  1473. mat[1*5+4] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1];
  1474. mat[2*5+3] = r1[2][0] * r3[0][0] + r1[2][1] * r3[1][0];
  1475. mat[2*5+4] = r1[2][0] * r3[0][1] + r1[2][1] * r3[1][1];
  1476. // m3 = -r3; // 2x2 = - 2x2
  1477. mat[3*5+3] = -r3[0][0];
  1478. mat[3*5+4] = -r3[0][1];
  1479. mat[4*5+3] = -r3[1][0];
  1480. mat[4*5+4] = -r3[1][1];
  1481. return true;
  1482. #endif
  1483. }
  1484. /*
  1485. =============
  1486. idMat5::ToString
  1487. =============
  1488. */
  1489. const char *idMat5::ToString( int precision ) const {
  1490. return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
  1491. }
  1492. //===============================================================
  1493. //
  1494. // idMat6
  1495. //
  1496. //===============================================================
  1497. idMat6 mat6_zero( idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ), idVec6( 0, 0, 0, 0, 0, 0 ) );
  1498. idMat6 mat6_identity( idVec6( 1, 0, 0, 0, 0, 0 ), idVec6( 0, 1, 0, 0, 0, 0 ), idVec6( 0, 0, 1, 0, 0, 0 ), idVec6( 0, 0, 0, 1, 0, 0 ), idVec6( 0, 0, 0, 0, 1, 0 ), idVec6( 0, 0, 0, 0, 0, 1 ) );
  1499. /*
  1500. ============
  1501. idMat6::Transpose
  1502. ============
  1503. */
  1504. idMat6 idMat6::Transpose() const {
  1505. idMat6 transpose;
  1506. int i, j;
  1507. for( i = 0; i < 6; i++ ) {
  1508. for( j = 0; j < 6; j++ ) {
  1509. transpose[ i ][ j ] = mat[ j ][ i ];
  1510. }
  1511. }
  1512. return transpose;
  1513. }
  1514. /*
  1515. ============
  1516. idMat6::TransposeSelf
  1517. ============
  1518. */
  1519. idMat6 &idMat6::TransposeSelf() {
  1520. float temp;
  1521. int i, j;
  1522. for( i = 0; i < 6; i++ ) {
  1523. for( j = i + 1; j < 6; j++ ) {
  1524. temp = mat[ i ][ j ];
  1525. mat[ i ][ j ] = mat[ j ][ i ];
  1526. mat[ j ][ i ] = temp;
  1527. }
  1528. }
  1529. return *this;
  1530. }
  1531. /*
  1532. ============
  1533. idMat6::Determinant
  1534. ============
  1535. */
  1536. float idMat6::Determinant() const {
  1537. // 2x2 sub-determinants required to calculate 6x6 determinant
  1538. float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0];
  1539. float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0];
  1540. float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0];
  1541. float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0];
  1542. float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0];
  1543. float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1];
  1544. float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1];
  1545. float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1];
  1546. float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1];
  1547. float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2];
  1548. float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2];
  1549. float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2];
  1550. float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3];
  1551. float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3];
  1552. float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4];
  1553. // 3x3 sub-determinants required to calculate 6x6 determinant
  1554. float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01;
  1555. float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01;
  1556. float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01;
  1557. float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01;
  1558. float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02;
  1559. float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02;
  1560. float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02;
  1561. float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03;
  1562. float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03;
  1563. float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04;
  1564. float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12;
  1565. float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12;
  1566. float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12;
  1567. float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13;
  1568. float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13;
  1569. float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14;
  1570. float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23;
  1571. float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23;
  1572. float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24;
  1573. float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34;
  1574. // 4x4 sub-determinants required to calculate 6x6 determinant
  1575. float det4_2345_0123 = mat[2][0] * det3_345_123 - mat[2][1] * det3_345_023 + mat[2][2] * det3_345_013 - mat[2][3] * det3_345_012;
  1576. float det4_2345_0124 = mat[2][0] * det3_345_124 - mat[2][1] * det3_345_024 + mat[2][2] * det3_345_014 - mat[2][4] * det3_345_012;
  1577. float det4_2345_0125 = mat[2][0] * det3_345_125 - mat[2][1] * det3_345_025 + mat[2][2] * det3_345_015 - mat[2][5] * det3_345_012;
  1578. float det4_2345_0134 = mat[2][0] * det3_345_134 - mat[2][1] * det3_345_034 + mat[2][3] * det3_345_014 - mat[2][4] * det3_345_013;
  1579. float det4_2345_0135 = mat[2][0] * det3_345_135 - mat[2][1] * det3_345_035 + mat[2][3] * det3_345_015 - mat[2][5] * det3_345_013;
  1580. float det4_2345_0145 = mat[2][0] * det3_345_145 - mat[2][1] * det3_345_045 + mat[2][4] * det3_345_015 - mat[2][5] * det3_345_014;
  1581. float det4_2345_0234 = mat[2][0] * det3_345_234 - mat[2][2] * det3_345_034 + mat[2][3] * det3_345_024 - mat[2][4] * det3_345_023;
  1582. float det4_2345_0235 = mat[2][0] * det3_345_235 - mat[2][2] * det3_345_035 + mat[2][3] * det3_345_025 - mat[2][5] * det3_345_023;
  1583. float det4_2345_0245 = mat[2][0] * det3_345_245 - mat[2][2] * det3_345_045 + mat[2][4] * det3_345_025 - mat[2][5] * det3_345_024;
  1584. float det4_2345_0345 = mat[2][0] * det3_345_345 - mat[2][3] * det3_345_045 + mat[2][4] * det3_345_035 - mat[2][5] * det3_345_034;
  1585. float det4_2345_1234 = mat[2][1] * det3_345_234 - mat[2][2] * det3_345_134 + mat[2][3] * det3_345_124 - mat[2][4] * det3_345_123;
  1586. float det4_2345_1235 = mat[2][1] * det3_345_235 - mat[2][2] * det3_345_135 + mat[2][3] * det3_345_125 - mat[2][5] * det3_345_123;
  1587. float det4_2345_1245 = mat[2][1] * det3_345_245 - mat[2][2] * det3_345_145 + mat[2][4] * det3_345_125 - mat[2][5] * det3_345_124;
  1588. float det4_2345_1345 = mat[2][1] * det3_345_345 - mat[2][3] * det3_345_145 + mat[2][4] * det3_345_135 - mat[2][5] * det3_345_134;
  1589. float det4_2345_2345 = mat[2][2] * det3_345_345 - mat[2][3] * det3_345_245 + mat[2][4] * det3_345_235 - mat[2][5] * det3_345_234;
  1590. // 5x5 sub-determinants required to calculate 6x6 determinant
  1591. float det5_12345_01234 = mat[1][0] * det4_2345_1234 - mat[1][1] * det4_2345_0234 + mat[1][2] * det4_2345_0134 - mat[1][3] * det4_2345_0124 + mat[1][4] * det4_2345_0123;
  1592. float det5_12345_01235 = mat[1][0] * det4_2345_1235 - mat[1][1] * det4_2345_0235 + mat[1][2] * det4_2345_0135 - mat[1][3] * det4_2345_0125 + mat[1][5] * det4_2345_0123;
  1593. float det5_12345_01245 = mat[1][0] * det4_2345_1245 - mat[1][1] * det4_2345_0245 + mat[1][2] * det4_2345_0145 - mat[1][4] * det4_2345_0125 + mat[1][5] * det4_2345_0124;
  1594. float det5_12345_01345 = mat[1][0] * det4_2345_1345 - mat[1][1] * det4_2345_0345 + mat[1][3] * det4_2345_0145 - mat[1][4] * det4_2345_0135 + mat[1][5] * det4_2345_0134;
  1595. float det5_12345_02345 = mat[1][0] * det4_2345_2345 - mat[1][2] * det4_2345_0345 + mat[1][3] * det4_2345_0245 - mat[1][4] * det4_2345_0235 + mat[1][5] * det4_2345_0234;
  1596. float det5_12345_12345 = mat[1][1] * det4_2345_2345 - mat[1][2] * det4_2345_1345 + mat[1][3] * det4_2345_1245 - mat[1][4] * det4_2345_1235 + mat[1][5] * det4_2345_1234;
  1597. // determinant of 6x6 matrix
  1598. return mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 -
  1599. mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234;
  1600. }
  1601. /*
  1602. ============
  1603. idMat6::InverseSelf
  1604. ============
  1605. */
  1606. bool idMat6::InverseSelf() {
  1607. // 810+6+36 = 852 multiplications
  1608. // 1 division
  1609. double det, invDet;
  1610. // 2x2 sub-determinants required to calculate 6x6 determinant
  1611. float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0];
  1612. float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0];
  1613. float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0];
  1614. float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0];
  1615. float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0];
  1616. float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1];
  1617. float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1];
  1618. float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1];
  1619. float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1];
  1620. float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2];
  1621. float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2];
  1622. float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2];
  1623. float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3];
  1624. float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3];
  1625. float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4];
  1626. // 3x3 sub-determinants required to calculate 6x6 determinant
  1627. float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01;
  1628. float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01;
  1629. float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01;
  1630. float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01;
  1631. float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02;
  1632. float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02;
  1633. float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02;
  1634. float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03;
  1635. float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03;
  1636. float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04;
  1637. float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12;
  1638. float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12;
  1639. float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12;
  1640. float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13;
  1641. float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13;
  1642. float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14;
  1643. float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23;
  1644. float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23;
  1645. float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24;
  1646. float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34;
  1647. // 4x4 sub-determinants required to calculate 6x6 determinant
  1648. float det4_2345_0123 = mat[2][0] * det3_345_123 - mat[2][1] * det3_345_023 + mat[2][2] * det3_345_013 - mat[2][3] * det3_345_012;
  1649. float det4_2345_0124 = mat[2][0] * det3_345_124 - mat[2][1] * det3_345_024 + mat[2][2] * det3_345_014 - mat[2][4] * det3_345_012;
  1650. float det4_2345_0125 = mat[2][0] * det3_345_125 - mat[2][1] * det3_345_025 + mat[2][2] * det3_345_015 - mat[2][5] * det3_345_012;
  1651. float det4_2345_0134 = mat[2][0] * det3_345_134 - mat[2][1] * det3_345_034 + mat[2][3] * det3_345_014 - mat[2][4] * det3_345_013;
  1652. float det4_2345_0135 = mat[2][0] * det3_345_135 - mat[2][1] * det3_345_035 + mat[2][3] * det3_345_015 - mat[2][5] * det3_345_013;
  1653. float det4_2345_0145 = mat[2][0] * det3_345_145 - mat[2][1] * det3_345_045 + mat[2][4] * det3_345_015 - mat[2][5] * det3_345_014;
  1654. float det4_2345_0234 = mat[2][0] * det3_345_234 - mat[2][2] * det3_345_034 + mat[2][3] * det3_345_024 - mat[2][4] * det3_345_023;
  1655. float det4_2345_0235 = mat[2][0] * det3_345_235 - mat[2][2] * det3_345_035 + mat[2][3] * det3_345_025 - mat[2][5] * det3_345_023;
  1656. float det4_2345_0245 = mat[2][0] * det3_345_245 - mat[2][2] * det3_345_045 + mat[2][4] * det3_345_025 - mat[2][5] * det3_345_024;
  1657. float det4_2345_0345 = mat[2][0] * det3_345_345 - mat[2][3] * det3_345_045 + mat[2][4] * det3_345_035 - mat[2][5] * det3_345_034;
  1658. float det4_2345_1234 = mat[2][1] * det3_345_234 - mat[2][2] * det3_345_134 + mat[2][3] * det3_345_124 - mat[2][4] * det3_345_123;
  1659. float det4_2345_1235 = mat[2][1] * det3_345_235 - mat[2][2] * det3_345_135 + mat[2][3] * det3_345_125 - mat[2][5] * det3_345_123;
  1660. float det4_2345_1245 = mat[2][1] * det3_345_245 - mat[2][2] * det3_345_145 + mat[2][4] * det3_345_125 - mat[2][5] * det3_345_124;
  1661. float det4_2345_1345 = mat[2][1] * det3_345_345 - mat[2][3] * det3_345_145 + mat[2][4] * det3_345_135 - mat[2][5] * det3_345_134;
  1662. float det4_2345_2345 = mat[2][2] * det3_345_345 - mat[2][3] * det3_345_245 + mat[2][4] * det3_345_235 - mat[2][5] * det3_345_234;
  1663. // 5x5 sub-determinants required to calculate 6x6 determinant
  1664. float det5_12345_01234 = mat[1][0] * det4_2345_1234 - mat[1][1] * det4_2345_0234 + mat[1][2] * det4_2345_0134 - mat[1][3] * det4_2345_0124 + mat[1][4] * det4_2345_0123;
  1665. float det5_12345_01235 = mat[1][0] * det4_2345_1235 - mat[1][1] * det4_2345_0235 + mat[1][2] * det4_2345_0135 - mat[1][3] * det4_2345_0125 + mat[1][5] * det4_2345_0123;
  1666. float det5_12345_01245 = mat[1][0] * det4_2345_1245 - mat[1][1] * det4_2345_0245 + mat[1][2] * det4_2345_0145 - mat[1][4] * det4_2345_0125 + mat[1][5] * det4_2345_0124;
  1667. float det5_12345_01345 = mat[1][0] * det4_2345_1345 - mat[1][1] * det4_2345_0345 + mat[1][3] * det4_2345_0145 - mat[1][4] * det4_2345_0135 + mat[1][5] * det4_2345_0134;
  1668. float det5_12345_02345 = mat[1][0] * det4_2345_2345 - mat[1][2] * det4_2345_0345 + mat[1][3] * det4_2345_0245 - mat[1][4] * det4_2345_0235 + mat[1][5] * det4_2345_0234;
  1669. float det5_12345_12345 = mat[1][1] * det4_2345_2345 - mat[1][2] * det4_2345_1345 + mat[1][3] * det4_2345_1245 - mat[1][4] * det4_2345_1235 + mat[1][5] * det4_2345_1234;
  1670. // determinant of 6x6 matrix
  1671. det = mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 -
  1672. mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234;
  1673. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  1674. return false;
  1675. }
  1676. invDet = 1.0f / det;
  1677. // remaining 2x2 sub-determinants
  1678. float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
  1679. float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
  1680. float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
  1681. float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
  1682. float det2_34_05 = mat[3][0] * mat[4][5] - mat[3][5] * mat[4][0];
  1683. float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
  1684. float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
  1685. float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
  1686. float det2_34_15 = mat[3][1] * mat[4][5] - mat[3][5] * mat[4][1];
  1687. float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
  1688. float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
  1689. float det2_34_25 = mat[3][2] * mat[4][5] - mat[3][5] * mat[4][2];
  1690. float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
  1691. float det2_34_35 = mat[3][3] * mat[4][5] - mat[3][5] * mat[4][3];
  1692. float det2_34_45 = mat[3][4] * mat[4][5] - mat[3][5] * mat[4][4];
  1693. float det2_35_01 = mat[3][0] * mat[5][1] - mat[3][1] * mat[5][0];
  1694. float det2_35_02 = mat[3][0] * mat[5][2] - mat[3][2] * mat[5][0];
  1695. float det2_35_03 = mat[3][0] * mat[5][3] - mat[3][3] * mat[5][0];
  1696. float det2_35_04 = mat[3][0] * mat[5][4] - mat[3][4] * mat[5][0];
  1697. float det2_35_05 = mat[3][0] * mat[5][5] - mat[3][5] * mat[5][0];
  1698. float det2_35_12 = mat[3][1] * mat[5][2] - mat[3][2] * mat[5][1];
  1699. float det2_35_13 = mat[3][1] * mat[5][3] - mat[3][3] * mat[5][1];
  1700. float det2_35_14 = mat[3][1] * mat[5][4] - mat[3][4] * mat[5][1];
  1701. float det2_35_15 = mat[3][1] * mat[5][5] - mat[3][5] * mat[5][1];
  1702. float det2_35_23 = mat[3][2] * mat[5][3] - mat[3][3] * mat[5][2];
  1703. float det2_35_24 = mat[3][2] * mat[5][4] - mat[3][4] * mat[5][2];
  1704. float det2_35_25 = mat[3][2] * mat[5][5] - mat[3][5] * mat[5][2];
  1705. float det2_35_34 = mat[3][3] * mat[5][4] - mat[3][4] * mat[5][3];
  1706. float det2_35_35 = mat[3][3] * mat[5][5] - mat[3][5] * mat[5][3];
  1707. float det2_35_45 = mat[3][4] * mat[5][5] - mat[3][5] * mat[5][4];
  1708. // remaining 3x3 sub-determinants
  1709. float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
  1710. float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
  1711. float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
  1712. float det3_234_015 = mat[2][0] * det2_34_15 - mat[2][1] * det2_34_05 + mat[2][5] * det2_34_01;
  1713. float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
  1714. float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
  1715. float det3_234_025 = mat[2][0] * det2_34_25 - mat[2][2] * det2_34_05 + mat[2][5] * det2_34_02;
  1716. float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
  1717. float det3_234_035 = mat[2][0] * det2_34_35 - mat[2][3] * det2_34_05 + mat[2][5] * det2_34_03;
  1718. float det3_234_045 = mat[2][0] * det2_34_45 - mat[2][4] * det2_34_05 + mat[2][5] * det2_34_04;
  1719. float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
  1720. float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
  1721. float det3_234_125 = mat[2][1] * det2_34_25 - mat[2][2] * det2_34_15 + mat[2][5] * det2_34_12;
  1722. float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
  1723. float det3_234_135 = mat[2][1] * det2_34_35 - mat[2][3] * det2_34_15 + mat[2][5] * det2_34_13;
  1724. float det3_234_145 = mat[2][1] * det2_34_45 - mat[2][4] * det2_34_15 + mat[2][5] * det2_34_14;
  1725. float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
  1726. float det3_234_235 = mat[2][2] * det2_34_35 - mat[2][3] * det2_34_25 + mat[2][5] * det2_34_23;
  1727. float det3_234_245 = mat[2][2] * det2_34_45 - mat[2][4] * det2_34_25 + mat[2][5] * det2_34_24;
  1728. float det3_234_345 = mat[2][3] * det2_34_45 - mat[2][4] * det2_34_35 + mat[2][5] * det2_34_34;
  1729. float det3_235_012 = mat[2][0] * det2_35_12 - mat[2][1] * det2_35_02 + mat[2][2] * det2_35_01;
  1730. float det3_235_013 = mat[2][0] * det2_35_13 - mat[2][1] * det2_35_03 + mat[2][3] * det2_35_01;
  1731. float det3_235_014 = mat[2][0] * det2_35_14 - mat[2][1] * det2_35_04 + mat[2][4] * det2_35_01;
  1732. float det3_235_015 = mat[2][0] * det2_35_15 - mat[2][1] * det2_35_05 + mat[2][5] * det2_35_01;
  1733. float det3_235_023 = mat[2][0] * det2_35_23 - mat[2][2] * det2_35_03 + mat[2][3] * det2_35_02;
  1734. float det3_235_024 = mat[2][0] * det2_35_24 - mat[2][2] * det2_35_04 + mat[2][4] * det2_35_02;
  1735. float det3_235_025 = mat[2][0] * det2_35_25 - mat[2][2] * det2_35_05 + mat[2][5] * det2_35_02;
  1736. float det3_235_034 = mat[2][0] * det2_35_34 - mat[2][3] * det2_35_04 + mat[2][4] * det2_35_03;
  1737. float det3_235_035 = mat[2][0] * det2_35_35 - mat[2][3] * det2_35_05 + mat[2][5] * det2_35_03;
  1738. float det3_235_045 = mat[2][0] * det2_35_45 - mat[2][4] * det2_35_05 + mat[2][5] * det2_35_04;
  1739. float det3_235_123 = mat[2][1] * det2_35_23 - mat[2][2] * det2_35_13 + mat[2][3] * det2_35_12;
  1740. float det3_235_124 = mat[2][1] * det2_35_24 - mat[2][2] * det2_35_14 + mat[2][4] * det2_35_12;
  1741. float det3_235_125 = mat[2][1] * det2_35_25 - mat[2][2] * det2_35_15 + mat[2][5] * det2_35_12;
  1742. float det3_235_134 = mat[2][1] * det2_35_34 - mat[2][3] * det2_35_14 + mat[2][4] * det2_35_13;
  1743. float det3_235_135 = mat[2][1] * det2_35_35 - mat[2][3] * det2_35_15 + mat[2][5] * det2_35_13;
  1744. float det3_235_145 = mat[2][1] * det2_35_45 - mat[2][4] * det2_35_15 + mat[2][5] * det2_35_14;
  1745. float det3_235_234 = mat[2][2] * det2_35_34 - mat[2][3] * det2_35_24 + mat[2][4] * det2_35_23;
  1746. float det3_235_235 = mat[2][2] * det2_35_35 - mat[2][3] * det2_35_25 + mat[2][5] * det2_35_23;
  1747. float det3_235_245 = mat[2][2] * det2_35_45 - mat[2][4] * det2_35_25 + mat[2][5] * det2_35_24;
  1748. float det3_235_345 = mat[2][3] * det2_35_45 - mat[2][4] * det2_35_35 + mat[2][5] * det2_35_34;
  1749. float det3_245_012 = mat[2][0] * det2_45_12 - mat[2][1] * det2_45_02 + mat[2][2] * det2_45_01;
  1750. float det3_245_013 = mat[2][0] * det2_45_13 - mat[2][1] * det2_45_03 + mat[2][3] * det2_45_01;
  1751. float det3_245_014 = mat[2][0] * det2_45_14 - mat[2][1] * det2_45_04 + mat[2][4] * det2_45_01;
  1752. float det3_245_015 = mat[2][0] * det2_45_15 - mat[2][1] * det2_45_05 + mat[2][5] * det2_45_01;
  1753. float det3_245_023 = mat[2][0] * det2_45_23 - mat[2][2] * det2_45_03 + mat[2][3] * det2_45_02;
  1754. float det3_245_024 = mat[2][0] * det2_45_24 - mat[2][2] * det2_45_04 + mat[2][4] * det2_45_02;
  1755. float det3_245_025 = mat[2][0] * det2_45_25 - mat[2][2] * det2_45_05 + mat[2][5] * det2_45_02;
  1756. float det3_245_034 = mat[2][0] * det2_45_34 - mat[2][3] * det2_45_04 + mat[2][4] * det2_45_03;
  1757. float det3_245_035 = mat[2][0] * det2_45_35 - mat[2][3] * det2_45_05 + mat[2][5] * det2_45_03;
  1758. float det3_245_045 = mat[2][0] * det2_45_45 - mat[2][4] * det2_45_05 + mat[2][5] * det2_45_04;
  1759. float det3_245_123 = mat[2][1] * det2_45_23 - mat[2][2] * det2_45_13 + mat[2][3] * det2_45_12;
  1760. float det3_245_124 = mat[2][1] * det2_45_24 - mat[2][2] * det2_45_14 + mat[2][4] * det2_45_12;
  1761. float det3_245_125 = mat[2][1] * det2_45_25 - mat[2][2] * det2_45_15 + mat[2][5] * det2_45_12;
  1762. float det3_245_134 = mat[2][1] * det2_45_34 - mat[2][3] * det2_45_14 + mat[2][4] * det2_45_13;
  1763. float det3_245_135 = mat[2][1] * det2_45_35 - mat[2][3] * det2_45_15 + mat[2][5] * det2_45_13;
  1764. float det3_245_145 = mat[2][1] * det2_45_45 - mat[2][4] * det2_45_15 + mat[2][5] * det2_45_14;
  1765. float det3_245_234 = mat[2][2] * det2_45_34 - mat[2][3] * det2_45_24 + mat[2][4] * det2_45_23;
  1766. float det3_245_235 = mat[2][2] * det2_45_35 - mat[2][3] * det2_45_25 + mat[2][5] * det2_45_23;
  1767. float det3_245_245 = mat[2][2] * det2_45_45 - mat[2][4] * det2_45_25 + mat[2][5] * det2_45_24;
  1768. float det3_245_345 = mat[2][3] * det2_45_45 - mat[2][4] * det2_45_35 + mat[2][5] * det2_45_34;
  1769. // remaining 4x4 sub-determinants
  1770. float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
  1771. float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
  1772. float det4_1234_0125 = mat[1][0] * det3_234_125 - mat[1][1] * det3_234_025 + mat[1][2] * det3_234_015 - mat[1][5] * det3_234_012;
  1773. float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
  1774. float det4_1234_0135 = mat[1][0] * det3_234_135 - mat[1][1] * det3_234_035 + mat[1][3] * det3_234_015 - mat[1][5] * det3_234_013;
  1775. float det4_1234_0145 = mat[1][0] * det3_234_145 - mat[1][1] * det3_234_045 + mat[1][4] * det3_234_015 - mat[1][5] * det3_234_014;
  1776. float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
  1777. float det4_1234_0235 = mat[1][0] * det3_234_235 - mat[1][2] * det3_234_035 + mat[1][3] * det3_234_025 - mat[1][5] * det3_234_023;
  1778. float det4_1234_0245 = mat[1][0] * det3_234_245 - mat[1][2] * det3_234_045 + mat[1][4] * det3_234_025 - mat[1][5] * det3_234_024;
  1779. float det4_1234_0345 = mat[1][0] * det3_234_345 - mat[1][3] * det3_234_045 + mat[1][4] * det3_234_035 - mat[1][5] * det3_234_034;
  1780. float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
  1781. float det4_1234_1235 = mat[1][1] * det3_234_235 - mat[1][2] * det3_234_135 + mat[1][3] * det3_234_125 - mat[1][5] * det3_234_123;
  1782. float det4_1234_1245 = mat[1][1] * det3_234_245 - mat[1][2] * det3_234_145 + mat[1][4] * det3_234_125 - mat[1][5] * det3_234_124;
  1783. float det4_1234_1345 = mat[1][1] * det3_234_345 - mat[1][3] * det3_234_145 + mat[1][4] * det3_234_135 - mat[1][5] * det3_234_134;
  1784. float det4_1234_2345 = mat[1][2] * det3_234_345 - mat[1][3] * det3_234_245 + mat[1][4] * det3_234_235 - mat[1][5] * det3_234_234;
  1785. float det4_1235_0123 = mat[1][0] * det3_235_123 - mat[1][1] * det3_235_023 + mat[1][2] * det3_235_013 - mat[1][3] * det3_235_012;
  1786. float det4_1235_0124 = mat[1][0] * det3_235_124 - mat[1][1] * det3_235_024 + mat[1][2] * det3_235_014 - mat[1][4] * det3_235_012;
  1787. float det4_1235_0125 = mat[1][0] * det3_235_125 - mat[1][1] * det3_235_025 + mat[1][2] * det3_235_015 - mat[1][5] * det3_235_012;
  1788. float det4_1235_0134 = mat[1][0] * det3_235_134 - mat[1][1] * det3_235_034 + mat[1][3] * det3_235_014 - mat[1][4] * det3_235_013;
  1789. float det4_1235_0135 = mat[1][0] * det3_235_135 - mat[1][1] * det3_235_035 + mat[1][3] * det3_235_015 - mat[1][5] * det3_235_013;
  1790. float det4_1235_0145 = mat[1][0] * det3_235_145 - mat[1][1] * det3_235_045 + mat[1][4] * det3_235_015 - mat[1][5] * det3_235_014;
  1791. float det4_1235_0234 = mat[1][0] * det3_235_234 - mat[1][2] * det3_235_034 + mat[1][3] * det3_235_024 - mat[1][4] * det3_235_023;
  1792. float det4_1235_0235 = mat[1][0] * det3_235_235 - mat[1][2] * det3_235_035 + mat[1][3] * det3_235_025 - mat[1][5] * det3_235_023;
  1793. float det4_1235_0245 = mat[1][0] * det3_235_245 - mat[1][2] * det3_235_045 + mat[1][4] * det3_235_025 - mat[1][5] * det3_235_024;
  1794. float det4_1235_0345 = mat[1][0] * det3_235_345 - mat[1][3] * det3_235_045 + mat[1][4] * det3_235_035 - mat[1][5] * det3_235_034;
  1795. float det4_1235_1234 = mat[1][1] * det3_235_234 - mat[1][2] * det3_235_134 + mat[1][3] * det3_235_124 - mat[1][4] * det3_235_123;
  1796. float det4_1235_1235 = mat[1][1] * det3_235_235 - mat[1][2] * det3_235_135 + mat[1][3] * det3_235_125 - mat[1][5] * det3_235_123;
  1797. float det4_1235_1245 = mat[1][1] * det3_235_245 - mat[1][2] * det3_235_145 + mat[1][4] * det3_235_125 - mat[1][5] * det3_235_124;
  1798. float det4_1235_1345 = mat[1][1] * det3_235_345 - mat[1][3] * det3_235_145 + mat[1][4] * det3_235_135 - mat[1][5] * det3_235_134;
  1799. float det4_1235_2345 = mat[1][2] * det3_235_345 - mat[1][3] * det3_235_245 + mat[1][4] * det3_235_235 - mat[1][5] * det3_235_234;
  1800. float det4_1245_0123 = mat[1][0] * det3_245_123 - mat[1][1] * det3_245_023 + mat[1][2] * det3_245_013 - mat[1][3] * det3_245_012;
  1801. float det4_1245_0124 = mat[1][0] * det3_245_124 - mat[1][1] * det3_245_024 + mat[1][2] * det3_245_014 - mat[1][4] * det3_245_012;
  1802. float det4_1245_0125 = mat[1][0] * det3_245_125 - mat[1][1] * det3_245_025 + mat[1][2] * det3_245_015 - mat[1][5] * det3_245_012;
  1803. float det4_1245_0134 = mat[1][0] * det3_245_134 - mat[1][1] * det3_245_034 + mat[1][3] * det3_245_014 - mat[1][4] * det3_245_013;
  1804. float det4_1245_0135 = mat[1][0] * det3_245_135 - mat[1][1] * det3_245_035 + mat[1][3] * det3_245_015 - mat[1][5] * det3_245_013;
  1805. float det4_1245_0145 = mat[1][0] * det3_245_145 - mat[1][1] * det3_245_045 + mat[1][4] * det3_245_015 - mat[1][5] * det3_245_014;
  1806. float det4_1245_0234 = mat[1][0] * det3_245_234 - mat[1][2] * det3_245_034 + mat[1][3] * det3_245_024 - mat[1][4] * det3_245_023;
  1807. float det4_1245_0235 = mat[1][0] * det3_245_235 - mat[1][2] * det3_245_035 + mat[1][3] * det3_245_025 - mat[1][5] * det3_245_023;
  1808. float det4_1245_0245 = mat[1][0] * det3_245_245 - mat[1][2] * det3_245_045 + mat[1][4] * det3_245_025 - mat[1][5] * det3_245_024;
  1809. float det4_1245_0345 = mat[1][0] * det3_245_345 - mat[1][3] * det3_245_045 + mat[1][4] * det3_245_035 - mat[1][5] * det3_245_034;
  1810. float det4_1245_1234 = mat[1][1] * det3_245_234 - mat[1][2] * det3_245_134 + mat[1][3] * det3_245_124 - mat[1][4] * det3_245_123;
  1811. float det4_1245_1235 = mat[1][1] * det3_245_235 - mat[1][2] * det3_245_135 + mat[1][3] * det3_245_125 - mat[1][5] * det3_245_123;
  1812. float det4_1245_1245 = mat[1][1] * det3_245_245 - mat[1][2] * det3_245_145 + mat[1][4] * det3_245_125 - mat[1][5] * det3_245_124;
  1813. float det4_1245_1345 = mat[1][1] * det3_245_345 - mat[1][3] * det3_245_145 + mat[1][4] * det3_245_135 - mat[1][5] * det3_245_134;
  1814. float det4_1245_2345 = mat[1][2] * det3_245_345 - mat[1][3] * det3_245_245 + mat[1][4] * det3_245_235 - mat[1][5] * det3_245_234;
  1815. float det4_1345_0123 = mat[1][0] * det3_345_123 - mat[1][1] * det3_345_023 + mat[1][2] * det3_345_013 - mat[1][3] * det3_345_012;
  1816. float det4_1345_0124 = mat[1][0] * det3_345_124 - mat[1][1] * det3_345_024 + mat[1][2] * det3_345_014 - mat[1][4] * det3_345_012;
  1817. float det4_1345_0125 = mat[1][0] * det3_345_125 - mat[1][1] * det3_345_025 + mat[1][2] * det3_345_015 - mat[1][5] * det3_345_012;
  1818. float det4_1345_0134 = mat[1][0] * det3_345_134 - mat[1][1] * det3_345_034 + mat[1][3] * det3_345_014 - mat[1][4] * det3_345_013;
  1819. float det4_1345_0135 = mat[1][0] * det3_345_135 - mat[1][1] * det3_345_035 + mat[1][3] * det3_345_015 - mat[1][5] * det3_345_013;
  1820. float det4_1345_0145 = mat[1][0] * det3_345_145 - mat[1][1] * det3_345_045 + mat[1][4] * det3_345_015 - mat[1][5] * det3_345_014;
  1821. float det4_1345_0234 = mat[1][0] * det3_345_234 - mat[1][2] * det3_345_034 + mat[1][3] * det3_345_024 - mat[1][4] * det3_345_023;
  1822. float det4_1345_0235 = mat[1][0] * det3_345_235 - mat[1][2] * det3_345_035 + mat[1][3] * det3_345_025 - mat[1][5] * det3_345_023;
  1823. float det4_1345_0245 = mat[1][0] * det3_345_245 - mat[1][2] * det3_345_045 + mat[1][4] * det3_345_025 - mat[1][5] * det3_345_024;
  1824. float det4_1345_0345 = mat[1][0] * det3_345_345 - mat[1][3] * det3_345_045 + mat[1][4] * det3_345_035 - mat[1][5] * det3_345_034;
  1825. float det4_1345_1234 = mat[1][1] * det3_345_234 - mat[1][2] * det3_345_134 + mat[1][3] * det3_345_124 - mat[1][4] * det3_345_123;
  1826. float det4_1345_1235 = mat[1][1] * det3_345_235 - mat[1][2] * det3_345_135 + mat[1][3] * det3_345_125 - mat[1][5] * det3_345_123;
  1827. float det4_1345_1245 = mat[1][1] * det3_345_245 - mat[1][2] * det3_345_145 + mat[1][4] * det3_345_125 - mat[1][5] * det3_345_124;
  1828. float det4_1345_1345 = mat[1][1] * det3_345_345 - mat[1][3] * det3_345_145 + mat[1][4] * det3_345_135 - mat[1][5] * det3_345_134;
  1829. float det4_1345_2345 = mat[1][2] * det3_345_345 - mat[1][3] * det3_345_245 + mat[1][4] * det3_345_235 - mat[1][5] * det3_345_234;
  1830. // remaining 5x5 sub-determinants
  1831. float det5_01234_01234 = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
  1832. float det5_01234_01235 = mat[0][0] * det4_1234_1235 - mat[0][1] * det4_1234_0235 + mat[0][2] * det4_1234_0135 - mat[0][3] * det4_1234_0125 + mat[0][5] * det4_1234_0123;
  1833. float det5_01234_01245 = mat[0][0] * det4_1234_1245 - mat[0][1] * det4_1234_0245 + mat[0][2] * det4_1234_0145 - mat[0][4] * det4_1234_0125 + mat[0][5] * det4_1234_0124;
  1834. float det5_01234_01345 = mat[0][0] * det4_1234_1345 - mat[0][1] * det4_1234_0345 + mat[0][3] * det4_1234_0145 - mat[0][4] * det4_1234_0135 + mat[0][5] * det4_1234_0134;
  1835. float det5_01234_02345 = mat[0][0] * det4_1234_2345 - mat[0][2] * det4_1234_0345 + mat[0][3] * det4_1234_0245 - mat[0][4] * det4_1234_0235 + mat[0][5] * det4_1234_0234;
  1836. float det5_01234_12345 = mat[0][1] * det4_1234_2345 - mat[0][2] * det4_1234_1345 + mat[0][3] * det4_1234_1245 - mat[0][4] * det4_1234_1235 + mat[0][5] * det4_1234_1234;
  1837. float det5_01235_01234 = mat[0][0] * det4_1235_1234 - mat[0][1] * det4_1235_0234 + mat[0][2] * det4_1235_0134 - mat[0][3] * det4_1235_0124 + mat[0][4] * det4_1235_0123;
  1838. float det5_01235_01235 = mat[0][0] * det4_1235_1235 - mat[0][1] * det4_1235_0235 + mat[0][2] * det4_1235_0135 - mat[0][3] * det4_1235_0125 + mat[0][5] * det4_1235_0123;
  1839. float det5_01235_01245 = mat[0][0] * det4_1235_1245 - mat[0][1] * det4_1235_0245 + mat[0][2] * det4_1235_0145 - mat[0][4] * det4_1235_0125 + mat[0][5] * det4_1235_0124;
  1840. float det5_01235_01345 = mat[0][0] * det4_1235_1345 - mat[0][1] * det4_1235_0345 + mat[0][3] * det4_1235_0145 - mat[0][4] * det4_1235_0135 + mat[0][5] * det4_1235_0134;
  1841. float det5_01235_02345 = mat[0][0] * det4_1235_2345 - mat[0][2] * det4_1235_0345 + mat[0][3] * det4_1235_0245 - mat[0][4] * det4_1235_0235 + mat[0][5] * det4_1235_0234;
  1842. float det5_01235_12345 = mat[0][1] * det4_1235_2345 - mat[0][2] * det4_1235_1345 + mat[0][3] * det4_1235_1245 - mat[0][4] * det4_1235_1235 + mat[0][5] * det4_1235_1234;
  1843. float det5_01245_01234 = mat[0][0] * det4_1245_1234 - mat[0][1] * det4_1245_0234 + mat[0][2] * det4_1245_0134 - mat[0][3] * det4_1245_0124 + mat[0][4] * det4_1245_0123;
  1844. float det5_01245_01235 = mat[0][0] * det4_1245_1235 - mat[0][1] * det4_1245_0235 + mat[0][2] * det4_1245_0135 - mat[0][3] * det4_1245_0125 + mat[0][5] * det4_1245_0123;
  1845. float det5_01245_01245 = mat[0][0] * det4_1245_1245 - mat[0][1] * det4_1245_0245 + mat[0][2] * det4_1245_0145 - mat[0][4] * det4_1245_0125 + mat[0][5] * det4_1245_0124;
  1846. float det5_01245_01345 = mat[0][0] * det4_1245_1345 - mat[0][1] * det4_1245_0345 + mat[0][3] * det4_1245_0145 - mat[0][4] * det4_1245_0135 + mat[0][5] * det4_1245_0134;
  1847. float det5_01245_02345 = mat[0][0] * det4_1245_2345 - mat[0][2] * det4_1245_0345 + mat[0][3] * det4_1245_0245 - mat[0][4] * det4_1245_0235 + mat[0][5] * det4_1245_0234;
  1848. float det5_01245_12345 = mat[0][1] * det4_1245_2345 - mat[0][2] * det4_1245_1345 + mat[0][3] * det4_1245_1245 - mat[0][4] * det4_1245_1235 + mat[0][5] * det4_1245_1234;
  1849. float det5_01345_01234 = mat[0][0] * det4_1345_1234 - mat[0][1] * det4_1345_0234 + mat[0][2] * det4_1345_0134 - mat[0][3] * det4_1345_0124 + mat[0][4] * det4_1345_0123;
  1850. float det5_01345_01235 = mat[0][0] * det4_1345_1235 - mat[0][1] * det4_1345_0235 + mat[0][2] * det4_1345_0135 - mat[0][3] * det4_1345_0125 + mat[0][5] * det4_1345_0123;
  1851. float det5_01345_01245 = mat[0][0] * det4_1345_1245 - mat[0][1] * det4_1345_0245 + mat[0][2] * det4_1345_0145 - mat[0][4] * det4_1345_0125 + mat[0][5] * det4_1345_0124;
  1852. float det5_01345_01345 = mat[0][0] * det4_1345_1345 - mat[0][1] * det4_1345_0345 + mat[0][3] * det4_1345_0145 - mat[0][4] * det4_1345_0135 + mat[0][5] * det4_1345_0134;
  1853. float det5_01345_02345 = mat[0][0] * det4_1345_2345 - mat[0][2] * det4_1345_0345 + mat[0][3] * det4_1345_0245 - mat[0][4] * det4_1345_0235 + mat[0][5] * det4_1345_0234;
  1854. float det5_01345_12345 = mat[0][1] * det4_1345_2345 - mat[0][2] * det4_1345_1345 + mat[0][3] * det4_1345_1245 - mat[0][4] * det4_1345_1235 + mat[0][5] * det4_1345_1234;
  1855. float det5_02345_01234 = mat[0][0] * det4_2345_1234 - mat[0][1] * det4_2345_0234 + mat[0][2] * det4_2345_0134 - mat[0][3] * det4_2345_0124 + mat[0][4] * det4_2345_0123;
  1856. float det5_02345_01235 = mat[0][0] * det4_2345_1235 - mat[0][1] * det4_2345_0235 + mat[0][2] * det4_2345_0135 - mat[0][3] * det4_2345_0125 + mat[0][5] * det4_2345_0123;
  1857. float det5_02345_01245 = mat[0][0] * det4_2345_1245 - mat[0][1] * det4_2345_0245 + mat[0][2] * det4_2345_0145 - mat[0][4] * det4_2345_0125 + mat[0][5] * det4_2345_0124;
  1858. float det5_02345_01345 = mat[0][0] * det4_2345_1345 - mat[0][1] * det4_2345_0345 + mat[0][3] * det4_2345_0145 - mat[0][4] * det4_2345_0135 + mat[0][5] * det4_2345_0134;
  1859. float det5_02345_02345 = mat[0][0] * det4_2345_2345 - mat[0][2] * det4_2345_0345 + mat[0][3] * det4_2345_0245 - mat[0][4] * det4_2345_0235 + mat[0][5] * det4_2345_0234;
  1860. float det5_02345_12345 = mat[0][1] * det4_2345_2345 - mat[0][2] * det4_2345_1345 + mat[0][3] * det4_2345_1245 - mat[0][4] * det4_2345_1235 + mat[0][5] * det4_2345_1234;
  1861. mat[0][0] = det5_12345_12345 * invDet;
  1862. mat[0][1] = -det5_02345_12345 * invDet;
  1863. mat[0][2] = det5_01345_12345 * invDet;
  1864. mat[0][3] = -det5_01245_12345 * invDet;
  1865. mat[0][4] = det5_01235_12345 * invDet;
  1866. mat[0][5] = -det5_01234_12345 * invDet;
  1867. mat[1][0] = -det5_12345_02345 * invDet;
  1868. mat[1][1] = det5_02345_02345 * invDet;
  1869. mat[1][2] = -det5_01345_02345 * invDet;
  1870. mat[1][3] = det5_01245_02345 * invDet;
  1871. mat[1][4] = -det5_01235_02345 * invDet;
  1872. mat[1][5] = det5_01234_02345 * invDet;
  1873. mat[2][0] = det5_12345_01345 * invDet;
  1874. mat[2][1] = -det5_02345_01345 * invDet;
  1875. mat[2][2] = det5_01345_01345 * invDet;
  1876. mat[2][3] = -det5_01245_01345 * invDet;
  1877. mat[2][4] = det5_01235_01345 * invDet;
  1878. mat[2][5] = -det5_01234_01345 * invDet;
  1879. mat[3][0] = -det5_12345_01245 * invDet;
  1880. mat[3][1] = det5_02345_01245 * invDet;
  1881. mat[3][2] = -det5_01345_01245 * invDet;
  1882. mat[3][3] = det5_01245_01245 * invDet;
  1883. mat[3][4] = -det5_01235_01245 * invDet;
  1884. mat[3][5] = det5_01234_01245 * invDet;
  1885. mat[4][0] = det5_12345_01235 * invDet;
  1886. mat[4][1] = -det5_02345_01235 * invDet;
  1887. mat[4][2] = det5_01345_01235 * invDet;
  1888. mat[4][3] = -det5_01245_01235 * invDet;
  1889. mat[4][4] = det5_01235_01235 * invDet;
  1890. mat[4][5] = -det5_01234_01235 * invDet;
  1891. mat[5][0] = -det5_12345_01234 * invDet;
  1892. mat[5][1] = det5_02345_01234 * invDet;
  1893. mat[5][2] = -det5_01345_01234 * invDet;
  1894. mat[5][3] = det5_01245_01234 * invDet;
  1895. mat[5][4] = -det5_01235_01234 * invDet;
  1896. mat[5][5] = det5_01234_01234 * invDet;
  1897. return true;
  1898. }
  1899. /*
  1900. ============
  1901. idMat6::InverseFastSelf
  1902. ============
  1903. */
  1904. bool idMat6::InverseFastSelf() {
  1905. #if 0
  1906. // 810+6+36 = 852 multiplications
  1907. // 1 division
  1908. double det, invDet;
  1909. // 2x2 sub-determinants required to calculate 6x6 determinant
  1910. float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0];
  1911. float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0];
  1912. float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0];
  1913. float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0];
  1914. float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0];
  1915. float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1];
  1916. float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1];
  1917. float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1];
  1918. float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1];
  1919. float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2];
  1920. float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2];
  1921. float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2];
  1922. float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3];
  1923. float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3];
  1924. float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4];
  1925. // 3x3 sub-determinants required to calculate 6x6 determinant
  1926. float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01;
  1927. float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01;
  1928. float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01;
  1929. float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01;
  1930. float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02;
  1931. float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02;
  1932. float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02;
  1933. float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03;
  1934. float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03;
  1935. float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04;
  1936. float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12;
  1937. float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12;
  1938. float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12;
  1939. float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13;
  1940. float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13;
  1941. float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14;
  1942. float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23;
  1943. float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23;
  1944. float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24;
  1945. float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34;
  1946. // 4x4 sub-determinants required to calculate 6x6 determinant
  1947. float det4_2345_0123 = mat[2][0] * det3_345_123 - mat[2][1] * det3_345_023 + mat[2][2] * det3_345_013 - mat[2][3] * det3_345_012;
  1948. float det4_2345_0124 = mat[2][0] * det3_345_124 - mat[2][1] * det3_345_024 + mat[2][2] * det3_345_014 - mat[2][4] * det3_345_012;
  1949. float det4_2345_0125 = mat[2][0] * det3_345_125 - mat[2][1] * det3_345_025 + mat[2][2] * det3_345_015 - mat[2][5] * det3_345_012;
  1950. float det4_2345_0134 = mat[2][0] * det3_345_134 - mat[2][1] * det3_345_034 + mat[2][3] * det3_345_014 - mat[2][4] * det3_345_013;
  1951. float det4_2345_0135 = mat[2][0] * det3_345_135 - mat[2][1] * det3_345_035 + mat[2][3] * det3_345_015 - mat[2][5] * det3_345_013;
  1952. float det4_2345_0145 = mat[2][0] * det3_345_145 - mat[2][1] * det3_345_045 + mat[2][4] * det3_345_015 - mat[2][5] * det3_345_014;
  1953. float det4_2345_0234 = mat[2][0] * det3_345_234 - mat[2][2] * det3_345_034 + mat[2][3] * det3_345_024 - mat[2][4] * det3_345_023;
  1954. float det4_2345_0235 = mat[2][0] * det3_345_235 - mat[2][2] * det3_345_035 + mat[2][3] * det3_345_025 - mat[2][5] * det3_345_023;
  1955. float det4_2345_0245 = mat[2][0] * det3_345_245 - mat[2][2] * det3_345_045 + mat[2][4] * det3_345_025 - mat[2][5] * det3_345_024;
  1956. float det4_2345_0345 = mat[2][0] * det3_345_345 - mat[2][3] * det3_345_045 + mat[2][4] * det3_345_035 - mat[2][5] * det3_345_034;
  1957. float det4_2345_1234 = mat[2][1] * det3_345_234 - mat[2][2] * det3_345_134 + mat[2][3] * det3_345_124 - mat[2][4] * det3_345_123;
  1958. float det4_2345_1235 = mat[2][1] * det3_345_235 - mat[2][2] * det3_345_135 + mat[2][3] * det3_345_125 - mat[2][5] * det3_345_123;
  1959. float det4_2345_1245 = mat[2][1] * det3_345_245 - mat[2][2] * det3_345_145 + mat[2][4] * det3_345_125 - mat[2][5] * det3_345_124;
  1960. float det4_2345_1345 = mat[2][1] * det3_345_345 - mat[2][3] * det3_345_145 + mat[2][4] * det3_345_135 - mat[2][5] * det3_345_134;
  1961. float det4_2345_2345 = mat[2][2] * det3_345_345 - mat[2][3] * det3_345_245 + mat[2][4] * det3_345_235 - mat[2][5] * det3_345_234;
  1962. // 5x5 sub-determinants required to calculate 6x6 determinant
  1963. float det5_12345_01234 = mat[1][0] * det4_2345_1234 - mat[1][1] * det4_2345_0234 + mat[1][2] * det4_2345_0134 - mat[1][3] * det4_2345_0124 + mat[1][4] * det4_2345_0123;
  1964. float det5_12345_01235 = mat[1][0] * det4_2345_1235 - mat[1][1] * det4_2345_0235 + mat[1][2] * det4_2345_0135 - mat[1][3] * det4_2345_0125 + mat[1][5] * det4_2345_0123;
  1965. float det5_12345_01245 = mat[1][0] * det4_2345_1245 - mat[1][1] * det4_2345_0245 + mat[1][2] * det4_2345_0145 - mat[1][4] * det4_2345_0125 + mat[1][5] * det4_2345_0124;
  1966. float det5_12345_01345 = mat[1][0] * det4_2345_1345 - mat[1][1] * det4_2345_0345 + mat[1][3] * det4_2345_0145 - mat[1][4] * det4_2345_0135 + mat[1][5] * det4_2345_0134;
  1967. float det5_12345_02345 = mat[1][0] * det4_2345_2345 - mat[1][2] * det4_2345_0345 + mat[1][3] * det4_2345_0245 - mat[1][4] * det4_2345_0235 + mat[1][5] * det4_2345_0234;
  1968. float det5_12345_12345 = mat[1][1] * det4_2345_2345 - mat[1][2] * det4_2345_1345 + mat[1][3] * det4_2345_1245 - mat[1][4] * det4_2345_1235 + mat[1][5] * det4_2345_1234;
  1969. // determinant of 6x6 matrix
  1970. det = mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 -
  1971. mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234;
  1972. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  1973. return false;
  1974. }
  1975. invDet = 1.0f / det;
  1976. // remaining 2x2 sub-determinants
  1977. float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
  1978. float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
  1979. float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
  1980. float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
  1981. float det2_34_05 = mat[3][0] * mat[4][5] - mat[3][5] * mat[4][0];
  1982. float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
  1983. float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
  1984. float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
  1985. float det2_34_15 = mat[3][1] * mat[4][5] - mat[3][5] * mat[4][1];
  1986. float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
  1987. float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
  1988. float det2_34_25 = mat[3][2] * mat[4][5] - mat[3][5] * mat[4][2];
  1989. float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
  1990. float det2_34_35 = mat[3][3] * mat[4][5] - mat[3][5] * mat[4][3];
  1991. float det2_34_45 = mat[3][4] * mat[4][5] - mat[3][5] * mat[4][4];
  1992. float det2_35_01 = mat[3][0] * mat[5][1] - mat[3][1] * mat[5][0];
  1993. float det2_35_02 = mat[3][0] * mat[5][2] - mat[3][2] * mat[5][0];
  1994. float det2_35_03 = mat[3][0] * mat[5][3] - mat[3][3] * mat[5][0];
  1995. float det2_35_04 = mat[3][0] * mat[5][4] - mat[3][4] * mat[5][0];
  1996. float det2_35_05 = mat[3][0] * mat[5][5] - mat[3][5] * mat[5][0];
  1997. float det2_35_12 = mat[3][1] * mat[5][2] - mat[3][2] * mat[5][1];
  1998. float det2_35_13 = mat[3][1] * mat[5][3] - mat[3][3] * mat[5][1];
  1999. float det2_35_14 = mat[3][1] * mat[5][4] - mat[3][4] * mat[5][1];
  2000. float det2_35_15 = mat[3][1] * mat[5][5] - mat[3][5] * mat[5][1];
  2001. float det2_35_23 = mat[3][2] * mat[5][3] - mat[3][3] * mat[5][2];
  2002. float det2_35_24 = mat[3][2] * mat[5][4] - mat[3][4] * mat[5][2];
  2003. float det2_35_25 = mat[3][2] * mat[5][5] - mat[3][5] * mat[5][2];
  2004. float det2_35_34 = mat[3][3] * mat[5][4] - mat[3][4] * mat[5][3];
  2005. float det2_35_35 = mat[3][3] * mat[5][5] - mat[3][5] * mat[5][3];
  2006. float det2_35_45 = mat[3][4] * mat[5][5] - mat[3][5] * mat[5][4];
  2007. // remaining 3x3 sub-determinants
  2008. float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
  2009. float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
  2010. float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
  2011. float det3_234_015 = mat[2][0] * det2_34_15 - mat[2][1] * det2_34_05 + mat[2][5] * det2_34_01;
  2012. float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
  2013. float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
  2014. float det3_234_025 = mat[2][0] * det2_34_25 - mat[2][2] * det2_34_05 + mat[2][5] * det2_34_02;
  2015. float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
  2016. float det3_234_035 = mat[2][0] * det2_34_35 - mat[2][3] * det2_34_05 + mat[2][5] * det2_34_03;
  2017. float det3_234_045 = mat[2][0] * det2_34_45 - mat[2][4] * det2_34_05 + mat[2][5] * det2_34_04;
  2018. float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
  2019. float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
  2020. float det3_234_125 = mat[2][1] * det2_34_25 - mat[2][2] * det2_34_15 + mat[2][5] * det2_34_12;
  2021. float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
  2022. float det3_234_135 = mat[2][1] * det2_34_35 - mat[2][3] * det2_34_15 + mat[2][5] * det2_34_13;
  2023. float det3_234_145 = mat[2][1] * det2_34_45 - mat[2][4] * det2_34_15 + mat[2][5] * det2_34_14;
  2024. float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
  2025. float det3_234_235 = mat[2][2] * det2_34_35 - mat[2][3] * det2_34_25 + mat[2][5] * det2_34_23;
  2026. float det3_234_245 = mat[2][2] * det2_34_45 - mat[2][4] * det2_34_25 + mat[2][5] * det2_34_24;
  2027. float det3_234_345 = mat[2][3] * det2_34_45 - mat[2][4] * det2_34_35 + mat[2][5] * det2_34_34;
  2028. float det3_235_012 = mat[2][0] * det2_35_12 - mat[2][1] * det2_35_02 + mat[2][2] * det2_35_01;
  2029. float det3_235_013 = mat[2][0] * det2_35_13 - mat[2][1] * det2_35_03 + mat[2][3] * det2_35_01;
  2030. float det3_235_014 = mat[2][0] * det2_35_14 - mat[2][1] * det2_35_04 + mat[2][4] * det2_35_01;
  2031. float det3_235_015 = mat[2][0] * det2_35_15 - mat[2][1] * det2_35_05 + mat[2][5] * det2_35_01;
  2032. float det3_235_023 = mat[2][0] * det2_35_23 - mat[2][2] * det2_35_03 + mat[2][3] * det2_35_02;
  2033. float det3_235_024 = mat[2][0] * det2_35_24 - mat[2][2] * det2_35_04 + mat[2][4] * det2_35_02;
  2034. float det3_235_025 = mat[2][0] * det2_35_25 - mat[2][2] * det2_35_05 + mat[2][5] * det2_35_02;
  2035. float det3_235_034 = mat[2][0] * det2_35_34 - mat[2][3] * det2_35_04 + mat[2][4] * det2_35_03;
  2036. float det3_235_035 = mat[2][0] * det2_35_35 - mat[2][3] * det2_35_05 + mat[2][5] * det2_35_03;
  2037. float det3_235_045 = mat[2][0] * det2_35_45 - mat[2][4] * det2_35_05 + mat[2][5] * det2_35_04;
  2038. float det3_235_123 = mat[2][1] * det2_35_23 - mat[2][2] * det2_35_13 + mat[2][3] * det2_35_12;
  2039. float det3_235_124 = mat[2][1] * det2_35_24 - mat[2][2] * det2_35_14 + mat[2][4] * det2_35_12;
  2040. float det3_235_125 = mat[2][1] * det2_35_25 - mat[2][2] * det2_35_15 + mat[2][5] * det2_35_12;
  2041. float det3_235_134 = mat[2][1] * det2_35_34 - mat[2][3] * det2_35_14 + mat[2][4] * det2_35_13;
  2042. float det3_235_135 = mat[2][1] * det2_35_35 - mat[2][3] * det2_35_15 + mat[2][5] * det2_35_13;
  2043. float det3_235_145 = mat[2][1] * det2_35_45 - mat[2][4] * det2_35_15 + mat[2][5] * det2_35_14;
  2044. float det3_235_234 = mat[2][2] * det2_35_34 - mat[2][3] * det2_35_24 + mat[2][4] * det2_35_23;
  2045. float det3_235_235 = mat[2][2] * det2_35_35 - mat[2][3] * det2_35_25 + mat[2][5] * det2_35_23;
  2046. float det3_235_245 = mat[2][2] * det2_35_45 - mat[2][4] * det2_35_25 + mat[2][5] * det2_35_24;
  2047. float det3_235_345 = mat[2][3] * det2_35_45 - mat[2][4] * det2_35_35 + mat[2][5] * det2_35_34;
  2048. float det3_245_012 = mat[2][0] * det2_45_12 - mat[2][1] * det2_45_02 + mat[2][2] * det2_45_01;
  2049. float det3_245_013 = mat[2][0] * det2_45_13 - mat[2][1] * det2_45_03 + mat[2][3] * det2_45_01;
  2050. float det3_245_014 = mat[2][0] * det2_45_14 - mat[2][1] * det2_45_04 + mat[2][4] * det2_45_01;
  2051. float det3_245_015 = mat[2][0] * det2_45_15 - mat[2][1] * det2_45_05 + mat[2][5] * det2_45_01;
  2052. float det3_245_023 = mat[2][0] * det2_45_23 - mat[2][2] * det2_45_03 + mat[2][3] * det2_45_02;
  2053. float det3_245_024 = mat[2][0] * det2_45_24 - mat[2][2] * det2_45_04 + mat[2][4] * det2_45_02;
  2054. float det3_245_025 = mat[2][0] * det2_45_25 - mat[2][2] * det2_45_05 + mat[2][5] * det2_45_02;
  2055. float det3_245_034 = mat[2][0] * det2_45_34 - mat[2][3] * det2_45_04 + mat[2][4] * det2_45_03;
  2056. float det3_245_035 = mat[2][0] * det2_45_35 - mat[2][3] * det2_45_05 + mat[2][5] * det2_45_03;
  2057. float det3_245_045 = mat[2][0] * det2_45_45 - mat[2][4] * det2_45_05 + mat[2][5] * det2_45_04;
  2058. float det3_245_123 = mat[2][1] * det2_45_23 - mat[2][2] * det2_45_13 + mat[2][3] * det2_45_12;
  2059. float det3_245_124 = mat[2][1] * det2_45_24 - mat[2][2] * det2_45_14 + mat[2][4] * det2_45_12;
  2060. float det3_245_125 = mat[2][1] * det2_45_25 - mat[2][2] * det2_45_15 + mat[2][5] * det2_45_12;
  2061. float det3_245_134 = mat[2][1] * det2_45_34 - mat[2][3] * det2_45_14 + mat[2][4] * det2_45_13;
  2062. float det3_245_135 = mat[2][1] * det2_45_35 - mat[2][3] * det2_45_15 + mat[2][5] * det2_45_13;
  2063. float det3_245_145 = mat[2][1] * det2_45_45 - mat[2][4] * det2_45_15 + mat[2][5] * det2_45_14;
  2064. float det3_245_234 = mat[2][2] * det2_45_34 - mat[2][3] * det2_45_24 + mat[2][4] * det2_45_23;
  2065. float det3_245_235 = mat[2][2] * det2_45_35 - mat[2][3] * det2_45_25 + mat[2][5] * det2_45_23;
  2066. float det3_245_245 = mat[2][2] * det2_45_45 - mat[2][4] * det2_45_25 + mat[2][5] * det2_45_24;
  2067. float det3_245_345 = mat[2][3] * det2_45_45 - mat[2][4] * det2_45_35 + mat[2][5] * det2_45_34;
  2068. // remaining 4x4 sub-determinants
  2069. float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
  2070. float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
  2071. float det4_1234_0125 = mat[1][0] * det3_234_125 - mat[1][1] * det3_234_025 + mat[1][2] * det3_234_015 - mat[1][5] * det3_234_012;
  2072. float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
  2073. float det4_1234_0135 = mat[1][0] * det3_234_135 - mat[1][1] * det3_234_035 + mat[1][3] * det3_234_015 - mat[1][5] * det3_234_013;
  2074. float det4_1234_0145 = mat[1][0] * det3_234_145 - mat[1][1] * det3_234_045 + mat[1][4] * det3_234_015 - mat[1][5] * det3_234_014;
  2075. float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
  2076. float det4_1234_0235 = mat[1][0] * det3_234_235 - mat[1][2] * det3_234_035 + mat[1][3] * det3_234_025 - mat[1][5] * det3_234_023;
  2077. float det4_1234_0245 = mat[1][0] * det3_234_245 - mat[1][2] * det3_234_045 + mat[1][4] * det3_234_025 - mat[1][5] * det3_234_024;
  2078. float det4_1234_0345 = mat[1][0] * det3_234_345 - mat[1][3] * det3_234_045 + mat[1][4] * det3_234_035 - mat[1][5] * det3_234_034;
  2079. float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
  2080. float det4_1234_1235 = mat[1][1] * det3_234_235 - mat[1][2] * det3_234_135 + mat[1][3] * det3_234_125 - mat[1][5] * det3_234_123;
  2081. float det4_1234_1245 = mat[1][1] * det3_234_245 - mat[1][2] * det3_234_145 + mat[1][4] * det3_234_125 - mat[1][5] * det3_234_124;
  2082. float det4_1234_1345 = mat[1][1] * det3_234_345 - mat[1][3] * det3_234_145 + mat[1][4] * det3_234_135 - mat[1][5] * det3_234_134;
  2083. float det4_1234_2345 = mat[1][2] * det3_234_345 - mat[1][3] * det3_234_245 + mat[1][4] * det3_234_235 - mat[1][5] * det3_234_234;
  2084. float det4_1235_0123 = mat[1][0] * det3_235_123 - mat[1][1] * det3_235_023 + mat[1][2] * det3_235_013 - mat[1][3] * det3_235_012;
  2085. float det4_1235_0124 = mat[1][0] * det3_235_124 - mat[1][1] * det3_235_024 + mat[1][2] * det3_235_014 - mat[1][4] * det3_235_012;
  2086. float det4_1235_0125 = mat[1][0] * det3_235_125 - mat[1][1] * det3_235_025 + mat[1][2] * det3_235_015 - mat[1][5] * det3_235_012;
  2087. float det4_1235_0134 = mat[1][0] * det3_235_134 - mat[1][1] * det3_235_034 + mat[1][3] * det3_235_014 - mat[1][4] * det3_235_013;
  2088. float det4_1235_0135 = mat[1][0] * det3_235_135 - mat[1][1] * det3_235_035 + mat[1][3] * det3_235_015 - mat[1][5] * det3_235_013;
  2089. float det4_1235_0145 = mat[1][0] * det3_235_145 - mat[1][1] * det3_235_045 + mat[1][4] * det3_235_015 - mat[1][5] * det3_235_014;
  2090. float det4_1235_0234 = mat[1][0] * det3_235_234 - mat[1][2] * det3_235_034 + mat[1][3] * det3_235_024 - mat[1][4] * det3_235_023;
  2091. float det4_1235_0235 = mat[1][0] * det3_235_235 - mat[1][2] * det3_235_035 + mat[1][3] * det3_235_025 - mat[1][5] * det3_235_023;
  2092. float det4_1235_0245 = mat[1][0] * det3_235_245 - mat[1][2] * det3_235_045 + mat[1][4] * det3_235_025 - mat[1][5] * det3_235_024;
  2093. float det4_1235_0345 = mat[1][0] * det3_235_345 - mat[1][3] * det3_235_045 + mat[1][4] * det3_235_035 - mat[1][5] * det3_235_034;
  2094. float det4_1235_1234 = mat[1][1] * det3_235_234 - mat[1][2] * det3_235_134 + mat[1][3] * det3_235_124 - mat[1][4] * det3_235_123;
  2095. float det4_1235_1235 = mat[1][1] * det3_235_235 - mat[1][2] * det3_235_135 + mat[1][3] * det3_235_125 - mat[1][5] * det3_235_123;
  2096. float det4_1235_1245 = mat[1][1] * det3_235_245 - mat[1][2] * det3_235_145 + mat[1][4] * det3_235_125 - mat[1][5] * det3_235_124;
  2097. float det4_1235_1345 = mat[1][1] * det3_235_345 - mat[1][3] * det3_235_145 + mat[1][4] * det3_235_135 - mat[1][5] * det3_235_134;
  2098. float det4_1235_2345 = mat[1][2] * det3_235_345 - mat[1][3] * det3_235_245 + mat[1][4] * det3_235_235 - mat[1][5] * det3_235_234;
  2099. float det4_1245_0123 = mat[1][0] * det3_245_123 - mat[1][1] * det3_245_023 + mat[1][2] * det3_245_013 - mat[1][3] * det3_245_012;
  2100. float det4_1245_0124 = mat[1][0] * det3_245_124 - mat[1][1] * det3_245_024 + mat[1][2] * det3_245_014 - mat[1][4] * det3_245_012;
  2101. float det4_1245_0125 = mat[1][0] * det3_245_125 - mat[1][1] * det3_245_025 + mat[1][2] * det3_245_015 - mat[1][5] * det3_245_012;
  2102. float det4_1245_0134 = mat[1][0] * det3_245_134 - mat[1][1] * det3_245_034 + mat[1][3] * det3_245_014 - mat[1][4] * det3_245_013;
  2103. float det4_1245_0135 = mat[1][0] * det3_245_135 - mat[1][1] * det3_245_035 + mat[1][3] * det3_245_015 - mat[1][5] * det3_245_013;
  2104. float det4_1245_0145 = mat[1][0] * det3_245_145 - mat[1][1] * det3_245_045 + mat[1][4] * det3_245_015 - mat[1][5] * det3_245_014;
  2105. float det4_1245_0234 = mat[1][0] * det3_245_234 - mat[1][2] * det3_245_034 + mat[1][3] * det3_245_024 - mat[1][4] * det3_245_023;
  2106. float det4_1245_0235 = mat[1][0] * det3_245_235 - mat[1][2] * det3_245_035 + mat[1][3] * det3_245_025 - mat[1][5] * det3_245_023;
  2107. float det4_1245_0245 = mat[1][0] * det3_245_245 - mat[1][2] * det3_245_045 + mat[1][4] * det3_245_025 - mat[1][5] * det3_245_024;
  2108. float det4_1245_0345 = mat[1][0] * det3_245_345 - mat[1][3] * det3_245_045 + mat[1][4] * det3_245_035 - mat[1][5] * det3_245_034;
  2109. float det4_1245_1234 = mat[1][1] * det3_245_234 - mat[1][2] * det3_245_134 + mat[1][3] * det3_245_124 - mat[1][4] * det3_245_123;
  2110. float det4_1245_1235 = mat[1][1] * det3_245_235 - mat[1][2] * det3_245_135 + mat[1][3] * det3_245_125 - mat[1][5] * det3_245_123;
  2111. float det4_1245_1245 = mat[1][1] * det3_245_245 - mat[1][2] * det3_245_145 + mat[1][4] * det3_245_125 - mat[1][5] * det3_245_124;
  2112. float det4_1245_1345 = mat[1][1] * det3_245_345 - mat[1][3] * det3_245_145 + mat[1][4] * det3_245_135 - mat[1][5] * det3_245_134;
  2113. float det4_1245_2345 = mat[1][2] * det3_245_345 - mat[1][3] * det3_245_245 + mat[1][4] * det3_245_235 - mat[1][5] * det3_245_234;
  2114. float det4_1345_0123 = mat[1][0] * det3_345_123 - mat[1][1] * det3_345_023 + mat[1][2] * det3_345_013 - mat[1][3] * det3_345_012;
  2115. float det4_1345_0124 = mat[1][0] * det3_345_124 - mat[1][1] * det3_345_024 + mat[1][2] * det3_345_014 - mat[1][4] * det3_345_012;
  2116. float det4_1345_0125 = mat[1][0] * det3_345_125 - mat[1][1] * det3_345_025 + mat[1][2] * det3_345_015 - mat[1][5] * det3_345_012;
  2117. float det4_1345_0134 = mat[1][0] * det3_345_134 - mat[1][1] * det3_345_034 + mat[1][3] * det3_345_014 - mat[1][4] * det3_345_013;
  2118. float det4_1345_0135 = mat[1][0] * det3_345_135 - mat[1][1] * det3_345_035 + mat[1][3] * det3_345_015 - mat[1][5] * det3_345_013;
  2119. float det4_1345_0145 = mat[1][0] * det3_345_145 - mat[1][1] * det3_345_045 + mat[1][4] * det3_345_015 - mat[1][5] * det3_345_014;
  2120. float det4_1345_0234 = mat[1][0] * det3_345_234 - mat[1][2] * det3_345_034 + mat[1][3] * det3_345_024 - mat[1][4] * det3_345_023;
  2121. float det4_1345_0235 = mat[1][0] * det3_345_235 - mat[1][2] * det3_345_035 + mat[1][3] * det3_345_025 - mat[1][5] * det3_345_023;
  2122. float det4_1345_0245 = mat[1][0] * det3_345_245 - mat[1][2] * det3_345_045 + mat[1][4] * det3_345_025 - mat[1][5] * det3_345_024;
  2123. float det4_1345_0345 = mat[1][0] * det3_345_345 - mat[1][3] * det3_345_045 + mat[1][4] * det3_345_035 - mat[1][5] * det3_345_034;
  2124. float det4_1345_1234 = mat[1][1] * det3_345_234 - mat[1][2] * det3_345_134 + mat[1][3] * det3_345_124 - mat[1][4] * det3_345_123;
  2125. float det4_1345_1235 = mat[1][1] * det3_345_235 - mat[1][2] * det3_345_135 + mat[1][3] * det3_345_125 - mat[1][5] * det3_345_123;
  2126. float det4_1345_1245 = mat[1][1] * det3_345_245 - mat[1][2] * det3_345_145 + mat[1][4] * det3_345_125 - mat[1][5] * det3_345_124;
  2127. float det4_1345_1345 = mat[1][1] * det3_345_345 - mat[1][3] * det3_345_145 + mat[1][4] * det3_345_135 - mat[1][5] * det3_345_134;
  2128. float det4_1345_2345 = mat[1][2] * det3_345_345 - mat[1][3] * det3_345_245 + mat[1][4] * det3_345_235 - mat[1][5] * det3_345_234;
  2129. // remaining 5x5 sub-determinants
  2130. float det5_01234_01234 = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
  2131. float det5_01234_01235 = mat[0][0] * det4_1234_1235 - mat[0][1] * det4_1234_0235 + mat[0][2] * det4_1234_0135 - mat[0][3] * det4_1234_0125 + mat[0][5] * det4_1234_0123;
  2132. float det5_01234_01245 = mat[0][0] * det4_1234_1245 - mat[0][1] * det4_1234_0245 + mat[0][2] * det4_1234_0145 - mat[0][4] * det4_1234_0125 + mat[0][5] * det4_1234_0124;
  2133. float det5_01234_01345 = mat[0][0] * det4_1234_1345 - mat[0][1] * det4_1234_0345 + mat[0][3] * det4_1234_0145 - mat[0][4] * det4_1234_0135 + mat[0][5] * det4_1234_0134;
  2134. float det5_01234_02345 = mat[0][0] * det4_1234_2345 - mat[0][2] * det4_1234_0345 + mat[0][3] * det4_1234_0245 - mat[0][4] * det4_1234_0235 + mat[0][5] * det4_1234_0234;
  2135. float det5_01234_12345 = mat[0][1] * det4_1234_2345 - mat[0][2] * det4_1234_1345 + mat[0][3] * det4_1234_1245 - mat[0][4] * det4_1234_1235 + mat[0][5] * det4_1234_1234;
  2136. float det5_01235_01234 = mat[0][0] * det4_1235_1234 - mat[0][1] * det4_1235_0234 + mat[0][2] * det4_1235_0134 - mat[0][3] * det4_1235_0124 + mat[0][4] * det4_1235_0123;
  2137. float det5_01235_01235 = mat[0][0] * det4_1235_1235 - mat[0][1] * det4_1235_0235 + mat[0][2] * det4_1235_0135 - mat[0][3] * det4_1235_0125 + mat[0][5] * det4_1235_0123;
  2138. float det5_01235_01245 = mat[0][0] * det4_1235_1245 - mat[0][1] * det4_1235_0245 + mat[0][2] * det4_1235_0145 - mat[0][4] * det4_1235_0125 + mat[0][5] * det4_1235_0124;
  2139. float det5_01235_01345 = mat[0][0] * det4_1235_1345 - mat[0][1] * det4_1235_0345 + mat[0][3] * det4_1235_0145 - mat[0][4] * det4_1235_0135 + mat[0][5] * det4_1235_0134;
  2140. float det5_01235_02345 = mat[0][0] * det4_1235_2345 - mat[0][2] * det4_1235_0345 + mat[0][3] * det4_1235_0245 - mat[0][4] * det4_1235_0235 + mat[0][5] * det4_1235_0234;
  2141. float det5_01235_12345 = mat[0][1] * det4_1235_2345 - mat[0][2] * det4_1235_1345 + mat[0][3] * det4_1235_1245 - mat[0][4] * det4_1235_1235 + mat[0][5] * det4_1235_1234;
  2142. float det5_01245_01234 = mat[0][0] * det4_1245_1234 - mat[0][1] * det4_1245_0234 + mat[0][2] * det4_1245_0134 - mat[0][3] * det4_1245_0124 + mat[0][4] * det4_1245_0123;
  2143. float det5_01245_01235 = mat[0][0] * det4_1245_1235 - mat[0][1] * det4_1245_0235 + mat[0][2] * det4_1245_0135 - mat[0][3] * det4_1245_0125 + mat[0][5] * det4_1245_0123;
  2144. float det5_01245_01245 = mat[0][0] * det4_1245_1245 - mat[0][1] * det4_1245_0245 + mat[0][2] * det4_1245_0145 - mat[0][4] * det4_1245_0125 + mat[0][5] * det4_1245_0124;
  2145. float det5_01245_01345 = mat[0][0] * det4_1245_1345 - mat[0][1] * det4_1245_0345 + mat[0][3] * det4_1245_0145 - mat[0][4] * det4_1245_0135 + mat[0][5] * det4_1245_0134;
  2146. float det5_01245_02345 = mat[0][0] * det4_1245_2345 - mat[0][2] * det4_1245_0345 + mat[0][3] * det4_1245_0245 - mat[0][4] * det4_1245_0235 + mat[0][5] * det4_1245_0234;
  2147. float det5_01245_12345 = mat[0][1] * det4_1245_2345 - mat[0][2] * det4_1245_1345 + mat[0][3] * det4_1245_1245 - mat[0][4] * det4_1245_1235 + mat[0][5] * det4_1245_1234;
  2148. float det5_01345_01234 = mat[0][0] * det4_1345_1234 - mat[0][1] * det4_1345_0234 + mat[0][2] * det4_1345_0134 - mat[0][3] * det4_1345_0124 + mat[0][4] * det4_1345_0123;
  2149. float det5_01345_01235 = mat[0][0] * det4_1345_1235 - mat[0][1] * det4_1345_0235 + mat[0][2] * det4_1345_0135 - mat[0][3] * det4_1345_0125 + mat[0][5] * det4_1345_0123;
  2150. float det5_01345_01245 = mat[0][0] * det4_1345_1245 - mat[0][1] * det4_1345_0245 + mat[0][2] * det4_1345_0145 - mat[0][4] * det4_1345_0125 + mat[0][5] * det4_1345_0124;
  2151. float det5_01345_01345 = mat[0][0] * det4_1345_1345 - mat[0][1] * det4_1345_0345 + mat[0][3] * det4_1345_0145 - mat[0][4] * det4_1345_0135 + mat[0][5] * det4_1345_0134;
  2152. float det5_01345_02345 = mat[0][0] * det4_1345_2345 - mat[0][2] * det4_1345_0345 + mat[0][3] * det4_1345_0245 - mat[0][4] * det4_1345_0235 + mat[0][5] * det4_1345_0234;
  2153. float det5_01345_12345 = mat[0][1] * det4_1345_2345 - mat[0][2] * det4_1345_1345 + mat[0][3] * det4_1345_1245 - mat[0][4] * det4_1345_1235 + mat[0][5] * det4_1345_1234;
  2154. float det5_02345_01234 = mat[0][0] * det4_2345_1234 - mat[0][1] * det4_2345_0234 + mat[0][2] * det4_2345_0134 - mat[0][3] * det4_2345_0124 + mat[0][4] * det4_2345_0123;
  2155. float det5_02345_01235 = mat[0][0] * det4_2345_1235 - mat[0][1] * det4_2345_0235 + mat[0][2] * det4_2345_0135 - mat[0][3] * det4_2345_0125 + mat[0][5] * det4_2345_0123;
  2156. float det5_02345_01245 = mat[0][0] * det4_2345_1245 - mat[0][1] * det4_2345_0245 + mat[0][2] * det4_2345_0145 - mat[0][4] * det4_2345_0125 + mat[0][5] * det4_2345_0124;
  2157. float det5_02345_01345 = mat[0][0] * det4_2345_1345 - mat[0][1] * det4_2345_0345 + mat[0][3] * det4_2345_0145 - mat[0][4] * det4_2345_0135 + mat[0][5] * det4_2345_0134;
  2158. float det5_02345_02345 = mat[0][0] * det4_2345_2345 - mat[0][2] * det4_2345_0345 + mat[0][3] * det4_2345_0245 - mat[0][4] * det4_2345_0235 + mat[0][5] * det4_2345_0234;
  2159. float det5_02345_12345 = mat[0][1] * det4_2345_2345 - mat[0][2] * det4_2345_1345 + mat[0][3] * det4_2345_1245 - mat[0][4] * det4_2345_1235 + mat[0][5] * det4_2345_1234;
  2160. mat[0][0] = det5_12345_12345 * invDet;
  2161. mat[0][1] = -det5_02345_12345 * invDet;
  2162. mat[0][2] = det5_01345_12345 * invDet;
  2163. mat[0][3] = -det5_01245_12345 * invDet;
  2164. mat[0][4] = det5_01235_12345 * invDet;
  2165. mat[0][5] = -det5_01234_12345 * invDet;
  2166. mat[1][0] = -det5_12345_02345 * invDet;
  2167. mat[1][1] = det5_02345_02345 * invDet;
  2168. mat[1][2] = -det5_01345_02345 * invDet;
  2169. mat[1][3] = det5_01245_02345 * invDet;
  2170. mat[1][4] = -det5_01235_02345 * invDet;
  2171. mat[1][5] = det5_01234_02345 * invDet;
  2172. mat[2][0] = det5_12345_01345 * invDet;
  2173. mat[2][1] = -det5_02345_01345 * invDet;
  2174. mat[2][2] = det5_01345_01345 * invDet;
  2175. mat[2][3] = -det5_01245_01345 * invDet;
  2176. mat[2][4] = det5_01235_01345 * invDet;
  2177. mat[2][5] = -det5_01234_01345 * invDet;
  2178. mat[3][0] = -det5_12345_01245 * invDet;
  2179. mat[3][1] = det5_02345_01245 * invDet;
  2180. mat[3][2] = -det5_01345_01245 * invDet;
  2181. mat[3][3] = det5_01245_01245 * invDet;
  2182. mat[3][4] = -det5_01235_01245 * invDet;
  2183. mat[3][5] = det5_01234_01245 * invDet;
  2184. mat[4][0] = det5_12345_01235 * invDet;
  2185. mat[4][1] = -det5_02345_01235 * invDet;
  2186. mat[4][2] = det5_01345_01235 * invDet;
  2187. mat[4][3] = -det5_01245_01235 * invDet;
  2188. mat[4][4] = det5_01235_01235 * invDet;
  2189. mat[4][5] = -det5_01234_01235 * invDet;
  2190. mat[5][0] = -det5_12345_01234 * invDet;
  2191. mat[5][1] = det5_02345_01234 * invDet;
  2192. mat[5][2] = -det5_01345_01234 * invDet;
  2193. mat[5][3] = det5_01245_01234 * invDet;
  2194. mat[5][4] = -det5_01235_01234 * invDet;
  2195. mat[5][5] = det5_01234_01234 * invDet;
  2196. return true;
  2197. #elif 0
  2198. // 6*40 = 240 multiplications
  2199. // 6 divisions
  2200. float *mat = reinterpret_cast<float *>(this);
  2201. float s;
  2202. double d, di;
  2203. di = mat[0];
  2204. s = di;
  2205. mat[0] = d = 1.0f / di;
  2206. mat[1] *= d;
  2207. mat[2] *= d;
  2208. mat[3] *= d;
  2209. mat[4] *= d;
  2210. mat[5] *= d;
  2211. d = -d;
  2212. mat[6] *= d;
  2213. mat[12] *= d;
  2214. mat[18] *= d;
  2215. mat[24] *= d;
  2216. mat[30] *= d;
  2217. d = mat[6] * di;
  2218. mat[7] += mat[1] * d;
  2219. mat[8] += mat[2] * d;
  2220. mat[9] += mat[3] * d;
  2221. mat[10] += mat[4] * d;
  2222. mat[11] += mat[5] * d;
  2223. d = mat[12] * di;
  2224. mat[13] += mat[1] * d;
  2225. mat[14] += mat[2] * d;
  2226. mat[15] += mat[3] * d;
  2227. mat[16] += mat[4] * d;
  2228. mat[17] += mat[5] * d;
  2229. d = mat[18] * di;
  2230. mat[19] += mat[1] * d;
  2231. mat[20] += mat[2] * d;
  2232. mat[21] += mat[3] * d;
  2233. mat[22] += mat[4] * d;
  2234. mat[23] += mat[5] * d;
  2235. d = mat[24] * di;
  2236. mat[25] += mat[1] * d;
  2237. mat[26] += mat[2] * d;
  2238. mat[27] += mat[3] * d;
  2239. mat[28] += mat[4] * d;
  2240. mat[29] += mat[5] * d;
  2241. d = mat[30] * di;
  2242. mat[31] += mat[1] * d;
  2243. mat[32] += mat[2] * d;
  2244. mat[33] += mat[3] * d;
  2245. mat[34] += mat[4] * d;
  2246. mat[35] += mat[5] * d;
  2247. di = mat[7];
  2248. s *= di;
  2249. mat[7] = d = 1.0f / di;
  2250. mat[6] *= d;
  2251. mat[8] *= d;
  2252. mat[9] *= d;
  2253. mat[10] *= d;
  2254. mat[11] *= d;
  2255. d = -d;
  2256. mat[1] *= d;
  2257. mat[13] *= d;
  2258. mat[19] *= d;
  2259. mat[25] *= d;
  2260. mat[31] *= d;
  2261. d = mat[1] * di;
  2262. mat[0] += mat[6] * d;
  2263. mat[2] += mat[8] * d;
  2264. mat[3] += mat[9] * d;
  2265. mat[4] += mat[10] * d;
  2266. mat[5] += mat[11] * d;
  2267. d = mat[13] * di;
  2268. mat[12] += mat[6] * d;
  2269. mat[14] += mat[8] * d;
  2270. mat[15] += mat[9] * d;
  2271. mat[16] += mat[10] * d;
  2272. mat[17] += mat[11] * d;
  2273. d = mat[19] * di;
  2274. mat[18] += mat[6] * d;
  2275. mat[20] += mat[8] * d;
  2276. mat[21] += mat[9] * d;
  2277. mat[22] += mat[10] * d;
  2278. mat[23] += mat[11] * d;
  2279. d = mat[25] * di;
  2280. mat[24] += mat[6] * d;
  2281. mat[26] += mat[8] * d;
  2282. mat[27] += mat[9] * d;
  2283. mat[28] += mat[10] * d;
  2284. mat[29] += mat[11] * d;
  2285. d = mat[31] * di;
  2286. mat[30] += mat[6] * d;
  2287. mat[32] += mat[8] * d;
  2288. mat[33] += mat[9] * d;
  2289. mat[34] += mat[10] * d;
  2290. mat[35] += mat[11] * d;
  2291. di = mat[14];
  2292. s *= di;
  2293. mat[14] = d = 1.0f / di;
  2294. mat[12] *= d;
  2295. mat[13] *= d;
  2296. mat[15] *= d;
  2297. mat[16] *= d;
  2298. mat[17] *= d;
  2299. d = -d;
  2300. mat[2] *= d;
  2301. mat[8] *= d;
  2302. mat[20] *= d;
  2303. mat[26] *= d;
  2304. mat[32] *= d;
  2305. d = mat[2] * di;
  2306. mat[0] += mat[12] * d;
  2307. mat[1] += mat[13] * d;
  2308. mat[3] += mat[15] * d;
  2309. mat[4] += mat[16] * d;
  2310. mat[5] += mat[17] * d;
  2311. d = mat[8] * di;
  2312. mat[6] += mat[12] * d;
  2313. mat[7] += mat[13] * d;
  2314. mat[9] += mat[15] * d;
  2315. mat[10] += mat[16] * d;
  2316. mat[11] += mat[17] * d;
  2317. d = mat[20] * di;
  2318. mat[18] += mat[12] * d;
  2319. mat[19] += mat[13] * d;
  2320. mat[21] += mat[15] * d;
  2321. mat[22] += mat[16] * d;
  2322. mat[23] += mat[17] * d;
  2323. d = mat[26] * di;
  2324. mat[24] += mat[12] * d;
  2325. mat[25] += mat[13] * d;
  2326. mat[27] += mat[15] * d;
  2327. mat[28] += mat[16] * d;
  2328. mat[29] += mat[17] * d;
  2329. d = mat[32] * di;
  2330. mat[30] += mat[12] * d;
  2331. mat[31] += mat[13] * d;
  2332. mat[33] += mat[15] * d;
  2333. mat[34] += mat[16] * d;
  2334. mat[35] += mat[17] * d;
  2335. di = mat[21];
  2336. s *= di;
  2337. mat[21] = d = 1.0f / di;
  2338. mat[18] *= d;
  2339. mat[19] *= d;
  2340. mat[20] *= d;
  2341. mat[22] *= d;
  2342. mat[23] *= d;
  2343. d = -d;
  2344. mat[3] *= d;
  2345. mat[9] *= d;
  2346. mat[15] *= d;
  2347. mat[27] *= d;
  2348. mat[33] *= d;
  2349. d = mat[3] * di;
  2350. mat[0] += mat[18] * d;
  2351. mat[1] += mat[19] * d;
  2352. mat[2] += mat[20] * d;
  2353. mat[4] += mat[22] * d;
  2354. mat[5] += mat[23] * d;
  2355. d = mat[9] * di;
  2356. mat[6] += mat[18] * d;
  2357. mat[7] += mat[19] * d;
  2358. mat[8] += mat[20] * d;
  2359. mat[10] += mat[22] * d;
  2360. mat[11] += mat[23] * d;
  2361. d = mat[15] * di;
  2362. mat[12] += mat[18] * d;
  2363. mat[13] += mat[19] * d;
  2364. mat[14] += mat[20] * d;
  2365. mat[16] += mat[22] * d;
  2366. mat[17] += mat[23] * d;
  2367. d = mat[27] * di;
  2368. mat[24] += mat[18] * d;
  2369. mat[25] += mat[19] * d;
  2370. mat[26] += mat[20] * d;
  2371. mat[28] += mat[22] * d;
  2372. mat[29] += mat[23] * d;
  2373. d = mat[33] * di;
  2374. mat[30] += mat[18] * d;
  2375. mat[31] += mat[19] * d;
  2376. mat[32] += mat[20] * d;
  2377. mat[34] += mat[22] * d;
  2378. mat[35] += mat[23] * d;
  2379. di = mat[28];
  2380. s *= di;
  2381. mat[28] = d = 1.0f / di;
  2382. mat[24] *= d;
  2383. mat[25] *= d;
  2384. mat[26] *= d;
  2385. mat[27] *= d;
  2386. mat[29] *= d;
  2387. d = -d;
  2388. mat[4] *= d;
  2389. mat[10] *= d;
  2390. mat[16] *= d;
  2391. mat[22] *= d;
  2392. mat[34] *= d;
  2393. d = mat[4] * di;
  2394. mat[0] += mat[24] * d;
  2395. mat[1] += mat[25] * d;
  2396. mat[2] += mat[26] * d;
  2397. mat[3] += mat[27] * d;
  2398. mat[5] += mat[29] * d;
  2399. d = mat[10] * di;
  2400. mat[6] += mat[24] * d;
  2401. mat[7] += mat[25] * d;
  2402. mat[8] += mat[26] * d;
  2403. mat[9] += mat[27] * d;
  2404. mat[11] += mat[29] * d;
  2405. d = mat[16] * di;
  2406. mat[12] += mat[24] * d;
  2407. mat[13] += mat[25] * d;
  2408. mat[14] += mat[26] * d;
  2409. mat[15] += mat[27] * d;
  2410. mat[17] += mat[29] * d;
  2411. d = mat[22] * di;
  2412. mat[18] += mat[24] * d;
  2413. mat[19] += mat[25] * d;
  2414. mat[20] += mat[26] * d;
  2415. mat[21] += mat[27] * d;
  2416. mat[23] += mat[29] * d;
  2417. d = mat[34] * di;
  2418. mat[30] += mat[24] * d;
  2419. mat[31] += mat[25] * d;
  2420. mat[32] += mat[26] * d;
  2421. mat[33] += mat[27] * d;
  2422. mat[35] += mat[29] * d;
  2423. di = mat[35];
  2424. s *= di;
  2425. mat[35] = d = 1.0f / di;
  2426. mat[30] *= d;
  2427. mat[31] *= d;
  2428. mat[32] *= d;
  2429. mat[33] *= d;
  2430. mat[34] *= d;
  2431. d = -d;
  2432. mat[5] *= d;
  2433. mat[11] *= d;
  2434. mat[17] *= d;
  2435. mat[23] *= d;
  2436. mat[29] *= d;
  2437. d = mat[5] * di;
  2438. mat[0] += mat[30] * d;
  2439. mat[1] += mat[31] * d;
  2440. mat[2] += mat[32] * d;
  2441. mat[3] += mat[33] * d;
  2442. mat[4] += mat[34] * d;
  2443. d = mat[11] * di;
  2444. mat[6] += mat[30] * d;
  2445. mat[7] += mat[31] * d;
  2446. mat[8] += mat[32] * d;
  2447. mat[9] += mat[33] * d;
  2448. mat[10] += mat[34] * d;
  2449. d = mat[17] * di;
  2450. mat[12] += mat[30] * d;
  2451. mat[13] += mat[31] * d;
  2452. mat[14] += mat[32] * d;
  2453. mat[15] += mat[33] * d;
  2454. mat[16] += mat[34] * d;
  2455. d = mat[23] * di;
  2456. mat[18] += mat[30] * d;
  2457. mat[19] += mat[31] * d;
  2458. mat[20] += mat[32] * d;
  2459. mat[21] += mat[33] * d;
  2460. mat[22] += mat[34] * d;
  2461. d = mat[29] * di;
  2462. mat[24] += mat[30] * d;
  2463. mat[25] += mat[31] * d;
  2464. mat[26] += mat[32] * d;
  2465. mat[27] += mat[33] * d;
  2466. mat[28] += mat[34] * d;
  2467. return ( s != 0.0f && !IEEE_FLT_IS_NAN( s ) );
  2468. #else
  2469. // 6*27+2*30 = 222 multiplications
  2470. // 2*1 = 2 divisions
  2471. idMat3 r0, r1, r2, r3;
  2472. float c0, c1, c2, det, invDet;
  2473. float *mat = reinterpret_cast<float *>(this);
  2474. // r0 = m0.Inverse();
  2475. c0 = mat[1*6+1] * mat[2*6+2] - mat[1*6+2] * mat[2*6+1];
  2476. c1 = mat[1*6+2] * mat[2*6+0] - mat[1*6+0] * mat[2*6+2];
  2477. c2 = mat[1*6+0] * mat[2*6+1] - mat[1*6+1] * mat[2*6+0];
  2478. det = mat[0*6+0] * c0 + mat[0*6+1] * c1 + mat[0*6+2] * c2;
  2479. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  2480. return false;
  2481. }
  2482. invDet = 1.0f / det;
  2483. r0[0][0] = c0 * invDet;
  2484. r0[0][1] = ( mat[0*6+2] * mat[2*6+1] - mat[0*6+1] * mat[2*6+2] ) * invDet;
  2485. r0[0][2] = ( mat[0*6+1] * mat[1*6+2] - mat[0*6+2] * mat[1*6+1] ) * invDet;
  2486. r0[1][0] = c1 * invDet;
  2487. r0[1][1] = ( mat[0*6+0] * mat[2*6+2] - mat[0*6+2] * mat[2*6+0] ) * invDet;
  2488. r0[1][2] = ( mat[0*6+2] * mat[1*6+0] - mat[0*6+0] * mat[1*6+2] ) * invDet;
  2489. r0[2][0] = c2 * invDet;
  2490. r0[2][1] = ( mat[0*6+1] * mat[2*6+0] - mat[0*6+0] * mat[2*6+1] ) * invDet;
  2491. r0[2][2] = ( mat[0*6+0] * mat[1*6+1] - mat[0*6+1] * mat[1*6+0] ) * invDet;
  2492. // r1 = r0 * m1;
  2493. r1[0][0] = r0[0][0] * mat[0*6+3] + r0[0][1] * mat[1*6+3] + r0[0][2] * mat[2*6+3];
  2494. r1[0][1] = r0[0][0] * mat[0*6+4] + r0[0][1] * mat[1*6+4] + r0[0][2] * mat[2*6+4];
  2495. r1[0][2] = r0[0][0] * mat[0*6+5] + r0[0][1] * mat[1*6+5] + r0[0][2] * mat[2*6+5];
  2496. r1[1][0] = r0[1][0] * mat[0*6+3] + r0[1][1] * mat[1*6+3] + r0[1][2] * mat[2*6+3];
  2497. r1[1][1] = r0[1][0] * mat[0*6+4] + r0[1][1] * mat[1*6+4] + r0[1][2] * mat[2*6+4];
  2498. r1[1][2] = r0[1][0] * mat[0*6+5] + r0[1][1] * mat[1*6+5] + r0[1][2] * mat[2*6+5];
  2499. r1[2][0] = r0[2][0] * mat[0*6+3] + r0[2][1] * mat[1*6+3] + r0[2][2] * mat[2*6+3];
  2500. r1[2][1] = r0[2][0] * mat[0*6+4] + r0[2][1] * mat[1*6+4] + r0[2][2] * mat[2*6+4];
  2501. r1[2][2] = r0[2][0] * mat[0*6+5] + r0[2][1] * mat[1*6+5] + r0[2][2] * mat[2*6+5];
  2502. // r2 = m2 * r1;
  2503. r2[0][0] = mat[3*6+0] * r1[0][0] + mat[3*6+1] * r1[1][0] + mat[3*6+2] * r1[2][0];
  2504. r2[0][1] = mat[3*6+0] * r1[0][1] + mat[3*6+1] * r1[1][1] + mat[3*6+2] * r1[2][1];
  2505. r2[0][2] = mat[3*6+0] * r1[0][2] + mat[3*6+1] * r1[1][2] + mat[3*6+2] * r1[2][2];
  2506. r2[1][0] = mat[4*6+0] * r1[0][0] + mat[4*6+1] * r1[1][0] + mat[4*6+2] * r1[2][0];
  2507. r2[1][1] = mat[4*6+0] * r1[0][1] + mat[4*6+1] * r1[1][1] + mat[4*6+2] * r1[2][1];
  2508. r2[1][2] = mat[4*6+0] * r1[0][2] + mat[4*6+1] * r1[1][2] + mat[4*6+2] * r1[2][2];
  2509. r2[2][0] = mat[5*6+0] * r1[0][0] + mat[5*6+1] * r1[1][0] + mat[5*6+2] * r1[2][0];
  2510. r2[2][1] = mat[5*6+0] * r1[0][1] + mat[5*6+1] * r1[1][1] + mat[5*6+2] * r1[2][1];
  2511. r2[2][2] = mat[5*6+0] * r1[0][2] + mat[5*6+1] * r1[1][2] + mat[5*6+2] * r1[2][2];
  2512. // r3 = r2 - m3;
  2513. r3[0][0] = r2[0][0] - mat[3*6+3];
  2514. r3[0][1] = r2[0][1] - mat[3*6+4];
  2515. r3[0][2] = r2[0][2] - mat[3*6+5];
  2516. r3[1][0] = r2[1][0] - mat[4*6+3];
  2517. r3[1][1] = r2[1][1] - mat[4*6+4];
  2518. r3[1][2] = r2[1][2] - mat[4*6+5];
  2519. r3[2][0] = r2[2][0] - mat[5*6+3];
  2520. r3[2][1] = r2[2][1] - mat[5*6+4];
  2521. r3[2][2] = r2[2][2] - mat[5*6+5];
  2522. // r3.InverseSelf();
  2523. r2[0][0] = r3[1][1] * r3[2][2] - r3[1][2] * r3[2][1];
  2524. r2[1][0] = r3[1][2] * r3[2][0] - r3[1][0] * r3[2][2];
  2525. r2[2][0] = r3[1][0] * r3[2][1] - r3[1][1] * r3[2][0];
  2526. det = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0] + r3[0][2] * r2[2][0];
  2527. if ( idMath::Fabs( det ) < MATRIX_INVERSE_EPSILON ) {
  2528. return false;
  2529. }
  2530. invDet = 1.0f / det;
  2531. r2[0][1] = r3[0][2] * r3[2][1] - r3[0][1] * r3[2][2];
  2532. r2[0][2] = r3[0][1] * r3[1][2] - r3[0][2] * r3[1][1];
  2533. r2[1][1] = r3[0][0] * r3[2][2] - r3[0][2] * r3[2][0];
  2534. r2[1][2] = r3[0][2] * r3[1][0] - r3[0][0] * r3[1][2];
  2535. r2[2][1] = r3[0][1] * r3[2][0] - r3[0][0] * r3[2][1];
  2536. r2[2][2] = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];
  2537. r3[0][0] = r2[0][0] * invDet;
  2538. r3[0][1] = r2[0][1] * invDet;
  2539. r3[0][2] = r2[0][2] * invDet;
  2540. r3[1][0] = r2[1][0] * invDet;
  2541. r3[1][1] = r2[1][1] * invDet;
  2542. r3[1][2] = r2[1][2] * invDet;
  2543. r3[2][0] = r2[2][0] * invDet;
  2544. r3[2][1] = r2[2][1] * invDet;
  2545. r3[2][2] = r2[2][2] * invDet;
  2546. // r2 = m2 * r0;
  2547. r2[0][0] = mat[3*6+0] * r0[0][0] + mat[3*6+1] * r0[1][0] + mat[3*6+2] * r0[2][0];
  2548. r2[0][1] = mat[3*6+0] * r0[0][1] + mat[3*6+1] * r0[1][1] + mat[3*6+2] * r0[2][1];
  2549. r2[0][2] = mat[3*6+0] * r0[0][2] + mat[3*6+1] * r0[1][2] + mat[3*6+2] * r0[2][2];
  2550. r2[1][0] = mat[4*6+0] * r0[0][0] + mat[4*6+1] * r0[1][0] + mat[4*6+2] * r0[2][0];
  2551. r2[1][1] = mat[4*6+0] * r0[0][1] + mat[4*6+1] * r0[1][1] + mat[4*6+2] * r0[2][1];
  2552. r2[1][2] = mat[4*6+0] * r0[0][2] + mat[4*6+1] * r0[1][2] + mat[4*6+2] * r0[2][2];
  2553. r2[2][0] = mat[5*6+0] * r0[0][0] + mat[5*6+1] * r0[1][0] + mat[5*6+2] * r0[2][0];
  2554. r2[2][1] = mat[5*6+0] * r0[0][1] + mat[5*6+1] * r0[1][1] + mat[5*6+2] * r0[2][1];
  2555. r2[2][2] = mat[5*6+0] * r0[0][2] + mat[5*6+1] * r0[1][2] + mat[5*6+2] * r0[2][2];
  2556. // m2 = r3 * r2;
  2557. mat[3*6+0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0] + r3[0][2] * r2[2][0];
  2558. mat[3*6+1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1] + r3[0][2] * r2[2][1];
  2559. mat[3*6+2] = r3[0][0] * r2[0][2] + r3[0][1] * r2[1][2] + r3[0][2] * r2[2][2];
  2560. mat[4*6+0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0] + r3[1][2] * r2[2][0];
  2561. mat[4*6+1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1] + r3[1][2] * r2[2][1];
  2562. mat[4*6+2] = r3[1][0] * r2[0][2] + r3[1][1] * r2[1][2] + r3[1][2] * r2[2][2];
  2563. mat[5*6+0] = r3[2][0] * r2[0][0] + r3[2][1] * r2[1][0] + r3[2][2] * r2[2][0];
  2564. mat[5*6+1] = r3[2][0] * r2[0][1] + r3[2][1] * r2[1][1] + r3[2][2] * r2[2][1];
  2565. mat[5*6+2] = r3[2][0] * r2[0][2] + r3[2][1] * r2[1][2] + r3[2][2] * r2[2][2];
  2566. // m0 = r0 - r1 * m2;
  2567. mat[0*6+0] = r0[0][0] - r1[0][0] * mat[3*6+0] - r1[0][1] * mat[4*6+0] - r1[0][2] * mat[5*6+0];
  2568. mat[0*6+1] = r0[0][1] - r1[0][0] * mat[3*6+1] - r1[0][1] * mat[4*6+1] - r1[0][2] * mat[5*6+1];
  2569. mat[0*6+2] = r0[0][2] - r1[0][0] * mat[3*6+2] - r1[0][1] * mat[4*6+2] - r1[0][2] * mat[5*6+2];
  2570. mat[1*6+0] = r0[1][0] - r1[1][0] * mat[3*6+0] - r1[1][1] * mat[4*6+0] - r1[1][2] * mat[5*6+0];
  2571. mat[1*6+1] = r0[1][1] - r1[1][0] * mat[3*6+1] - r1[1][1] * mat[4*6+1] - r1[1][2] * mat[5*6+1];
  2572. mat[1*6+2] = r0[1][2] - r1[1][0] * mat[3*6+2] - r1[1][1] * mat[4*6+2] - r1[1][2] * mat[5*6+2];
  2573. mat[2*6+0] = r0[2][0] - r1[2][0] * mat[3*6+0] - r1[2][1] * mat[4*6+0] - r1[2][2] * mat[5*6+0];
  2574. mat[2*6+1] = r0[2][1] - r1[2][0] * mat[3*6+1] - r1[2][1] * mat[4*6+1] - r1[2][2] * mat[5*6+1];
  2575. mat[2*6+2] = r0[2][2] - r1[2][0] * mat[3*6+2] - r1[2][1] * mat[4*6+2] - r1[2][2] * mat[5*6+2];
  2576. // m1 = r1 * r3;
  2577. mat[0*6+3] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0] + r1[0][2] * r3[2][0];
  2578. mat[0*6+4] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1] + r1[0][2] * r3[2][1];
  2579. mat[0*6+5] = r1[0][0] * r3[0][2] + r1[0][1] * r3[1][2] + r1[0][2] * r3[2][2];
  2580. mat[1*6+3] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0] + r1[1][2] * r3[2][0];
  2581. mat[1*6+4] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1] + r1[1][2] * r3[2][1];
  2582. mat[1*6+5] = r1[1][0] * r3[0][2] + r1[1][1] * r3[1][2] + r1[1][2] * r3[2][2];
  2583. mat[2*6+3] = r1[2][0] * r3[0][0] + r1[2][1] * r3[1][0] + r1[2][2] * r3[2][0];
  2584. mat[2*6+4] = r1[2][0] * r3[0][1] + r1[2][1] * r3[1][1] + r1[2][2] * r3[2][1];
  2585. mat[2*6+5] = r1[2][0] * r3[0][2] + r1[2][1] * r3[1][2] + r1[2][2] * r3[2][2];
  2586. // m3 = -r3;
  2587. mat[3*6+3] = -r3[0][0];
  2588. mat[3*6+4] = -r3[0][1];
  2589. mat[3*6+5] = -r3[0][2];
  2590. mat[4*6+3] = -r3[1][0];
  2591. mat[4*6+4] = -r3[1][1];
  2592. mat[4*6+5] = -r3[1][2];
  2593. mat[5*6+3] = -r3[2][0];
  2594. mat[5*6+4] = -r3[2][1];
  2595. mat[5*6+5] = -r3[2][2];
  2596. return true;
  2597. #endif
  2598. }
  2599. /*
  2600. =============
  2601. idMat6::ToString
  2602. =============
  2603. */
  2604. const char *idMat6::ToString( int precision ) const {
  2605. return idStr::FloatArrayToString( ToFloatPtr(), GetDimension(), precision );
  2606. }