Debian Codegraph @

.deprecated/simd/simd_intersect.h

raw
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2016, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /** \author Jia Pan */
37
38 #ifndef FCL_MULTIPLE_INTERSECT
39 #define FCL_MULTIPLE_INTERSECT
40
41 #include <xmmintrin.h>
42 #include <pmmintrin.h>
43
44
45 namespace fcl
46 {
47
48
49 static inline __m128 sse_four_spheres_intersect(const Vector3d& o1, FCL_REAL r1,
50 const Vector3d& o2, FCL_REAL r2,
51 const Vector3d& o3, FCL_REAL r3,
52 const Vector3d& o4, FCL_REAL r4,
53 const Vector3d& o5, FCL_REAL r5,
54 const Vector3d& o6, FCL_REAL r6,
55 const Vector3d& o7, FCL_REAL r7,
56 const Vector3d& o8, FCL_REAL r8)
57 {
58 __m128 PX, PY, PZ, PR, QX, QY, QZ, QR;
59 PX = _mm_set_ps(o1[0], o2[0], o3[0], o4[0]);
60 PY = _mm_set_ps(o1[1], o2[1], o3[1], o4[1]);
61 PZ = _mm_set_ps(o1[2], o2[2], o3[2], o4[2]);
62 PR = _mm_set_ps(r1, r2, r3, r4);
63 QX = _mm_set_ps(o5[0], o6[0], o7[0], o8[0]);
64 QY = _mm_set_ps(o5[1], o6[1], o7[1], o8[1]);
65 QZ = _mm_set_ps(o5[2], o6[2], o7[2], o8[2]);
66 QR = _mm_set_ps(r5, r6, r7, r8);
67
68 __m128 T1 = _mm_sub_ps(PX, QX);
69 __m128 T2 = _mm_sub_ps(PY, QY);
70 __m128 T3 = _mm_sub_ps(PZ, QZ);
71 __m128 T4 = _mm_add_ps(PR, QR);
72 T1 = _mm_mul_ps(T1, T1);
73 T2 = _mm_mul_ps(T2, T2);
74 T3 = _mm_mul_ps(T3, T3);
75 T4 = _mm_mul_ps(T4, T4);
76 T1 = _mm_add_ps(T1, T2);
77 T2 = _mm_sub_ps(R2, T3);
78 return _mm_cmple_ps(T1, T2);
79 }
80
81
82 static inline __m128 sse_four_spheres_four_AABBs_intersect(const Vector3d& o1, FCL_REAL r1,
83 const Vector3d& o2, FCL_REAL r2,
84 const Vector3d& o3, FCL_REAL r3,
85 const Vector3d& o4, FCL_REAL r4,
86 const Vector3d& min1, const Vector3d& max1,
87 const Vector3d& min2, const Vector3d& max2,
88 const Vector3d& min3, const Vector3d& max3,
89 const Vector3d& min4, const Vector3d& max4)
90 {
91 __m128 MINX, MINY, MINZ, MAXX, MAXX, MAXY, MAXZ, SX, SY, SZ, SR;
92 MINX = _mm_set_ps(min1[0], min2[0], min3[0], min4[0]);
93 MAXX = _mm_set_ps(max1[0], max2[0], max3[0], max4[0]);
94 MINY = _mm_set_ps(min1[1], min2[1], min3[1], min4[1]);
95 MAXY = _mm_set_ps(max1[1], max2[1], max3[1], max4[1]);
96 MINZ = _mm_set_ps(min1[2], min2[2], min3[2], min4[2]);
97 MAXZ = _mm_set_ps(max1[2], max2[2], max3[2], max4[2]);
98 SX = _mm_set_ps(o1[0], o2[0], o3[0], o4[0]);
99 SY = _mm_set_ps(o1[1], o2[1], o3[1], o4[1]);
100 SZ = _mm_set_ps(o1[2], o2[2], o3[2], o4[2]);
101 SR = _mm_set_ps(r1, r2, r3, r4);
102
103
104 __m128 TX = _mm_max_ps(SX, MINX);
105 __m128 TY = _mm_max_ps(SY, MINY);
106 __m128 TZ = _mm_max_ps(SZ, MINZ);
107 TX = _mm_min_ps(TX, MAXX);
108 TY = _mm_min_ps(TY, MAXY);
109 TZ = _mm_min_ps(TZ, MAXZ);
110 __m128 DX = _mm_sub_ps(SX, TX);
111 __m128 DY = _mm_sub_ps(SY, TY);
112 __m128 DZ = _mm_sub_ps(SZ, TZ);
113 __m128 R2 = _mm_mul_ps(SR, SR);
114 DX = _mm_mul_ps(DX, DX);
115 DY = _mm_mul_ps(DY, DY);
116 DZ = _mm_mul_ps(DZ, DZ);
117 __m128 T1 = _mm_add_ps(DX, DY);
118 __m128 T2 = _mm_sub_ps(R2, DZ);
119 return _mm_cmple_ps(T1, T2);
120 }
121
122
123 static inline __m128 sse_four_AABBs_intersect(const Vector3d& min1, const Vector3d& max1,
124 const Vector3d& min2, const Vector3d& max2,
125 const Vector3d& min3, const Vector3d& max3,
126 const Vector3d& min4, const Vector3d& max4,
127 const Vector3d& min5, const Vector3d& max5,
128 const Vector3d& min6, const Vector3d& max6,
129 const Vector3d& min7, const Vector3d& max7,
130 const Vector3d& min8, const Vector3d& max8)
131 {
132 __m128 MIN1X, MIN1Y, MIN1Z, MAX1X, MAX1Y, MAX1Z, MIN2X, MIN2Y, MIN2Z, MAX2X, MAX2Y, MAX2Z;
133 MIN1X = _mm_set_ps(min1[0], min2[0], min3[0], min4[0]);
134 MAX1X = _mm_set_ps(max1[0], max2[0], max3[0], max4[0]);
135 MIN1Y = _mm_set_ps(min1[1], min2[1], min3[1], min4[1]);
136 MAX1Y = _mm_set_ps(max1[1], max2[1], max3[1], max4[1]);
137 MIN1Z = _mm_set_ps(min1[2], min2[2], min3[2], min4[2]);
138 MAX1Z = _mm_set_ps(max1[2], max2[2], max3[2], max4[2]);
139 MIN2X = _mm_set_ps(min5[0], min6[0], min7[0], min8[0]);
140 MAX2X = _mm_set_ps(max5[0], max6[0], max7[0], max8[0]);
141 MIN2Y = _mm_set_ps(min5[1], min6[1], min7[1], min8[1]);
142 MAX2Y = _mm_set_ps(max5[1], max6[1], max7[1], max8[1]);
143 MIN2Z = _mm_set_ps(min5[2], min6[2], min7[2], min8[2]);
144 MAX2Z = _mm_set_ps(max5[2], max6[2], max7[2], max8[2]);
145
146 __m128 AX = _mm_max_ps(MIN1X, MIN2X);
147 __m128 BX = _mm_min_ps(MAX1X, MAX2X);
148 __m128 AY = _mm_max_ps(MIN1Y, MIN2Y);
149 __m128 BY = _mm_min_ps(MAX1Y, MAX2Y);
150 __m128 AZ = _mm_max_ps(MIN1Z, MIN2Z);
151 __m128 BZ = _mm_min_ps(MAX1Z, MAX2Z);
152 __m128 T1 = _mm_cmple_ps(AX, BX);
153 __m128 T2 = _mm_cmple_ps(AY, BY);
154 __m128 T3 = _mm_cmple_ps(AZ, BZ);
155 __m128 T4 = _mm_and_ps(T1, T2);
156 return _mm_and_ps(T3, T4);
157 }
158
159 static bool four_spheres_intersect_and(const Vector3d& o1, FCL_REAL r1,
160 const Vector3d& o2, FCL_REAL r2,
161 const Vector3d& o3, FCL_REAL r3,
162 const Vector3d& o4, FCL_REAL r4,
163 const Vector3d& o5, FCL_REAL r5,
164 const Vector3d& o6, FCL_REAL r6,
165 const Vector3d& o7, FCL_REAL r7,
166 const Vector3d& o8, FCL_REAL r8)
167 {
168 __m128 CMP = four_spheres_intersect(o1, r1, o2, r2, o3, r3, o4, r4, o5, r5, o6, r6, o7, r7, o8, r8);
169 return _mm_movemask_ps(CMP) == 15.f;
170 }
171
172 static bool four_spheres_intersect_or(const Vector3d& o1, FCL_REAL r1,
173 const Vector3d& o2, FCL_REAL r2,
174 const Vector3d& o3, FCL_REAL r3,
175 const Vector3d& o4, FCL_REAL r4,
176 const Vector3d& o5, FCL_REAL r5,
177 const Vector3d& o6, FCL_REAL r6,
178 const Vector3d& o7, FCL_REAL r7,
179 const Vector3d& o8, FCL_REAL r8)
180 {
181 __m128 CMP = four_spheres_intersect(o1, r1, o2, r2, o3, r3, o4, r4, o5, r5, o6, r6, o7, r7, o8, r8);
182 return __mm_movemask_ps(CMP) > 0;
183 }
184
185 /** @brief four spheres versus four AABBs SIMD test */
186 static bool four_spheres_four_AABBs_intersect_and(const Vector3d& o1, FCL_REAL r1,
187 const Vector3d& o2, FCL_REAL r2,
188 const Vector3d& o3, FCL_REAL r3,
189 const Vector3d& o4, FCL_REAL r4,
190 const Vector3d& min1, const Vector3d& max1,
191 const Vector3d& min2, const Vector3d& max2,
192 const Vector3d& min3, const Vector3d& max3,
193 const Vector3d& min4, const Vector3d& max4)
194 {
195 __m128 CMP = four_spheres_four_AABBs_intersect(o1, r1, o2, r2, o3, r3, o4, r4, min1, max1, min2, max2, min3, max3, min4, max4);
196 return _mm_movemask_ps(CMP) == 15.f;
197 }
198
199 static bool four_spheres_four_AABBs_intersect_or(const Vector3d& o1, FCL_REAL r1,
200 const Vector3d& o2, FCL_REAL r2,
201 const Vector3d& o3, FCL_REAL r3,
202 const Vector3d& o4, FCL_REAL r4,
203 const Vector3d& min1, const Vector3d& max1,
204 const Vector3d& min2, const Vector3d& max2,
205 const Vector3d& min3, const Vector3d& max3,
206 const Vector3d& min4, const Vector3d& max4)
207 {
208 __m128 CMP = four_spheres_four_AABBs_intersect(o1, r1, o2, r2, o3, r3, o4, r4, min1, max1, min2, max2, min3, max3, min4, max4);
209 return _mm_movemask_ps(CMP) > 0;
210 }
211
212 /** @brief four AABBs versus four AABBs SIMD test */
213 static bool four_AABBs_intersect_and(const Vector3d& min1, const Vector3d& max1,
214 const Vector3d& min2, const Vector3d& max2,
215 const Vector3d& min3, const Vector3d& max3,
216 const Vector3d& min4, const Vector3d& max4,
217 const Vector3d& min5, const Vector3d& max5,
218 const Vector3d& min6, const Vector3d& max6,
219 const Vector3d& min7, const Vector3d& max7,
220 const Vector3d& min8, const Vector3d& max8)
221 {
222 __m128 CMP = four_AABBs_intersect(min1, max1, min2, max2, min3, max3, min4, max4, min5, max5, min6, max6, min7, max7, min8, max8);
223 return _mm_movemask_ps(CMP) == 15.f;
224 }
225
226 static bool four_AABBs_intersect_or(const Vector3d& min1, const Vector3d& max1,
227 const Vector3d& min2, const Vector3d& max2,
228 const Vector3d& min3, const Vector3d& max3,
229 const Vector3d& min4, const Vector3d& max4,
230 const Vector3d& min5, const Vector3d& max5,
231 const Vector3d& min6, const Vector3d& max6,
232 const Vector3d& min7, const Vector3d& max7,
233 const Vector3d& min8, const Vector3d& max8)
234 {
235 __m128 CMP = four_AABBs_intersect(min1, max1, min2, max2, min3, max3, min4, max4, min5, max5, min6, max6, min7, max7, min8, max8);
236 return _mm_movemask_ps(CMP) > 0;
237 }
238
239 }
240
241 #endif