line2d.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2015, JSK Lab
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/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab 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 
37 
38 namespace jsk_footstep_planner
39 {
40  Line2D::Line2D(const Eigen::Vector3f& p, const Eigen::Vector3f& q):
41  u_(p[0], p[1]), v_(q[0], q[1])
42  {
43  }
44 
46  {
47  const float d = ((v_[0] - u_[0]) * (other.v_[1] - other.u_[1])
48  - (v_[1] - u_[1]) * (other.v_[0] - other.u_[0]));
49  if (d == 0) { // parallel
50  return false;
51  }
52  const float u = ((other.u_[0] - u_[0])*(other.v_[1] - other.u_[1])
53  - (other.u_[1] - u_[1])*(other.v_[0] - other.u_[0]))/d;
54  const float v = ((other.u_[0] - u_[0])*(v_[1] - u_[1])
55  - (other.u_[1] - u_[1])*(v_[0] - u_[0]))/d;
56  if (u < 0.0 || u > 1.0) {
57  return false;
58  }
59  if (v < 0.0 || v > 1.0) {
60  return false;
61  }
62  return true;
63  }
64 }
65 
d
virtual bool isCrossing(Line2D &other)
Definition: line2d.cpp:45
Eigen::Vector2f u_
Definition: line2d.h:52
Line2D(const Eigen::Vector3f &p, const Eigen::Vector3f &q)
Definition: line2d.cpp:40
Eigen::Vector2f v_
Definition: line2d.h:53
GLfloat v[8][3]


jsk_footstep_planner
Author(s): Ryohei Ueda
autogenerated on Thu Nov 14 2019 03:53:28