MathUtilities.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #ifndef SURGSIM_TESTING_MATHUTILITIES_H
17 #define SURGSIM_TESTING_MATHUTILITIES_H
18 
19 #include <utility>
20 
22 #include "SurgSim/Math/Vector.h"
24 
25 #include <Eigen/Geometry>
26 
27 namespace SurgSim
28 {
29 namespace Testing
30 {
37 template <class T>
38 T interpolate(const T& start, const T& end, const double& t)
39 {
40  return (1 - t) * start + t * end;
41 }
42 
43 template <class T>
44 T interpolate(const std::pair<T, T>& values, const double& t)
45 {
46  return interpolate<T>(values.first, values.second, t);
47 }
48 
55 template <>
56 SurgSim::Math::Quaterniond interpolate<SurgSim::Math::Quaterniond>(
57  const SurgSim::Math::Quaterniond& start,
58  const SurgSim::Math::Quaterniond& end,
59  const double& t);
60 
67 template <>
68 SurgSim::Math::RigidTransform3d interpolate<SurgSim::Math::RigidTransform3d>(
71  const double& t);
72 
83  const SurgSim::Math::Vector3d& startAngles,
84  const SurgSim::Math::Vector3d& endAngles,
85  const SurgSim::Math::Vector3d& startPosition,
86  const SurgSim::Math::Vector3d& endPosition,
87  const double& t);
88 
89 
90 }
91 }
92 
93 
94 namespace Eigen
95 {
96 
97 template<class T, int Dim>
98 ::std::ostream& operator<<(::std::ostream& os, const Eigen::AlignedBox<T, Dim>& box)
99 {
100  os << "[" << box.min().transpose() << ", " << box.max().transpose() << "]";
101  return os;
102 }
103 }
104 #endif
105 
106 
Eigen
Definition: MathUtilities.h:95
Vector.h
Definitions of small fixed-size vector types.
SurgSim::Math::RigidTransform3d
Eigen::Transform< double, 3, Eigen::Isometry > RigidTransform3d
A 3D rigid (isometric) transform, represented as doubles.
Definition: RigidTransform.h:46
Eigen::operator<<
::std::ostream & operator<<(::std::ostream &os, const Eigen::AlignedBox< T, Dim > &box)
Definition: MathUtilities.h:98
SurgSim::Math::Vector3d
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
SurgSim
Definition: CompoundShapeToGraphics.cpp:30
SurgSim::Testing::interpolate
SurgSim::Math::Quaterniond interpolate(const SurgSim::Math::Quaterniond &start, const SurgSim::Math::Quaterniond &end, const double &t)
Definition: MathUtilities.cpp:49
RigidTransform.h
Definitions of 2x2 and 3x3 rigid (isometric) transforms.
Quaternion.h
Definitions of quaternion types.
SurgSim::Math::Quaterniond
Eigen::Quaternion< double > Quaterniond
A quaternion of doubles.
Definition: Quaternion.h:38
SurgSim::Testing::interpolatePose
SurgSim::Math::RigidTransform3d interpolatePose(const Vector3d &startAngles, const Vector3d &endAngles, const Vector3d &startPosition, const Vector3d &endPosition, const double &t)
Definition: MathUtilities.cpp:32