rotation_cache.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018-2019, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <cmath>
31 #include <memory>
32 #include <utility>
33 #include <vector>
34 
37 
38 namespace planner_cspace
39 {
40 namespace planner_3d
41 {
43 {
44  size_t ser_size = 1;
45  for (int i = 0; i < 3; i++)
46  ser_size *= size[i];
47 
48  size_ = size;
49  ser_size_ = ser_size;
50 
51  c_.reset(new CyclicVecFloat<3, 2>[ser_size]);
52  r_.reset(new std::pair<float, float>[ser_size]);
53 }
54 
56  const float linear_resolution,
57  const float angular_resolution,
58  const int range)
59 {
60  const int angle = std::lround(M_PI * 2 / angular_resolution);
61 
62  pages_.resize(angle);
63  for (int i = 0; i < angle; i++)
64  {
65  Page& r = pages_[i];
66  r.reset(CyclicVecInt<3, 2>(range * 2 + 1, range * 2 + 1, angle));
67 
69 
70  for (d[0] = 0; d[0] <= range * 2; d[0]++)
71  {
72  for (d[1] = 0; d[1] <= range * 2; d[1]++)
73  {
74  for (d[2] = 0; d[2] < angle; d[2]++)
75  {
76  auto v = CyclicVecFloat<3, 2>(
77  (d[0] - range) * linear_resolution,
78  (d[1] - range) * linear_resolution,
79  d[2] * angular_resolution);
80  v.rotate(-i * angular_resolution);
81  r.motion(d) = v;
82 
83  const float sin_v = std::sin(v[2]);
84  const float cos_v = std::cos(v[2]);
85  const float r1 = v[1] + v[0] * cos_v / sin_v;
86  const float r2 = std::copysign(
87  std::sqrt(std::pow(v[0], 2) + std::pow(v[0] * cos_v / sin_v, 2)),
88  v[0] * sin_v);
89  r.radiuses(d) = std::pair<float, float>(r1, r2);
90  }
91  }
92  }
93  }
94 }
95 } // namespace planner_3d
96 } // namespace planner_cspace
d
std::unique_ptr< CyclicVecFloat< 3, 2 >[]> c_
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
std::unique_ptr< std::pair< float, float >[]> r_
void reset(const CyclicVecInt< 3, 2 > &size)
void reset(const float linear_resolution, const float angular_resolution, const int range)
CyclicVecFloat< 3, 2 > & motion(const CyclicVecInt< 3, 2 > &pos)
void rotate(const float ang)
Definition: cyclic_vec.h:268
std::pair< float, float > & radiuses(const CyclicVecInt< 3, 2 > &pos)


planner_cspace
Author(s): Atsushi Watanabe
autogenerated on Wed May 12 2021 02:20:42