id/doom3-bfg
0
0
Fork 0
mirror of https://github.com/id-Software/DOOM-3-BFG.git synced 2024-11-28 23:02:14 +00:00
Code Issues Projects Releases Packages Wiki Activity
doom3-bfg/neo/idlib/math/Simd_SSE.cpp

935 lines
38 KiB
C++
Raw Normal View History

Initial commit
2012-11-26 18:58:24 +00:00
/*
===========================================================================
Doom 3 BFG Edition GPL Source Code
Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company.
This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").
Doom 3 BFG Edition Source Code is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
Doom 3 BFG Edition Source Code is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with Doom 3 BFG Edition Source Code. If not, see <http://www.gnu.org/licenses/>.
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.
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.
===========================================================================
*/
#pragma hdrstop
#include "../precompiled.h"
#include "Simd_Generic.h"
#include "Simd_SSE.h"
//===============================================================
// M
// SSE implementation of idSIMDProcessor MrE
// E
//===============================================================
#include <xmmintrin.h>
#define M_PI 3.14159265358979323846f
/*
============
idSIMD_SSE::GetName
============
*/
const char * idSIMD_SSE::GetName() const {
return "MMX & SSE";
}
/*
============
idSIMD_SSE::BlendJoints
============
*/
void VPCALL idSIMD_SSE::BlendJoints( idJointQuat *joints, const idJointQuat *blendJoints, const float lerp, const int *index, const int numJoints ) {
if ( lerp <= 0.0f ) {
return;
} else if ( lerp >= 1.0f ) {
for ( int i = 0; i < numJoints; i++ ) {
int j = index[i];
joints[j] = blendJoints[j];
}
return;
}
const __m128 vlerp = { lerp, lerp, lerp, lerp };
const __m128 vector_float_one = { 1.0f, 1.0f, 1.0f, 1.0f };
const __m128 vector_float_sign_bit = __m128c( _mm_set_epi32( 0x80000000, 0x80000000, 0x80000000, 0x80000000 ) );
const __m128 vector_float_rsqrt_c0 = { -3.0f, -3.0f, -3.0f, -3.0f };
const __m128 vector_float_rsqrt_c1 = { -0.5f, -0.5f, -0.5f, -0.5f };
const __m128 vector_float_tiny = { 1e-10f, 1e-10f, 1e-10f, 1e-10f };
const __m128 vector_float_half_pi = { M_PI*0.5f, M_PI*0.5f, M_PI*0.5f, M_PI*0.5f };
const __m128 vector_float_sin_c0 = { -2.39e-08f, -2.39e-08f, -2.39e-08f, -2.39e-08f };
const __m128 vector_float_sin_c1 = { 2.7526e-06f, 2.7526e-06f, 2.7526e-06f, 2.7526e-06f };
const __m128 vector_float_sin_c2 = { -1.98409e-04f, -1.98409e-04f, -1.98409e-04f, -1.98409e-04f };
const __m128 vector_float_sin_c3 = { 8.3333315e-03f, 8.3333315e-03f, 8.3333315e-03f, 8.3333315e-03f };
const __m128 vector_float_sin_c4 = { -1.666666664e-01f, -1.666666664e-01f, -1.666666664e-01f, -1.666666664e-01f };
const __m128 vector_float_atan_c0 = { 0.0028662257f, 0.0028662257f, 0.0028662257f, 0.0028662257f };
const __m128 vector_float_atan_c1 = { -0.0161657367f, -0.0161657367f, -0.0161657367f, -0.0161657367f };
const __m128 vector_float_atan_c2 = { 0.0429096138f, 0.0429096138f, 0.0429096138f, 0.0429096138f };
const __m128 vector_float_atan_c3 = { -0.0752896400f, -0.0752896400f, -0.0752896400f, -0.0752896400f };
const __m128 vector_float_atan_c4 = { 0.1065626393f, 0.1065626393f, 0.1065626393f, 0.1065626393f };
const __m128 vector_float_atan_c5 = { -0.1420889944f, -0.1420889944f, -0.1420889944f, -0.1420889944f };
const __m128 vector_float_atan_c6 = { 0.1999355085f, 0.1999355085f, 0.1999355085f, 0.1999355085f };
const __m128 vector_float_atan_c7 = { -0.3333314528f, -0.3333314528f, -0.3333314528f, -0.3333314528f };
int i = 0;
for ( ; i < numJoints - 3; i += 4 ) {
const int n0 = index[i+0];
const int n1 = index[i+1];
const int n2 = index[i+2];
const int n3 = index[i+3];
__m128 jqa_0 = _mm_load_ps( joints[n0].q.ToFloatPtr() );
__m128 jqb_0 = _mm_load_ps( joints[n1].q.ToFloatPtr() );
__m128 jqc_0 = _mm_load_ps( joints[n2].q.ToFloatPtr() );
__m128 jqd_0 = _mm_load_ps( joints[n3].q.ToFloatPtr() );
__m128 jta_0 = _mm_load_ps( joints[n0].t.ToFloatPtr() );
__m128 jtb_0 = _mm_load_ps( joints[n1].t.ToFloatPtr() );
__m128 jtc_0 = _mm_load_ps( joints[n2].t.ToFloatPtr() );
__m128 jtd_0 = _mm_load_ps( joints[n3].t.ToFloatPtr() );
__m128 bqa_0 = _mm_load_ps( blendJoints[n0].q.ToFloatPtr() );
__m128 bqb_0 = _mm_load_ps( blendJoints[n1].q.ToFloatPtr() );
__m128 bqc_0 = _mm_load_ps( blendJoints[n2].q.ToFloatPtr() );
__m128 bqd_0 = _mm_load_ps( blendJoints[n3].q.ToFloatPtr() );
__m128 bta_0 = _mm_load_ps( blendJoints[n0].t.ToFloatPtr() );
__m128 btb_0 = _mm_load_ps( blendJoints[n1].t.ToFloatPtr() );
__m128 btc_0 = _mm_load_ps( blendJoints[n2].t.ToFloatPtr() );
__m128 btd_0 = _mm_load_ps( blendJoints[n3].t.ToFloatPtr() );
bta_0 = _mm_sub_ps( bta_0, jta_0 );
btb_0 = _mm_sub_ps( btb_0, jtb_0 );
btc_0 = _mm_sub_ps( btc_0, jtc_0 );
btd_0 = _mm_sub_ps( btd_0, jtd_0 );
jta_0 = _mm_madd_ps( vlerp, bta_0, jta_0 );
jtb_0 = _mm_madd_ps( vlerp, btb_0, jtb_0 );
jtc_0 = _mm_madd_ps( vlerp, btc_0, jtc_0 );
jtd_0 = _mm_madd_ps( vlerp, btd_0, jtd_0 );
_mm_store_ps( joints[n0].t.ToFloatPtr(), jta_0 );
_mm_store_ps( joints[n1].t.ToFloatPtr(), jtb_0 );
_mm_store_ps( joints[n2].t.ToFloatPtr(), jtc_0 );
_mm_store_ps( joints[n3].t.ToFloatPtr(), jtd_0 );
__m128 jqr_0 = _mm_unpacklo_ps( jqa_0, jqc_0 );
__m128 jqs_0 = _mm_unpackhi_ps( jqa_0, jqc_0 );
__m128 jqt_0 = _mm_unpacklo_ps( jqb_0, jqd_0 );
__m128 jqu_0 = _mm_unpackhi_ps( jqb_0, jqd_0 );
__m128 bqr_0 = _mm_unpacklo_ps( bqa_0, bqc_0 );
__m128 bqs_0 = _mm_unpackhi_ps( bqa_0, bqc_0 );
__m128 bqt_0 = _mm_unpacklo_ps( bqb_0, bqd_0 );
__m128 bqu_0 = _mm_unpackhi_ps( bqb_0, bqd_0 );
__m128 jqx_0 = _mm_unpacklo_ps( jqr_0, jqt_0 );
__m128 jqy_0 = _mm_unpackhi_ps( jqr_0, jqt_0 );
__m128 jqz_0 = _mm_unpacklo_ps( jqs_0, jqu_0 );
__m128 jqw_0 = _mm_unpackhi_ps( jqs_0, jqu_0 );
__m128 bqx_0 = _mm_unpacklo_ps( bqr_0, bqt_0 );
__m128 bqy_0 = _mm_unpackhi_ps( bqr_0, bqt_0 );
__m128 bqz_0 = _mm_unpacklo_ps( bqs_0, bqu_0 );
__m128 bqw_0 = _mm_unpackhi_ps( bqs_0, bqu_0 );
__m128 cosoma_0 = _mm_mul_ps( jqx_0, bqx_0 );
__m128 cosomb_0 = _mm_mul_ps( jqy_0, bqy_0 );
__m128 cosomc_0 = _mm_mul_ps( jqz_0, bqz_0 );
__m128 cosomd_0 = _mm_mul_ps( jqw_0, bqw_0 );
__m128 cosome_0 = _mm_add_ps( cosoma_0, cosomb_0 );
__m128 cosomf_0 = _mm_add_ps( cosomc_0, cosomd_0 );
__m128 cosomg_0 = _mm_add_ps( cosome_0, cosomf_0 );
__m128 sign_0 = _mm_and_ps( cosomg_0, vector_float_sign_bit );
__m128 cosom_0 = _mm_xor_ps( cosomg_0, sign_0 );
__m128 ss_0 = _mm_nmsub_ps( cosom_0, cosom_0, vector_float_one );
ss_0 = _mm_max_ps( ss_0, vector_float_tiny );
__m128 rs_0 = _mm_rsqrt_ps( ss_0 );
__m128 sq_0 = _mm_mul_ps( rs_0, rs_0 );
__m128 sh_0 = _mm_mul_ps( rs_0, vector_float_rsqrt_c1 );
__m128 sx_0 = _mm_madd_ps( ss_0, sq_0, vector_float_rsqrt_c0 );
__m128 sinom_0 = _mm_mul_ps( sh_0, sx_0 ); // sinom = sqrt( ss );
ss_0 = _mm_mul_ps( ss_0, sinom_0 );
__m128 min_0 = _mm_min_ps( ss_0, cosom_0 );
__m128 max_0 = _mm_max_ps( ss_0, cosom_0 );
__m128 mask_0 = _mm_cmpeq_ps( min_0, cosom_0 );
__m128 masksign_0 = _mm_and_ps( mask_0, vector_float_sign_bit );
__m128 maskPI_0 = _mm_and_ps( mask_0, vector_float_half_pi );
__m128 rcpa_0 = _mm_rcp_ps( max_0 );
__m128 rcpb_0 = _mm_mul_ps( max_0, rcpa_0 );
__m128 rcpd_0 = _mm_add_ps( rcpa_0, rcpa_0 );
__m128 rcp_0 = _mm_nmsub_ps( rcpb_0, rcpa_0, rcpd_0 ); // 1 / y or 1 / x
__m128 ata_0 = _mm_mul_ps( min_0, rcp_0 ); // x / y or y / x
__m128 atb_0 = _mm_xor_ps( ata_0, masksign_0 ); // -x / y or y / x
__m128 atc_0 = _mm_mul_ps( atb_0, atb_0 );
__m128 atd_0 = _mm_madd_ps( atc_0, vector_float_atan_c0, vector_float_atan_c1 );
atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_atan_c2 );
atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_atan_c3 );
atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_atan_c4 );
atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_atan_c5 );
atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_atan_c6 );
atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_atan_c7 );
atd_0 = _mm_madd_ps( atd_0, atc_0, vector_float_one );
__m128 omega_a_0 = _mm_madd_ps( atd_0, atb_0, maskPI_0 );
__m128 omega_b_0 = _mm_mul_ps( vlerp, omega_a_0 );
omega_a_0 = _mm_sub_ps( omega_a_0, omega_b_0 );
__m128 sinsa_0 = _mm_mul_ps( omega_a_0, omega_a_0 );
__m128 sinsb_0 = _mm_mul_ps( omega_b_0, omega_b_0 );
__m128 sina_0 = _mm_madd_ps( sinsa_0, vector_float_sin_c0, vector_float_sin_c1 );
__m128 sinb_0 = _mm_madd_ps( sinsb_0, vector_float_sin_c0, vector_float_sin_c1 );
sina_0 = _mm_madd_ps( sina_0, sinsa_0, vector_float_sin_c2 );
sinb_0 = _mm_madd_ps( sinb_0, sinsb_0, vector_float_sin_c2 );
sina_0 = _mm_madd_ps( sina_0, sinsa_0, vector_float_sin_c3 );
sinb_0 = _mm_madd_ps( sinb_0, sinsb_0, vector_float_sin_c3 );
sina_0 = _mm_madd_ps( sina_0, sinsa_0, vector_float_sin_c4 );
sinb_0 = _mm_madd_ps( sinb_0, sinsb_0, vector_float_sin_c4 );
sina_0 = _mm_madd_ps( sina_0, sinsa_0, vector_float_one );
sinb_0 = _mm_madd_ps( sinb_0, sinsb_0, vector_float_one );
sina_0 = _mm_mul_ps( sina_0, omega_a_0 );
sinb_0 = _mm_mul_ps( sinb_0, omega_b_0 );
__m128 scalea_0 = _mm_mul_ps( sina_0, sinom_0 );
__m128 scaleb_0 = _mm_mul_ps( sinb_0, sinom_0 );
scaleb_0 = _mm_xor_ps( scaleb_0, sign_0 );
jqx_0 = _mm_mul_ps( jqx_0, scalea_0 );
jqy_0 = _mm_mul_ps( jqy_0, scalea_0 );
jqz_0 = _mm_mul_ps( jqz_0, scalea_0 );
jqw_0 = _mm_mul_ps( jqw_0, scalea_0 );
jqx_0 = _mm_madd_ps( bqx_0, scaleb_0, jqx_0 );
jqy_0 = _mm_madd_ps( bqy_0, scaleb_0, jqy_0 );
jqz_0 = _mm_madd_ps( bqz_0, scaleb_0, jqz_0 );
jqw_0 = _mm_madd_ps( bqw_0, scaleb_0, jqw_0 );
__m128 tp0_0 = _mm_unpacklo_ps( jqx_0, jqz_0 );
__m128 tp1_0 = _mm_unpackhi_ps( jqx_0, jqz_0 );
__m128 tp2_0 = _mm_unpacklo_ps( jqy_0, jqw_0 );
__m128 tp3_0 = _mm_unpackhi_ps( jqy_0, jqw_0 );
__m128 p0_0 = _mm_unpacklo_ps( tp0_0, tp2_0 );
__m128 p1_0 = _mm_unpackhi_ps( tp0_0, tp2_0 );
__m128 p2_0 = _mm_unpacklo_ps( tp1_0, tp3_0 );
__m128 p3_0 = _mm_unpackhi_ps( tp1_0, tp3_0 );
_mm_store_ps( joints[n0].q.ToFloatPtr(), p0_0 );
_mm_store_ps( joints[n1].q.ToFloatPtr(), p1_0 );
_mm_store_ps( joints[n2].q.ToFloatPtr(), p2_0 );
_mm_store_ps( joints[n3].q.ToFloatPtr(), p3_0 );
}
for ( ; i < numJoints; i++ ) {
int n = index[i];
idVec3 &jointVert = joints[n].t;
const idVec3 &blendVert = blendJoints[n].t;
jointVert[0] += lerp * ( blendVert[0] - jointVert[0] );
jointVert[1] += lerp * ( blendVert[1] - jointVert[1] );
jointVert[2] += lerp * ( blendVert[2] - jointVert[2] );
joints[n].w = 0.0f;
idQuat &jointQuat = joints[n].q;
const idQuat &blendQuat = blendJoints[n].q;
float cosom;
float sinom;
float omega;
float scale0;
float scale1;
unsigned long signBit;
cosom = jointQuat.x * blendQuat.x + jointQuat.y * blendQuat.y + jointQuat.z * blendQuat.z + jointQuat.w * blendQuat.w;
signBit = (*(unsigned long *)&cosom) & ( 1 << 31 );
(*(unsigned long *)&cosom) ^= signBit;
scale0 = 1.0f - cosom * cosom;
scale0 = ( scale0 <= 0.0f ) ? 1e-10f : scale0;
sinom = idMath::InvSqrt( scale0 );
omega = idMath::ATan16( scale0 * sinom, cosom );
scale0 = idMath::Sin16( ( 1.0f - lerp ) * omega ) * sinom;
scale1 = idMath::Sin16( lerp * omega ) * sinom;
(*(unsigned long *)&scale1) ^= signBit;
jointQuat.x = scale0 * jointQuat.x + scale1 * blendQuat.x;
jointQuat.y = scale0 * jointQuat.y + scale1 * blendQuat.y;
jointQuat.z = scale0 * jointQuat.z + scale1 * blendQuat.z;
jointQuat.w = scale0 * jointQuat.w + scale1 * blendQuat.w;
}
}
/*
============
idSIMD_SSE::BlendJointsFast
============
*/
void VPCALL idSIMD_SSE::BlendJointsFast( idJointQuat *joints, const idJointQuat *blendJoints, const float lerp, const int *index, const int numJoints ) {
assert_16_byte_aligned( joints );
assert_16_byte_aligned( blendJoints );
assert_16_byte_aligned( JOINTQUAT_Q_OFFSET );
assert_16_byte_aligned( JOINTQUAT_T_OFFSET );
assert_sizeof_16_byte_multiple( idJointQuat );
if ( lerp <= 0.0f ) {
return;
} else if ( lerp >= 1.0f ) {
for ( int i = 0; i < numJoints; i++ ) {
int j = index[i];
joints[j] = blendJoints[j];
}
return;
}
const __m128 vector_float_sign_bit = __m128c( _mm_set_epi32( 0x80000000, 0x80000000, 0x80000000, 0x80000000 ) );
const __m128 vector_float_rsqrt_c0 = { -3.0f, -3.0f, -3.0f, -3.0f };
const __m128 vector_float_rsqrt_c1 = { -0.5f, -0.5f, -0.5f, -0.5f };
const float scaledLerp = lerp / ( 1.0f - lerp );
const __m128 vlerp = { lerp, lerp, lerp, lerp };
const __m128 vscaledLerp = { scaledLerp, scaledLerp, scaledLerp, scaledLerp };
int i = 0;
for ( ; i < numJoints - 3; i += 4 ) {
const int n0 = index[i+0];
const int n1 = index[i+1];
const int n2 = index[i+2];
const int n3 = index[i+3];
__m128 jqa_0 = _mm_load_ps( joints[n0].q.ToFloatPtr() );
__m128 jqb_0 = _mm_load_ps( joints[n1].q.ToFloatPtr() );
__m128 jqc_0 = _mm_load_ps( joints[n2].q.ToFloatPtr() );
__m128 jqd_0 = _mm_load_ps( joints[n3].q.ToFloatPtr() );
__m128 jta_0 = _mm_load_ps( joints[n0].t.ToFloatPtr() );
__m128 jtb_0 = _mm_load_ps( joints[n1].t.ToFloatPtr() );
__m128 jtc_0 = _mm_load_ps( joints[n2].t.ToFloatPtr() );
__m128 jtd_0 = _mm_load_ps( joints[n3].t.ToFloatPtr() );
__m128 bqa_0 = _mm_load_ps( blendJoints[n0].q.ToFloatPtr() );
__m128 bqb_0 = _mm_load_ps( blendJoints[n1].q.ToFloatPtr() );
__m128 bqc_0 = _mm_load_ps( blendJoints[n2].q.ToFloatPtr() );
__m128 bqd_0 = _mm_load_ps( blendJoints[n3].q.ToFloatPtr() );
__m128 bta_0 = _mm_load_ps( blendJoints[n0].t.ToFloatPtr() );
__m128 btb_0 = _mm_load_ps( blendJoints[n1].t.ToFloatPtr() );
__m128 btc_0 = _mm_load_ps( blendJoints[n2].t.ToFloatPtr() );
__m128 btd_0 = _mm_load_ps( blendJoints[n3].t.ToFloatPtr() );
bta_0 = _mm_sub_ps( bta_0, jta_0 );
btb_0 = _mm_sub_ps( btb_0, jtb_0 );
btc_0 = _mm_sub_ps( btc_0, jtc_0 );
btd_0 = _mm_sub_ps( btd_0, jtd_0 );
jta_0 = _mm_madd_ps( vlerp, bta_0, jta_0 );
jtb_0 = _mm_madd_ps( vlerp, btb_0, jtb_0 );
jtc_0 = _mm_madd_ps( vlerp, btc_0, jtc_0 );
jtd_0 = _mm_madd_ps( vlerp, btd_0, jtd_0 );
_mm_store_ps( joints[n0].t.ToFloatPtr(), jta_0 );
_mm_store_ps( joints[n1].t.ToFloatPtr(), jtb_0 );
_mm_store_ps( joints[n2].t.ToFloatPtr(), jtc_0 );
_mm_store_ps( joints[n3].t.ToFloatPtr(), jtd_0 );
__m128 jqr_0 = _mm_unpacklo_ps( jqa_0, jqc_0 );
__m128 jqs_0 = _mm_unpackhi_ps( jqa_0, jqc_0 );
__m128 jqt_0 = _mm_unpacklo_ps( jqb_0, jqd_0 );
__m128 jqu_0 = _mm_unpackhi_ps( jqb_0, jqd_0 );
__m128 bqr_0 = _mm_unpacklo_ps( bqa_0, bqc_0 );
__m128 bqs_0 = _mm_unpackhi_ps( bqa_0, bqc_0 );
__m128 bqt_0 = _mm_unpacklo_ps( bqb_0, bqd_0 );
__m128 bqu_0 = _mm_unpackhi_ps( bqb_0, bqd_0 );
__m128 jqx_0 = _mm_unpacklo_ps( jqr_0, jqt_0 );
__m128 jqy_0 = _mm_unpackhi_ps( jqr_0, jqt_0 );
__m128 jqz_0 = _mm_unpacklo_ps( jqs_0, jqu_0 );
__m128 jqw_0 = _mm_unpackhi_ps( jqs_0, jqu_0 );
__m128 bqx_0 = _mm_unpacklo_ps( bqr_0, bqt_0 );
__m128 bqy_0 = _mm_unpackhi_ps( bqr_0, bqt_0 );
__m128 bqz_0 = _mm_unpacklo_ps( bqs_0, bqu_0 );
__m128 bqw_0 = _mm_unpackhi_ps( bqs_0, bqu_0 );
__m128 cosoma_0 = _mm_mul_ps( jqx_0, bqx_0 );
__m128 cosomb_0 = _mm_mul_ps( jqy_0, bqy_0 );
__m128 cosomc_0 = _mm_mul_ps( jqz_0, bqz_0 );
__m128 cosomd_0 = _mm_mul_ps( jqw_0, bqw_0 );
__m128 cosome_0 = _mm_add_ps( cosoma_0, cosomb_0 );
__m128 cosomf_0 = _mm_add_ps( cosomc_0, cosomd_0 );
__m128 cosom_0 = _mm_add_ps( cosome_0, cosomf_0 );
__m128 sign_0 = _mm_and_ps( cosom_0, vector_float_sign_bit );
__m128 scale_0 = _mm_xor_ps( vscaledLerp, sign_0 );
jqx_0 = _mm_madd_ps( scale_0, bqx_0, jqx_0 );
jqy_0 = _mm_madd_ps( scale_0, bqy_0, jqy_0 );
jqz_0 = _mm_madd_ps( scale_0, bqz_0, jqz_0 );
jqw_0 = _mm_madd_ps( scale_0, bqw_0, jqw_0 );
__m128 da_0 = _mm_mul_ps( jqx_0, jqx_0 );
__m128 db_0 = _mm_mul_ps( jqy_0, jqy_0 );
__m128 dc_0 = _mm_mul_ps( jqz_0, jqz_0 );
__m128 dd_0 = _mm_mul_ps( jqw_0, jqw_0 );
__m128 de_0 = _mm_add_ps( da_0, db_0 );
__m128 df_0 = _mm_add_ps( dc_0, dd_0 );
__m128 d_0 = _mm_add_ps( de_0, df_0 );
__m128 rs_0 = _mm_rsqrt_ps( d_0 );
__m128 sq_0 = _mm_mul_ps( rs_0, rs_0 );
__m128 sh_0 = _mm_mul_ps( rs_0, vector_float_rsqrt_c1 );
__m128 sx_0 = _mm_madd_ps( d_0, sq_0, vector_float_rsqrt_c0 );
__m128 s_0 = _mm_mul_ps( sh_0, sx_0 );
jqx_0 = _mm_mul_ps( jqx_0, s_0 );
jqy_0 = _mm_mul_ps( jqy_0, s_0 );
jqz_0 = _mm_mul_ps( jqz_0, s_0 );
jqw_0 = _mm_mul_ps( jqw_0, s_0 );
__m128 tp0_0 = _mm_unpacklo_ps( jqx_0, jqz_0 );
__m128 tp1_0 = _mm_unpackhi_ps( jqx_0, jqz_0 );
__m128 tp2_0 = _mm_unpacklo_ps( jqy_0, jqw_0 );
__m128 tp3_0 = _mm_unpackhi_ps( jqy_0, jqw_0 );
__m128 p0_0 = _mm_unpacklo_ps( tp0_0, tp2_0 );
__m128 p1_0 = _mm_unpackhi_ps( tp0_0, tp2_0 );
__m128 p2_0 = _mm_unpacklo_ps( tp1_0, tp3_0 );
__m128 p3_0 = _mm_unpackhi_ps( tp1_0, tp3_0 );
_mm_store_ps( joints[n0].q.ToFloatPtr(), p0_0 );
_mm_store_ps( joints[n1].q.ToFloatPtr(), p1_0 );
_mm_store_ps( joints[n2].q.ToFloatPtr(), p2_0 );
_mm_store_ps( joints[n3].q.ToFloatPtr(), p3_0 );
}
for ( ; i < numJoints; i++ ) {
const int n = index[i];
idVec3 &jointVert = joints[n].t;
const idVec3 &blendVert = blendJoints[n].t;
jointVert[0] += lerp * ( blendVert[0] - jointVert[0] );
jointVert[1] += lerp * ( blendVert[1] - jointVert[1] );
jointVert[2] += lerp * ( blendVert[2] - jointVert[2] );
idQuat &jointQuat = joints[n].q;
const idQuat &blendQuat = blendJoints[n].q;
float cosom;
float scale;
float s;
cosom = jointQuat.x * blendQuat.x + jointQuat.y * blendQuat.y + jointQuat.z * blendQuat.z + jointQuat.w * blendQuat.w;
scale = __fsels( cosom, scaledLerp, -scaledLerp );
jointQuat.x += scale * blendQuat.x;
jointQuat.y += scale * blendQuat.y;
jointQuat.z += scale * blendQuat.z;
jointQuat.w += scale * blendQuat.w;
s = jointQuat.x * jointQuat.x + jointQuat.y * jointQuat.y + jointQuat.z * jointQuat.z + jointQuat.w * jointQuat.w;
s = __frsqrts( s );
jointQuat.x *= s;
jointQuat.y *= s;
jointQuat.z *= s;
jointQuat.w *= s;
}
}
/*
============
idSIMD_SSE::ConvertJointQuatsToJointMats
============
*/
void VPCALL idSIMD_SSE::ConvertJointQuatsToJointMats( idJointMat *jointMats, const idJointQuat *jointQuats, const int numJoints ) {
assert( sizeof( idJointQuat ) == JOINTQUAT_SIZE );
assert( sizeof( idJointMat ) == JOINTMAT_SIZE );
assert( (int)(&((idJointQuat *)0)->t) == (int)(&((idJointQuat *)0)->q) + (int)sizeof( ((idJointQuat *)0)->q ) );
const float * jointQuatPtr = (float *)jointQuats;
float * jointMatPtr = (float *)jointMats;
const __m128 vector_float_first_sign_bit = __m128c( _mm_set_epi32( 0x00000000, 0x00000000, 0x00000000, 0x80000000 ) );
const __m128 vector_float_last_three_sign_bits = __m128c( _mm_set_epi32( 0x80000000, 0x80000000, 0x80000000, 0x00000000 ) );
const __m128 vector_float_first_pos_half = { 0.5f, 0.0f, 0.0f, 0.0f }; // +.5 0 0 0
const __m128 vector_float_first_neg_half = { -0.5f, 0.0f, 0.0f, 0.0f }; // -.5 0 0 0
const __m128 vector_float_quat2mat_mad1 = { -1.0f, -1.0f, +1.0f, -1.0f }; // - - + -
const __m128 vector_float_quat2mat_mad2 = { -1.0f, +1.0f, -1.0f, -1.0f }; // - + - -
const __m128 vector_float_quat2mat_mad3 = { +1.0f, -1.0f, -1.0f, +1.0f }; // + - - +
int i = 0;
for ( ; i + 1 < numJoints; i += 2 ) {
__m128 q0 = _mm_load_ps( &jointQuatPtr[i*8+0*8+0] );
__m128 q1 = _mm_load_ps( &jointQuatPtr[i*8+1*8+0] );
__m128 t0 = _mm_load_ps( &jointQuatPtr[i*8+0*8+4] );
__m128 t1 = _mm_load_ps( &jointQuatPtr[i*8+1*8+4] );
__m128 d0 = _mm_add_ps( q0, q0 );
__m128 d1 = _mm_add_ps( q1, q1 );
__m128 sa0 = _mm_perm_ps( q0, _MM_SHUFFLE( 1, 0, 0, 1 ) ); // y, x, x, y
__m128 sb0 = _mm_perm_ps( d0, _MM_SHUFFLE( 2, 2, 1, 1 ) ); // y2, y2, z2, z2
__m128 sc0 = _mm_perm_ps( q0, _MM_SHUFFLE( 3, 3, 3, 2 ) ); // z, w, w, w
__m128 sd0 = _mm_perm_ps( d0, _MM_SHUFFLE( 0, 1, 2, 2 ) ); // z2, z2, y2, x2
__m128 sa1 = _mm_perm_ps( q1, _MM_SHUFFLE( 1, 0, 0, 1 ) ); // y, x, x, y
__m128 sb1 = _mm_perm_ps( d1, _MM_SHUFFLE( 2, 2, 1, 1 ) ); // y2, y2, z2, z2
__m128 sc1 = _mm_perm_ps( q1, _MM_SHUFFLE( 3, 3, 3, 2 ) ); // z, w, w, w
__m128 sd1 = _mm_perm_ps( d1, _MM_SHUFFLE( 0, 1, 2, 2 ) ); // z2, z2, y2, x2
sa0 = _mm_xor_ps( sa0, vector_float_first_sign_bit );
sa1 = _mm_xor_ps( sa1, vector_float_first_sign_bit );
sc0 = _mm_xor_ps( sc0, vector_float_last_three_sign_bits ); // flip stupid inverse quaternions
sc1 = _mm_xor_ps( sc1, vector_float_last_three_sign_bits ); // flip stupid inverse quaternions
__m128 ma0 = _mm_add_ps( _mm_mul_ps( sa0, sb0 ), vector_float_first_pos_half ); // .5 - yy2, xy2, xz2, yz2 // .5 0 0 0
__m128 mb0 = _mm_add_ps( _mm_mul_ps( sc0, sd0 ), vector_float_first_neg_half ); // -.5 + zz2, wz2, wy2, wx2 // -.5 0 0 0
__m128 mc0 = _mm_sub_ps( vector_float_first_pos_half, _mm_mul_ps( q0, d0 ) ); // .5 - xx2, -yy2, -zz2, -ww2 // .5 0 0 0
__m128 ma1 = _mm_add_ps( _mm_mul_ps( sa1, sb1 ), vector_float_first_pos_half ); // .5 - yy2, xy2, xz2, yz2 // .5 0 0 0
__m128 mb1 = _mm_add_ps( _mm_mul_ps( sc1, sd1 ), vector_float_first_neg_half ); // -.5 + zz2, wz2, wy2, wx2 // -.5 0 0 0
__m128 mc1 = _mm_sub_ps( vector_float_first_pos_half, _mm_mul_ps( q1, d1 ) ); // .5 - xx2, -yy2, -zz2, -ww2 // .5 0 0 0
__m128 mf0 = _mm_shuffle_ps( ma0, mc0, _MM_SHUFFLE( 0, 0, 1, 1 ) ); // xy2, xy2, .5 - xx2, .5 - xx2 // 01, 01, 10, 10
__m128 md0 = _mm_shuffle_ps( mf0, ma0, _MM_SHUFFLE( 3, 2, 0, 2 ) ); // .5 - xx2, xy2, xz2, yz2 // 10, 01, 02, 03
__m128 me0 = _mm_shuffle_ps( ma0, mb0, _MM_SHUFFLE( 3, 2, 1, 0 ) ); // .5 - yy2, xy2, wy2, wx2 // 00, 01, 12, 13
__m128 mf1 = _mm_shuffle_ps( ma1, mc1, _MM_SHUFFLE( 0, 0, 1, 1 ) ); // xy2, xy2, .5 - xx2, .5 - xx2 // 01, 01, 10, 10
__m128 md1 = _mm_shuffle_ps( mf1, ma1, _MM_SHUFFLE( 3, 2, 0, 2 ) ); // .5 - xx2, xy2, xz2, yz2 // 10, 01, 02, 03
__m128 me1 = _mm_shuffle_ps( ma1, mb1, _MM_SHUFFLE( 3, 2, 1, 0 ) ); // .5 - yy2, xy2, wy2, wx2 // 00, 01, 12, 13
__m128 ra0 = _mm_add_ps( _mm_mul_ps( mb0, vector_float_quat2mat_mad1 ), ma0 ); // 1 - yy2 - zz2, xy2 - wz2, xz2 + wy2, // - - + -
__m128 rb0 = _mm_add_ps( _mm_mul_ps( mb0, vector_float_quat2mat_mad2 ), md0 ); // 1 - xx2 - zz2, xy2 + wz2, , yz2 - wx2 // - + - -
__m128 rc0 = _mm_add_ps( _mm_mul_ps( me0, vector_float_quat2mat_mad3 ), md0 ); // 1 - xx2 - yy2, , xz2 - wy2, yz2 + wx2 // + - - +
__m128 ra1 = _mm_add_ps( _mm_mul_ps( mb1, vector_float_quat2mat_mad1 ), ma1 ); // 1 - yy2 - zz2, xy2 - wz2, xz2 + wy2, // - - + -
__m128 rb1 = _mm_add_ps( _mm_mul_ps( mb1, vector_float_quat2mat_mad2 ), md1 ); // 1 - xx2 - zz2, xy2 + wz2, , yz2 - wx2 // - + - -
__m128 rc1 = _mm_add_ps( _mm_mul_ps( me1, vector_float_quat2mat_mad3 ), md1 ); // 1 - xx2 - yy2, , xz2 - wy2, yz2 + wx2 // + - - +
__m128 ta0 = _mm_shuffle_ps( ra0, t0, _MM_SHUFFLE( 0, 0, 2, 2 ) );
__m128 tb0 = _mm_shuffle_ps( rb0, t0, _MM_SHUFFLE( 1, 1, 3, 3 ) );
__m128 tc0 = _mm_shuffle_ps( rc0, t0, _MM_SHUFFLE( 2, 2, 0, 0 ) );
__m128 ta1 = _mm_shuffle_ps( ra1, t1, _MM_SHUFFLE( 0, 0, 2, 2 ) );
__m128 tb1 = _mm_shuffle_ps( rb1, t1, _MM_SHUFFLE( 1, 1, 3, 3 ) );
__m128 tc1 = _mm_shuffle_ps( rc1, t1, _MM_SHUFFLE( 2, 2, 0, 0 ) );
ra0 = _mm_shuffle_ps( ra0, ta0, _MM_SHUFFLE( 2, 0, 1, 0 ) ); // 00 01 02 10
rb0 = _mm_shuffle_ps( rb0, tb0, _MM_SHUFFLE( 2, 0, 0, 1 ) ); // 01 00 03 11
rc0 = _mm_shuffle_ps( rc0, tc0, _MM_SHUFFLE( 2, 0, 3, 2 ) ); // 02 03 00 12
ra1 = _mm_shuffle_ps( ra1, ta1, _MM_SHUFFLE( 2, 0, 1, 0 ) ); // 00 01 02 10
rb1 = _mm_shuffle_ps( rb1, tb1, _MM_SHUFFLE( 2, 0, 0, 1 ) ); // 01 00 03 11
rc1 = _mm_shuffle_ps( rc1, tc1, _MM_SHUFFLE( 2, 0, 3, 2 ) ); // 02 03 00 12
_mm_store_ps( &jointMatPtr[i*12+0*12+0], ra0 );
_mm_store_ps( &jointMatPtr[i*12+0*12+4], rb0 );
_mm_store_ps( &jointMatPtr[i*12+0*12+8], rc0 );
_mm_store_ps( &jointMatPtr[i*12+1*12+0], ra1 );
_mm_store_ps( &jointMatPtr[i*12+1*12+4], rb1 );
_mm_store_ps( &jointMatPtr[i*12+1*12+8], rc1 );
}
for ( ; i < numJoints; i++ ) {
__m128 q0 = _mm_load_ps( &jointQuatPtr[i*8+0*8+0] );
__m128 t0 = _mm_load_ps( &jointQuatPtr[i*8+0*8+4] );
__m128 d0 = _mm_add_ps( q0, q0 );
__m128 sa0 = _mm_perm_ps( q0, _MM_SHUFFLE( 1, 0, 0, 1 ) ); // y, x, x, y
__m128 sb0 = _mm_perm_ps( d0, _MM_SHUFFLE( 2, 2, 1, 1 ) ); // y2, y2, z2, z2
__m128 sc0 = _mm_perm_ps( q0, _MM_SHUFFLE( 3, 3, 3, 2 ) ); // z, w, w, w
__m128 sd0 = _mm_perm_ps( d0, _MM_SHUFFLE( 0, 1, 2, 2 ) ); // z2, z2, y2, x2
sa0 = _mm_xor_ps( sa0, vector_float_first_sign_bit );
sc0 = _mm_xor_ps( sc0, vector_float_last_three_sign_bits ); // flip stupid inverse quaternions
__m128 ma0 = _mm_add_ps( _mm_mul_ps( sa0, sb0 ), vector_float_first_pos_half ); // .5 - yy2, xy2, xz2, yz2 // .5 0 0 0
__m128 mb0 = _mm_add_ps( _mm_mul_ps( sc0, sd0 ), vector_float_first_neg_half ); // -.5 + zz2, wz2, wy2, wx2 // -.5 0 0 0
__m128 mc0 = _mm_sub_ps( vector_float_first_pos_half, _mm_mul_ps( q0, d0 ) ); // .5 - xx2, -yy2, -zz2, -ww2 // .5 0 0 0
__m128 mf0 = _mm_shuffle_ps( ma0, mc0, _MM_SHUFFLE( 0, 0, 1, 1 ) ); // xy2, xy2, .5 - xx2, .5 - xx2 // 01, 01, 10, 10
__m128 md0 = _mm_shuffle_ps( mf0, ma0, _MM_SHUFFLE( 3, 2, 0, 2 ) ); // .5 - xx2, xy2, xz2, yz2 // 10, 01, 02, 03
__m128 me0 = _mm_shuffle_ps( ma0, mb0, _MM_SHUFFLE( 3, 2, 1, 0 ) ); // .5 - yy2, xy2, wy2, wx2 // 00, 01, 12, 13
__m128 ra0 = _mm_add_ps( _mm_mul_ps( mb0, vector_float_quat2mat_mad1 ), ma0 ); // 1 - yy2 - zz2, xy2 - wz2, xz2 + wy2, // - - + -
__m128 rb0 = _mm_add_ps( _mm_mul_ps( mb0, vector_float_quat2mat_mad2 ), md0 ); // 1 - xx2 - zz2, xy2 + wz2, , yz2 - wx2 // - + - -
__m128 rc0 = _mm_add_ps( _mm_mul_ps( me0, vector_float_quat2mat_mad3 ), md0 ); // 1 - xx2 - yy2, , xz2 - wy2, yz2 + wx2 // + - - +
__m128 ta0 = _mm_shuffle_ps( ra0, t0, _MM_SHUFFLE( 0, 0, 2, 2 ) );
__m128 tb0 = _mm_shuffle_ps( rb0, t0, _MM_SHUFFLE( 1, 1, 3, 3 ) );
__m128 tc0 = _mm_shuffle_ps( rc0, t0, _MM_SHUFFLE( 2, 2, 0, 0 ) );
ra0 = _mm_shuffle_ps( ra0, ta0, _MM_SHUFFLE( 2, 0, 1, 0 ) ); // 00 01 02 10
rb0 = _mm_shuffle_ps( rb0, tb0, _MM_SHUFFLE( 2, 0, 0, 1 ) ); // 01 00 03 11
rc0 = _mm_shuffle_ps( rc0, tc0, _MM_SHUFFLE( 2, 0, 3, 2 ) ); // 02 03 00 12
_mm_store_ps( &jointMatPtr[i*12+0*12+0], ra0 );
_mm_store_ps( &jointMatPtr[i*12+0*12+4], rb0 );
_mm_store_ps( &jointMatPtr[i*12+0*12+8], rc0 );
}
}
/*
============
idSIMD_SSE::ConvertJointMatsToJointQuats
============
*/
void VPCALL idSIMD_SSE::ConvertJointMatsToJointQuats( idJointQuat *jointQuats, const idJointMat *jointMats, const int numJoints ) {
assert( sizeof( idJointQuat ) == JOINTQUAT_SIZE );
assert( sizeof( idJointMat ) == JOINTMAT_SIZE );
assert( (int)(&((idJointQuat *)0)->t) == (int)(&((idJointQuat *)0)->q) + (int)sizeof( ((idJointQuat *)0)->q ) );
const __m128 vector_float_zero = _mm_setzero_ps();
const __m128 vector_float_one = { 1.0f, 1.0f, 1.0f, 1.0f };
const __m128 vector_float_not = __m128c( _mm_set_epi32( -1, -1, -1, -1 ) );
const __m128 vector_float_sign_bit = __m128c( _mm_set_epi32( 0x80000000, 0x80000000, 0x80000000, 0x80000000 ) );
const __m128 vector_float_rsqrt_c0 = { -3.0f, -3.0f, -3.0f, -3.0f };
const __m128 vector_float_rsqrt_c2 = { -0.25f, -0.25f, -0.25f, -0.25f };
int i = 0;
for ( ; i < numJoints - 3; i += 4 ) {
const float *__restrict m = (float *)&jointMats[i];
float *__restrict q = (float *)&jointQuats[i];
__m128 ma0 = _mm_load_ps( &m[0*12+0] );
__m128 ma1 = _mm_load_ps( &m[0*12+4] );
__m128 ma2 = _mm_load_ps( &m[0*12+8] );
__m128 mb0 = _mm_load_ps( &m[1*12+0] );
__m128 mb1 = _mm_load_ps( &m[1*12+4] );
__m128 mb2 = _mm_load_ps( &m[1*12+8] );
__m128 mc0 = _mm_load_ps( &m[2*12+0] );
__m128 mc1 = _mm_load_ps( &m[2*12+4] );
__m128 mc2 = _mm_load_ps( &m[2*12+8] );
__m128 md0 = _mm_load_ps( &m[3*12+0] );
__m128 md1 = _mm_load_ps( &m[3*12+4] );
__m128 md2 = _mm_load_ps( &m[3*12+8] );
__m128 ta0 = _mm_unpacklo_ps( ma0, mc0 ); // a0, c0, a1, c1
__m128 ta1 = _mm_unpackhi_ps( ma0, mc0 ); // a2, c2, a3, c3
__m128 ta2 = _mm_unpacklo_ps( mb0, md0 ); // b0, d0, b1, b2
__m128 ta3 = _mm_unpackhi_ps( mb0, md0 ); // b2, d2, b3, d3
__m128 tb0 = _mm_unpacklo_ps( ma1, mc1 ); // a0, c0, a1, c1
__m128 tb1 = _mm_unpackhi_ps( ma1, mc1 ); // a2, c2, a3, c3
__m128 tb2 = _mm_unpacklo_ps( mb1, md1 ); // b0, d0, b1, b2
__m128 tb3 = _mm_unpackhi_ps( mb1, md1 ); // b2, d2, b3, d3
__m128 tc0 = _mm_unpacklo_ps( ma2, mc2 ); // a0, c0, a1, c1
__m128 tc1 = _mm_unpackhi_ps( ma2, mc2 ); // a2, c2, a3, c3
__m128 tc2 = _mm_unpacklo_ps( mb2, md2 ); // b0, d0, b1, b2
__m128 tc3 = _mm_unpackhi_ps( mb2, md2 ); // b2, d2, b3, d3
__m128 m00 = _mm_unpacklo_ps( ta0, ta2 );
__m128 m01 = _mm_unpackhi_ps( ta0, ta2 );
__m128 m02 = _mm_unpacklo_ps( ta1, ta3 );
__m128 m03 = _mm_unpackhi_ps( ta1, ta3 );
__m128 m10 = _mm_unpacklo_ps( tb0, tb2 );
__m128 m11 = _mm_unpackhi_ps( tb0, tb2 );
__m128 m12 = _mm_unpacklo_ps( tb1, tb3 );
__m128 m13 = _mm_unpackhi_ps( tb1, tb3 );
__m128 m20 = _mm_unpacklo_ps( tc0, tc2 );
__m128 m21 = _mm_unpackhi_ps( tc0, tc2 );
__m128 m22 = _mm_unpacklo_ps( tc1, tc3 );
__m128 m23 = _mm_unpackhi_ps( tc1, tc3 );
__m128 b00 = _mm_add_ps( m00, m11 );
__m128 b11 = _mm_cmpgt_ps( m00, m22 );
__m128 b01 = _mm_add_ps( b00, m22 );
__m128 b10 = _mm_cmpgt_ps( m00, m11 );
__m128 b0 = _mm_cmpgt_ps( b01, vector_float_zero );
__m128 b1 = _mm_and_ps( b10, b11 );
__m128 b2 = _mm_cmpgt_ps( m11, m22 );
__m128 m0 = b0;
__m128 m1 = _mm_and_ps( _mm_xor_ps( b0, vector_float_not ), b1 );
__m128 p1 = _mm_or_ps( b0, b1 );
__m128 p2 = _mm_or_ps( p1, b2 );
__m128 m2 = _mm_and_ps( _mm_xor_ps( p1, vector_float_not ), b2 );
__m128 m3 = _mm_xor_ps( p2, vector_float_not );
__m128 i0 = _mm_or_ps( m2, m3 );
__m128 i1 = _mm_or_ps( m1, m3 );
__m128 i2 = _mm_or_ps( m1, m2 );
__m128 s0 = _mm_and_ps( i0, vector_float_sign_bit );
__m128 s1 = _mm_and_ps( i1, vector_float_sign_bit );
__m128 s2 = _mm_and_ps( i2, vector_float_sign_bit );
m00 = _mm_xor_ps( m00, s0 );
m11 = _mm_xor_ps( m11, s1 );
m22 = _mm_xor_ps( m22, s2 );
m21 = _mm_xor_ps( m21, s0 );
m02 = _mm_xor_ps( m02, s1 );
m10 = _mm_xor_ps( m10, s2 );
__m128 t0 = _mm_add_ps( m00, m11 );
__m128 t1 = _mm_add_ps( m22, vector_float_one );
__m128 q0 = _mm_add_ps( t0, t1 );
__m128 q1 = _mm_sub_ps( m01, m10 );
__m128 q2 = _mm_sub_ps( m20, m02 );
__m128 q3 = _mm_sub_ps( m12, m21 );
__m128 rs = _mm_rsqrt_ps( q0 );
__m128 sq = _mm_mul_ps( rs, rs );
__m128 sh = _mm_mul_ps( rs, vector_float_rsqrt_c2 );
__m128 sx = _mm_madd_ps( q0, sq, vector_float_rsqrt_c0 );
__m128 s = _mm_mul_ps( sh, sx );
q0 = _mm_mul_ps( q0, s );
q1 = _mm_mul_ps( q1, s );
q2 = _mm_mul_ps( q2, s );
q3 = _mm_mul_ps( q3, s );
m0 = _mm_or_ps( m0, m2 );
m2 = _mm_or_ps( m2, m3 );
__m128 fq0 = _mm_sel_ps( q0, q3, m0 );
__m128 fq1 = _mm_sel_ps( q1, q2, m0 );
__m128 fq2 = _mm_sel_ps( q2, q1, m0 );
__m128 fq3 = _mm_sel_ps( q3, q0, m0 );
__m128 rq0 = _mm_sel_ps( fq0, fq2, m2 );
__m128 rq1 = _mm_sel_ps( fq1, fq3, m2 );
__m128 rq2 = _mm_sel_ps( fq2, fq0, m2 );
__m128 rq3 = _mm_sel_ps( fq3, fq1, m2 );
__m128 tq0 = _mm_unpacklo_ps( rq0, rq2 );
__m128 tq1 = _mm_unpackhi_ps( rq0, rq2 );
__m128 tq2 = _mm_unpacklo_ps( rq1, rq3 );
__m128 tq3 = _mm_unpackhi_ps( rq1, rq3 );
__m128 sq0 = _mm_unpacklo_ps( tq0, tq2 );
__m128 sq1 = _mm_unpackhi_ps( tq0, tq2 );
__m128 sq2 = _mm_unpacklo_ps( tq1, tq3 );
__m128 sq3 = _mm_unpackhi_ps( tq1, tq3 );
__m128 tt0 = _mm_unpacklo_ps( m03, m23 );
__m128 tt1 = _mm_unpackhi_ps( m03, m23 );
__m128 tt2 = _mm_unpacklo_ps( m13, vector_float_zero );
__m128 tt3 = _mm_unpackhi_ps( m13, vector_float_zero );
__m128 st0 = _mm_unpacklo_ps( tt0, tt2 );
__m128 st1 = _mm_unpackhi_ps( tt0, tt2 );
__m128 st2 = _mm_unpacklo_ps( tt1, tt3 );
__m128 st3 = _mm_unpackhi_ps( tt1, tt3 );
_mm_store_ps( &q[0*4], sq0 );
_mm_store_ps( &q[1*4], st0 );
_mm_store_ps( &q[2*4], sq1 );
_mm_store_ps( &q[3*4], st1 );
_mm_store_ps( &q[4*4], sq2 );
_mm_store_ps( &q[5*4], st2 );
_mm_store_ps( &q[6*4], sq3 );
_mm_store_ps( &q[7*4], st3 );
}
float sign[2] = { 1.0f, -1.0f };
for ( ; i < numJoints; i++ ) {
const float *__restrict m = (float *)&jointMats[i];
float *__restrict q = (float *)&jointQuats[i];
int b0 = m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] > 0.0f;
int b1 = m[0 * 4 + 0] > m[1 * 4 + 1] && m[0 * 4 + 0] > m[2 * 4 + 2];
int b2 = m[1 * 4 + 1] > m[2 * 4 + 2];
int m0 = b0;
int m1 = ( !b0 ) & b1;
int m2 = ( !( b0 | b1 ) ) & b2;
int m3 = !( b0 | b1 | b2 );
int i0 = ( m2 | m3 );
int i1 = ( m1 | m3 );
int i2 = ( m1 | m2 );
float s0 = sign[i0];
float s1 = sign[i1];
float s2 = sign[i2];
float t = s0 * m[0 * 4 + 0] + s1 * m[1 * 4 + 1] + s2 * m[2 * 4 + 2] + 1.0f;
float s = __frsqrts( t );
s = ( t * s * s + -3.0f ) * ( s * -0.25f );
q[0] = t * s;
q[1] = ( m[0 * 4 + 1] - s2 * m[1 * 4 + 0] ) * s;
q[2] = ( m[2 * 4 + 0] - s1 * m[0 * 4 + 2] ) * s;
q[3] = ( m[1 * 4 + 2] - s0 * m[2 * 4 + 1] ) * s;
if ( m0 | m2 ) {
// reverse
SwapValues( q[0], q[3] );
SwapValues( q[1], q[2] );
}
if ( m2 | m3 ) {
// rotate 2
SwapValues( q[0], q[2] );
SwapValues( q[1], q[3] );
}
q[4] = m[0 * 4 + 3];
q[5] = m[1 * 4 + 3];
q[6] = m[2 * 4 + 3];
q[7] = 0.0f;
}
}
/*
============
idSIMD_SSE::TransformJoints
============
*/
void VPCALL idSIMD_SSE::TransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint ) {
const __m128 vector_float_mask_keep_last = __m128c( _mm_set_epi32( 0xFFFFFFFF, 0x00000000, 0x00000000, 0x00000000 ) );
const float *__restrict firstMatrix = jointMats->ToFloatPtr() + ( firstJoint + firstJoint + firstJoint - 3 ) * 4;
__m128 pma = _mm_load_ps( firstMatrix + 0 );
__m128 pmb = _mm_load_ps( firstMatrix + 4 );
__m128 pmc = _mm_load_ps( firstMatrix + 8 );
for ( int joint = firstJoint; joint <= lastJoint; joint++ ) {
const int parent = parents[joint];
const float *__restrict parentMatrix = jointMats->ToFloatPtr() + ( parent + parent + parent ) * 4;
float *__restrict childMatrix = jointMats->ToFloatPtr() + ( joint + joint + joint ) * 4;
if ( parent != joint - 1 ) {
pma = _mm_load_ps( parentMatrix + 0 );
pmb = _mm_load_ps( parentMatrix + 4 );
pmc = _mm_load_ps( parentMatrix + 8 );
}
__m128 cma = _mm_load_ps( childMatrix + 0 );
__m128 cmb = _mm_load_ps( childMatrix + 4 );
__m128 cmc = _mm_load_ps( childMatrix + 8 );
__m128 ta = _mm_splat_ps( pma, 0 );
__m128 tb = _mm_splat_ps( pmb, 0 );
__m128 tc = _mm_splat_ps( pmc, 0 );
__m128 td = _mm_splat_ps( pma, 1 );
__m128 te = _mm_splat_ps( pmb, 1 );
__m128 tf = _mm_splat_ps( pmc, 1 );
__m128 tg = _mm_splat_ps( pma, 2 );
__m128 th = _mm_splat_ps( pmb, 2 );
__m128 ti = _mm_splat_ps( pmc, 2 );
pma = _mm_madd_ps( ta, cma, _mm_and_ps( pma, vector_float_mask_keep_last ) );
pmb = _mm_madd_ps( tb, cma, _mm_and_ps( pmb, vector_float_mask_keep_last ) );
pmc = _mm_madd_ps( tc, cma, _mm_and_ps( pmc, vector_float_mask_keep_last ) );
pma = _mm_madd_ps( td, cmb, pma );
pmb = _mm_madd_ps( te, cmb, pmb );
pmc = _mm_madd_ps( tf, cmb, pmc );
pma = _mm_madd_ps( tg, cmc, pma );
pmb = _mm_madd_ps( th, cmc, pmb );
pmc = _mm_madd_ps( ti, cmc, pmc );
_mm_store_ps( childMatrix + 0, pma );
_mm_store_ps( childMatrix + 4, pmb );
_mm_store_ps( childMatrix + 8, pmc );
}
}
/*
============
idSIMD_SSE::UntransformJoints
============
*/
void VPCALL idSIMD_SSE::UntransformJoints( idJointMat *jointMats, const int *parents, const int firstJoint, const int lastJoint ) {
const __m128 vector_float_mask_keep_last = __m128c( _mm_set_epi32( 0xFFFFFFFF, 0x00000000, 0x00000000, 0x00000000 ) );
for ( int joint = lastJoint; joint >= firstJoint; joint-- ) {
assert( parents[joint] < joint );
const int parent = parents[joint];
const float *__restrict parentMatrix = jointMats->ToFloatPtr() + ( parent + parent + parent ) * 4;
float *__restrict childMatrix = jointMats->ToFloatPtr() + ( joint + joint + joint ) * 4;
__m128 pma = _mm_load_ps( parentMatrix + 0 );
__m128 pmb = _mm_load_ps( parentMatrix + 4 );
__m128 pmc = _mm_load_ps( parentMatrix + 8 );
__m128 cma = _mm_load_ps( childMatrix + 0 );
__m128 cmb = _mm_load_ps( childMatrix + 4 );
__m128 cmc = _mm_load_ps( childMatrix + 8 );
__m128 ta = _mm_splat_ps( pma, 0 );
__m128 tb = _mm_splat_ps( pma, 1 );
__m128 tc = _mm_splat_ps( pma, 2 );
__m128 td = _mm_splat_ps( pmb, 0 );
__m128 te = _mm_splat_ps( pmb, 1 );
__m128 tf = _mm_splat_ps( pmb, 2 );
__m128 tg = _mm_splat_ps( pmc, 0 );
__m128 th = _mm_splat_ps( pmc, 1 );
__m128 ti = _mm_splat_ps( pmc, 2 );
cma = _mm_sub_ps( cma, _mm_and_ps( pma, vector_float_mask_keep_last ) );
cmb = _mm_sub_ps( cmb, _mm_and_ps( pmb, vector_float_mask_keep_last ) );
cmc = _mm_sub_ps( cmc, _mm_and_ps( pmc, vector_float_mask_keep_last ) );
pma = _mm_mul_ps( ta, cma );
pmb = _mm_mul_ps( tb, cma );
pmc = _mm_mul_ps( tc, cma );
pma = _mm_madd_ps( td, cmb, pma );
pmb = _mm_madd_ps( te, cmb, pmb );
pmc = _mm_madd_ps( tf, cmb, pmc );
pma = _mm_madd_ps( tg, cmc, pma );
pmb = _mm_madd_ps( th, cmc, pmb );
pmc = _mm_madd_ps( ti, cmc, pmc );
_mm_store_ps( childMatrix + 0, pma );
_mm_store_ps( childMatrix + 4, pmb );
_mm_store_ps( childMatrix + 8, pmc );
}
}
Reference in a new issue Copy permalink