242 lines
12 KiB
C++
242 lines
12 KiB
C++
/*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* Copyright (c) 2011-2014, Willow Garage, Inc.
|
|
* Copyright (c) 2014-2016, Open Source Robotics Foundation
|
|
* All rights reserved.
|
|
*
|
|
* Redistribution and use in source and binary forms, with or without
|
|
* modification, are permitted provided that the following conditions
|
|
* are met:
|
|
*
|
|
* * Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* * Redistributions in binary form must reproduce the above
|
|
* copyright notice, this list of conditions and the following
|
|
* disclaimer in the documentation and/or other materials provided
|
|
* with the distribution.
|
|
* * Neither the name of Open Source Robotics Foundation nor the names of its
|
|
* contributors may be used to endorse or promote products derived
|
|
* from this software without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
/** \author Jia Pan */
|
|
|
|
#ifndef FCL_MULTIPLE_INTERSECT
|
|
#define FCL_MULTIPLE_INTERSECT
|
|
|
|
#include <xmmintrin.h>
|
|
#include <pmmintrin.h>
|
|
|
|
|
|
namespace fcl
|
|
{
|
|
|
|
|
|
static inline __m128 sse_four_spheres_intersect(const Vector3d& o1, FCL_REAL r1,
|
|
const Vector3d& o2, FCL_REAL r2,
|
|
const Vector3d& o3, FCL_REAL r3,
|
|
const Vector3d& o4, FCL_REAL r4,
|
|
const Vector3d& o5, FCL_REAL r5,
|
|
const Vector3d& o6, FCL_REAL r6,
|
|
const Vector3d& o7, FCL_REAL r7,
|
|
const Vector3d& o8, FCL_REAL r8)
|
|
{
|
|
__m128 PX, PY, PZ, PR, QX, QY, QZ, QR;
|
|
PX = _mm_set_ps(o1[0], o2[0], o3[0], o4[0]);
|
|
PY = _mm_set_ps(o1[1], o2[1], o3[1], o4[1]);
|
|
PZ = _mm_set_ps(o1[2], o2[2], o3[2], o4[2]);
|
|
PR = _mm_set_ps(r1, r2, r3, r4);
|
|
QX = _mm_set_ps(o5[0], o6[0], o7[0], o8[0]);
|
|
QY = _mm_set_ps(o5[1], o6[1], o7[1], o8[1]);
|
|
QZ = _mm_set_ps(o5[2], o6[2], o7[2], o8[2]);
|
|
QR = _mm_set_ps(r5, r6, r7, r8);
|
|
|
|
__m128 T1 = _mm_sub_ps(PX, QX);
|
|
__m128 T2 = _mm_sub_ps(PY, QY);
|
|
__m128 T3 = _mm_sub_ps(PZ, QZ);
|
|
__m128 T4 = _mm_add_ps(PR, QR);
|
|
T1 = _mm_mul_ps(T1, T1);
|
|
T2 = _mm_mul_ps(T2, T2);
|
|
T3 = _mm_mul_ps(T3, T3);
|
|
T4 = _mm_mul_ps(T4, T4);
|
|
T1 = _mm_add_ps(T1, T2);
|
|
T2 = _mm_sub_ps(R2, T3);
|
|
return _mm_cmple_ps(T1, T2);
|
|
}
|
|
|
|
|
|
static inline __m128 sse_four_spheres_four_AABBs_intersect(const Vector3d& o1, FCL_REAL r1,
|
|
const Vector3d& o2, FCL_REAL r2,
|
|
const Vector3d& o3, FCL_REAL r3,
|
|
const Vector3d& o4, FCL_REAL r4,
|
|
const Vector3d& min1, const Vector3d& max1,
|
|
const Vector3d& min2, const Vector3d& max2,
|
|
const Vector3d& min3, const Vector3d& max3,
|
|
const Vector3d& min4, const Vector3d& max4)
|
|
{
|
|
__m128 MINX, MINY, MINZ, MAXX, MAXX, MAXY, MAXZ, SX, SY, SZ, SR;
|
|
MINX = _mm_set_ps(min1[0], min2[0], min3[0], min4[0]);
|
|
MAXX = _mm_set_ps(max1[0], max2[0], max3[0], max4[0]);
|
|
MINY = _mm_set_ps(min1[1], min2[1], min3[1], min4[1]);
|
|
MAXY = _mm_set_ps(max1[1], max2[1], max3[1], max4[1]);
|
|
MINZ = _mm_set_ps(min1[2], min2[2], min3[2], min4[2]);
|
|
MAXZ = _mm_set_ps(max1[2], max2[2], max3[2], max4[2]);
|
|
SX = _mm_set_ps(o1[0], o2[0], o3[0], o4[0]);
|
|
SY = _mm_set_ps(o1[1], o2[1], o3[1], o4[1]);
|
|
SZ = _mm_set_ps(o1[2], o2[2], o3[2], o4[2]);
|
|
SR = _mm_set_ps(r1, r2, r3, r4);
|
|
|
|
|
|
__m128 TX = _mm_max_ps(SX, MINX);
|
|
__m128 TY = _mm_max_ps(SY, MINY);
|
|
__m128 TZ = _mm_max_ps(SZ, MINZ);
|
|
TX = _mm_min_ps(TX, MAXX);
|
|
TY = _mm_min_ps(TY, MAXY);
|
|
TZ = _mm_min_ps(TZ, MAXZ);
|
|
__m128 DX = _mm_sub_ps(SX, TX);
|
|
__m128 DY = _mm_sub_ps(SY, TY);
|
|
__m128 DZ = _mm_sub_ps(SZ, TZ);
|
|
__m128 R2 = _mm_mul_ps(SR, SR);
|
|
DX = _mm_mul_ps(DX, DX);
|
|
DY = _mm_mul_ps(DY, DY);
|
|
DZ = _mm_mul_ps(DZ, DZ);
|
|
__m128 T1 = _mm_add_ps(DX, DY);
|
|
__m128 T2 = _mm_sub_ps(R2, DZ);
|
|
return _mm_cmple_ps(T1, T2);
|
|
}
|
|
|
|
|
|
static inline __m128 sse_four_AABBs_intersect(const Vector3d& min1, const Vector3d& max1,
|
|
const Vector3d& min2, const Vector3d& max2,
|
|
const Vector3d& min3, const Vector3d& max3,
|
|
const Vector3d& min4, const Vector3d& max4,
|
|
const Vector3d& min5, const Vector3d& max5,
|
|
const Vector3d& min6, const Vector3d& max6,
|
|
const Vector3d& min7, const Vector3d& max7,
|
|
const Vector3d& min8, const Vector3d& max8)
|
|
{
|
|
__m128 MIN1X, MIN1Y, MIN1Z, MAX1X, MAX1Y, MAX1Z, MIN2X, MIN2Y, MIN2Z, MAX2X, MAX2Y, MAX2Z;
|
|
MIN1X = _mm_set_ps(min1[0], min2[0], min3[0], min4[0]);
|
|
MAX1X = _mm_set_ps(max1[0], max2[0], max3[0], max4[0]);
|
|
MIN1Y = _mm_set_ps(min1[1], min2[1], min3[1], min4[1]);
|
|
MAX1Y = _mm_set_ps(max1[1], max2[1], max3[1], max4[1]);
|
|
MIN1Z = _mm_set_ps(min1[2], min2[2], min3[2], min4[2]);
|
|
MAX1Z = _mm_set_ps(max1[2], max2[2], max3[2], max4[2]);
|
|
MIN2X = _mm_set_ps(min5[0], min6[0], min7[0], min8[0]);
|
|
MAX2X = _mm_set_ps(max5[0], max6[0], max7[0], max8[0]);
|
|
MIN2Y = _mm_set_ps(min5[1], min6[1], min7[1], min8[1]);
|
|
MAX2Y = _mm_set_ps(max5[1], max6[1], max7[1], max8[1]);
|
|
MIN2Z = _mm_set_ps(min5[2], min6[2], min7[2], min8[2]);
|
|
MAX2Z = _mm_set_ps(max5[2], max6[2], max7[2], max8[2]);
|
|
|
|
__m128 AX = _mm_max_ps(MIN1X, MIN2X);
|
|
__m128 BX = _mm_min_ps(MAX1X, MAX2X);
|
|
__m128 AY = _mm_max_ps(MIN1Y, MIN2Y);
|
|
__m128 BY = _mm_min_ps(MAX1Y, MAX2Y);
|
|
__m128 AZ = _mm_max_ps(MIN1Z, MIN2Z);
|
|
__m128 BZ = _mm_min_ps(MAX1Z, MAX2Z);
|
|
__m128 T1 = _mm_cmple_ps(AX, BX);
|
|
__m128 T2 = _mm_cmple_ps(AY, BY);
|
|
__m128 T3 = _mm_cmple_ps(AZ, BZ);
|
|
__m128 T4 = _mm_and_ps(T1, T2);
|
|
return _mm_and_ps(T3, T4);
|
|
}
|
|
|
|
static bool four_spheres_intersect_and(const Vector3d& o1, FCL_REAL r1,
|
|
const Vector3d& o2, FCL_REAL r2,
|
|
const Vector3d& o3, FCL_REAL r3,
|
|
const Vector3d& o4, FCL_REAL r4,
|
|
const Vector3d& o5, FCL_REAL r5,
|
|
const Vector3d& o6, FCL_REAL r6,
|
|
const Vector3d& o7, FCL_REAL r7,
|
|
const Vector3d& o8, FCL_REAL r8)
|
|
{
|
|
__m128 CMP = four_spheres_intersect(o1, r1, o2, r2, o3, r3, o4, r4, o5, r5, o6, r6, o7, r7, o8, r8);
|
|
return _mm_movemask_ps(CMP) == 15.f;
|
|
}
|
|
|
|
static bool four_spheres_intersect_or(const Vector3d& o1, FCL_REAL r1,
|
|
const Vector3d& o2, FCL_REAL r2,
|
|
const Vector3d& o3, FCL_REAL r3,
|
|
const Vector3d& o4, FCL_REAL r4,
|
|
const Vector3d& o5, FCL_REAL r5,
|
|
const Vector3d& o6, FCL_REAL r6,
|
|
const Vector3d& o7, FCL_REAL r7,
|
|
const Vector3d& o8, FCL_REAL r8)
|
|
{
|
|
__m128 CMP = four_spheres_intersect(o1, r1, o2, r2, o3, r3, o4, r4, o5, r5, o6, r6, o7, r7, o8, r8);
|
|
return __mm_movemask_ps(CMP) > 0;
|
|
}
|
|
|
|
/** @brief four spheres versus four AABBs SIMD test */
|
|
static bool four_spheres_four_AABBs_intersect_and(const Vector3d& o1, FCL_REAL r1,
|
|
const Vector3d& o2, FCL_REAL r2,
|
|
const Vector3d& o3, FCL_REAL r3,
|
|
const Vector3d& o4, FCL_REAL r4,
|
|
const Vector3d& min1, const Vector3d& max1,
|
|
const Vector3d& min2, const Vector3d& max2,
|
|
const Vector3d& min3, const Vector3d& max3,
|
|
const Vector3d& min4, const Vector3d& max4)
|
|
{
|
|
__m128 CMP = four_spheres_four_AABBs_intersect(o1, r1, o2, r2, o3, r3, o4, r4, min1, max1, min2, max2, min3, max3, min4, max4);
|
|
return _mm_movemask_ps(CMP) == 15.f;
|
|
}
|
|
|
|
static bool four_spheres_four_AABBs_intersect_or(const Vector3d& o1, FCL_REAL r1,
|
|
const Vector3d& o2, FCL_REAL r2,
|
|
const Vector3d& o3, FCL_REAL r3,
|
|
const Vector3d& o4, FCL_REAL r4,
|
|
const Vector3d& min1, const Vector3d& max1,
|
|
const Vector3d& min2, const Vector3d& max2,
|
|
const Vector3d& min3, const Vector3d& max3,
|
|
const Vector3d& min4, const Vector3d& max4)
|
|
{
|
|
__m128 CMP = four_spheres_four_AABBs_intersect(o1, r1, o2, r2, o3, r3, o4, r4, min1, max1, min2, max2, min3, max3, min4, max4);
|
|
return _mm_movemask_ps(CMP) > 0;
|
|
}
|
|
|
|
/** @brief four AABBs versus four AABBs SIMD test */
|
|
static bool four_AABBs_intersect_and(const Vector3d& min1, const Vector3d& max1,
|
|
const Vector3d& min2, const Vector3d& max2,
|
|
const Vector3d& min3, const Vector3d& max3,
|
|
const Vector3d& min4, const Vector3d& max4,
|
|
const Vector3d& min5, const Vector3d& max5,
|
|
const Vector3d& min6, const Vector3d& max6,
|
|
const Vector3d& min7, const Vector3d& max7,
|
|
const Vector3d& min8, const Vector3d& max8)
|
|
{
|
|
__m128 CMP = four_AABBs_intersect(min1, max1, min2, max2, min3, max3, min4, max4, min5, max5, min6, max6, min7, max7, min8, max8);
|
|
return _mm_movemask_ps(CMP) == 15.f;
|
|
}
|
|
|
|
static bool four_AABBs_intersect_or(const Vector3d& min1, const Vector3d& max1,
|
|
const Vector3d& min2, const Vector3d& max2,
|
|
const Vector3d& min3, const Vector3d& max3,
|
|
const Vector3d& min4, const Vector3d& max4,
|
|
const Vector3d& min5, const Vector3d& max5,
|
|
const Vector3d& min6, const Vector3d& max6,
|
|
const Vector3d& min7, const Vector3d& max7,
|
|
const Vector3d& min8, const Vector3d& max8)
|
|
{
|
|
__m128 CMP = four_AABBs_intersect(min1, max1, min2, max2, min3, max3, min4, max4, min5, max5, min6, max6, min7, max7, min8, max8);
|
|
return _mm_movemask_ps(CMP) > 0;
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|