From ec0ae33b8acae795aba691ee60d6715828ef71e3 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Fri, 30 Sep 2011 04:19:26 +0000
Subject: [PATCH] add screw motion in CA ccd.
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@40 253336fb-580f-4252-a368-f3cef5a2a82b
---
.../include/fcl/conservative_advancement.h | 11 +-
trunk/fcl/include/fcl/motion.h | 187 ++++++++++++++--
trunk/fcl/include/fcl/traversal_node_bvhs.h | 17 +-
trunk/fcl/src/conservative_advancement.cpp | 41 ++--
trunk/fcl/src/motion.cpp | 31 ++-
trunk/fcl/src/traversal_node_bvhs.cpp | 12 +-
.../test_core_conservative_advancement.cpp | 199 +++++++++++++++---
7 files changed, 415 insertions(+), 83 deletions(-)
diff --git a/trunk/fcl/include/fcl/conservative_advancement.h b/trunk/fcl/include/fcl/conservative_advancement.h
index fdaa8e65..0fe7b2d3 100644
--- a/trunk/fcl/include/fcl/conservative_advancement.h
+++ b/trunk/fcl/include/fcl/conservative_advancement.h
@@ -41,20 +41,17 @@
#include "fcl/vec_3f.h"
#include "fcl/collision_object.h"
#include "fcl/collision_data.h"
+#include "fcl/motion_base.h"
namespace fcl
{
-
+template<typename BV>
int conservativeAdvancement(const CollisionObject* o1,
- const Vec3f R1_1[3], const Vec3f& T1_1,
- const Vec3f R1_2[3], const Vec3f& T1_2,
- const Vec3f& O1,
+ MotionBase<BV>* motion1,
const CollisionObject* o2,
- const Vec3f R2_1[3], const Vec3f& T2_1,
- const Vec3f R2_2[3], const Vec3f& T2_2,
- const Vec3f& O2,
+ MotionBase<BV>* motion2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts,
BVH_REAL& toc);
diff --git a/trunk/fcl/include/fcl/motion.h b/trunk/fcl/include/fcl/motion.h
index 6d7ae88e..0df7beaf 100644
--- a/trunk/fcl/include/fcl/motion.h
+++ b/trunk/fcl/include/fcl/motion.h
@@ -46,6 +46,165 @@
namespace fcl
{
+template<typename BV>
+class ScrewMotion : public MotionBase<BV>
+{
+public:
+ /** Default transformations are all identities */
+ ScrewMotion()
+ {
+ /** Default angular velocity is zero */
+ axis = Vec3f(1, 0, 0);
+ angular_vel = 0;
+
+ /** Default reference point is local zero point */
+
+ /** Default linear velocity is zero */
+ linear_vel = 0;
+ }
+
+ /** \brief Construct motion from the initial rotation/translation and goal rotation/translation */
+ ScrewMotion(const Vec3f R1[3], const Vec3f& T1,
+ const Vec3f R2[3], const Vec3f& T2)
+ {
+ t1 = SimpleTransform(R1, T1);
+ t2 = SimpleTransform(R2, T2);
+
+ /** Current time is zero, so the transformation is t1 */
+ t = t1;
+
+ computeScrewParameter();
+ }
+
+ /** \brief Integrate the motion from 0 to dt
+ * We compute the current transformation from zero point instead of from last integrate time, for precision.
+ */
+ bool integrate(double dt)
+ {
+ if(dt > 1) dt = 1;
+
+ t.setQuatRotation(absoluteRotation(dt));
+ t.setTranslation(p + axis * (dt * linear_vel) - t.getQuatRotation().transform(p));
+
+ return true;
+ }
+
+ /** \brief Compute the motion bound for a bounding volume along a given direction n
+ * For general BV, not implemented so return trivial 0
+ */
+ BVH_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
+
+ BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
+ {
+ BVH_REAL proj_max = ((t1.getQuatRotation().transform(a) + t1.getTranslation() - p).cross(axis)).sqrLength();
+ BVH_REAL tmp;
+ tmp = ((t1.getQuatRotation().transform(b) + t1.getTranslation() - p).cross(axis)).sqrLength();
+ if(tmp > proj_max) proj_max = tmp;
+ tmp = ((t1.getQuatRotation().transform(c) + t1.getTranslation() - p).cross(axis)).sqrLength();
+ if(tmp > proj_max) proj_max = tmp;
+
+ proj_max = sqrt(proj_max);
+
+ BVH_REAL v_dot_n = axis.dot(n) * linear_vel;
+ BVH_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
+ BVH_REAL mu = v_dot_n + w_cross_n * proj_max;
+
+ return mu;
+ }
+
+ /** \brief Get the rotation and translation in current step */
+ void getCurrentTransform(Vec3f R[3], Vec3f& T) const
+ {
+ for(int i = 0; i < 3; ++i)
+ {
+ R[i] = t.getRotation()[i];
+ }
+
+ T = t.getTranslation();
+ }
+
+ void getCurrentRotation(Vec3f R[3]) const
+ {
+ for(int i = 0; i < 3; ++i)
+ R[i] = t.getRotation()[i];
+ }
+
+ void getCurrentTranslation(Vec3f& T) const
+ {
+ T = t.getTranslation();
+ }
+
+protected:
+ void computeScrewParameter()
+ {
+ SimpleQuaternion deltaq = t2.getQuatRotation() * t1.getQuatRotation().inverse();
+ deltaq.toAxisAngle(axis, angular_vel);
+ if(angular_vel < 0)
+ {
+ angular_vel = -angular_vel;
+ axis = -axis;
+ }
+
+ if(angular_vel < 1e-10)
+ {
+ angular_vel = 0;
+ axis = t2.getTranslation() - t1.getTranslation();
+ linear_vel = axis.length();
+ p = t1.getTranslation();
+ }
+ else
+ {
+ Vec3f o = t2.getTranslation() - t1.getTranslation();
+ p = (t1.getTranslation() + t2.getTranslation() + axis.cross(o) * (1.0 / tan(angular_vel / 2.0))) * 0.5;
+ linear_vel = o.dot(axis);
+ }
+ }
+
+ SimpleQuaternion deltaRotation(BVH_REAL dt) const
+ {
+ SimpleQuaternion res;
+ res.fromAxisAngle(axis, (BVH_REAL)(dt * angular_vel));
+ return res;
+ }
+
+ SimpleQuaternion absoluteRotation(BVH_REAL dt) const
+ {
+ SimpleQuaternion delta_t = deltaRotation(dt);
+ return delta_t * t1.getQuatRotation();
+ }
+
+ /** \brief The transformation at time 0 */
+ SimpleTransform t1;
+
+ /** \brief The transformation at time 1 */
+ SimpleTransform t2;
+
+ /** \brief The transformation at current time t */
+ SimpleTransform t;
+
+ /** \brief screw axis */
+ Vec3f axis;
+
+ /** \brief A point on the axis S */
+ Vec3f p;
+
+ /** \brief linear velocity along the axis */
+ BVH_REAL linear_vel;
+
+ /** \brief angular velocity */
+ BVH_REAL angular_vel;
+};
+
+
+/** \brief Compute the motion bound for a bounding volume along a given direction n
+ * according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized)
+ * and ci are the endpoints of the generator primitives of RSS.
+ * Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
+ */
+template<>
+BVH_REAL ScrewMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const;
+
+
/** \brief Linear interpolation motion
* Each Motion is assumed to have constant linear velocity and angular velocity
* The motion is R(t)(p - p_ref) + p_ref + T(t)
@@ -57,7 +216,7 @@ template<typename BV>
class InterpMotion : public MotionBase<BV>
{
public:
- /** Default transformations are all identities */
+ /** \brief Default transformations are all identities */
InterpMotion()
{
/** Default angular velocity is zero */
@@ -121,24 +280,24 @@ public:
BVH_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
/** \brief Compute the motion bound for a triangle along a given direction n
- * according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||ci x w||. w is the angular axis (normalized)
+ * according to mu < |v * n| + ||w x n||(max||ci*||) where ||ci*|| = ||R0(ci) x w|| / \|w\|. w is the angular velocity
* and ci are the triangle vertex coordinates.
* Notice that the triangle is in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
*/
BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
{
- BVH_REAL c_proj_max = ((a - reference_p).cross(angular_axis)).sqrLength();
+ BVH_REAL proj_max = ((t1.getQuatRotation().transform(a - reference_p)).cross(angular_axis)).sqrLength();
BVH_REAL tmp;
- tmp = ((b - reference_p).cross(angular_axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((c - reference_p).cross(angular_axis)).sqrLength();
- if(tmp > c_proj_max) c_proj_max = tmp;
+ tmp = ((t1.getQuatRotation().transform(b - reference_p)).cross(angular_axis)).sqrLength();
+ if(tmp > proj_max) proj_max = tmp;
+ tmp = ((t1.getQuatRotation().transform(c - reference_p)).cross(angular_axis)).sqrLength();
+ if(tmp > proj_max) proj_max = tmp;
- c_proj_max = sqrt(c_proj_max);
+ proj_max = sqrt(proj_max);
BVH_REAL v_dot_n = linear_vel.dot(n);
BVH_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
- BVH_REAL mu = v_dot_n + w_cross_n * c_proj_max;
+ BVH_REAL mu = v_dot_n + w_cross_n * proj_max;
return mu;
}
@@ -180,16 +339,16 @@ protected:
}
- SimpleQuaternion deltaRotation(BVH_REAL t) const
+ SimpleQuaternion deltaRotation(BVH_REAL dt) const
{
SimpleQuaternion res;
- res.fromAxisAngle(angular_axis, (BVH_REAL)(t * angular_vel));
+ res.fromAxisAngle(angular_axis, (BVH_REAL)(dt * angular_vel));
return res;
}
- SimpleQuaternion absoluteRotation(BVH_REAL t) const
+ SimpleQuaternion absoluteRotation(BVH_REAL dt) const
{
- SimpleQuaternion delta_t = deltaRotation(t);
+ SimpleQuaternion delta_t = deltaRotation(dt);
return delta_t * t1.getQuatRotation();
}
@@ -217,7 +376,7 @@ protected:
/** \brief Compute the motion bound for a bounding volume along a given direction n
- * according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||ci x w||. w is the angular axis (normalized)
+ * according to mu < |v * n| + ||w x n||(r + max(||ci*||)) where ||ci*|| = ||R0(ci) x w||. w is the angular axis (normalized)
* and ci are the endpoints of the generator primitives of RSS.
* Notice that all bv parameters are in the local frame of the object, but n should be in the global frame (the reason is that the motion (t1, t2 and t) is in global frame)
*/
diff --git a/trunk/fcl/include/fcl/traversal_node_bvhs.h b/trunk/fcl/include/fcl/traversal_node_bvhs.h
index 84fa3061..6cdfd7a5 100644
--- a/trunk/fcl/include/fcl/traversal_node_bvhs.h
+++ b/trunk/fcl/include/fcl/traversal_node_bvhs.h
@@ -951,8 +951,8 @@ public:
last_tri_id1 = 0;
last_tri_id2 = 0;
- rel_err = 0.01;
- abs_err = 0.01;
+ rel_err = 0;
+ abs_err = 0;
min_distance = std::numeric_limits<BVH_REAL>::max();
}
@@ -1061,7 +1061,7 @@ public:
{
delta_t = 1;
toc = 0;
- t_err = (BVH_REAL)0.001;
+ t_err = (BVH_REAL)0;
w = w_;
@@ -1126,9 +1126,10 @@ public:
BVH_REAL bound2 = motion2->computeMotionBound(q1, q2, q3, n);
BVH_REAL bound = bound1 + bound2;
- if(bound < d) bound = d;
- BVH_REAL cur_delta_t = d / bound;
+ BVH_REAL cur_delta_t;
+ if(bound <= d) cur_delta_t = 1;
+ else cur_delta_t = d / bound;
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
@@ -1166,9 +1167,11 @@ public:
BVH_REAL bound2 = motion2->computeMotionBound((this->tree2 + c2)->bv, n);
BVH_REAL bound = bound1 + bound2;
- if(bound < c) bound = c;
- BVH_REAL cur_delta_t = c / bound;
+ BVH_REAL cur_delta_t;
+ if(bound <= c) cur_delta_t = 1;
+ else cur_delta_t = c / bound;
+
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
diff --git a/trunk/fcl/src/conservative_advancement.cpp b/trunk/fcl/src/conservative_advancement.cpp
index 2a167bc0..9e7e1fb9 100644
--- a/trunk/fcl/src/conservative_advancement.cpp
+++ b/trunk/fcl/src/conservative_advancement.cpp
@@ -46,14 +46,11 @@
namespace fcl
{
+template<typename BV>
int conservativeAdvancement(const CollisionObject* o1,
- const Vec3f R1_1[3], const Vec3f& T1_1,
- const Vec3f R1_2[3], const Vec3f& T1_2,
- const Vec3f& O1,
+ MotionBase<BV>* motion1,
const CollisionObject* o2,
- const Vec3f R2_1[3], const Vec3f& T2_1,
- const Vec3f R2_2[3], const Vec3f& T2_2,
- const Vec3f& O2,
+ MotionBase<BV>* motion2,
int num_max_contacts, bool exhaustive, bool enable_contact,
std::vector<Contact>& contacts,
BVH_REAL& toc)
@@ -85,7 +82,16 @@ int conservativeAdvancement(const CollisionObject* o1,
if(!initialize(cnode, *model1, *model2))
std::cout << "initialize error" << std::endl;
- relativeTransform(R1_1, T1_1, R2_1, T2_1, cnode.R, cnode.T);
+
+ Vec3f R1_0[3];
+ Vec3f R2_0[3];
+ Vec3f T1_0;
+ Vec3f T2_0;
+
+ motion1->getCurrentTransform(R1_0, T1_0);
+ motion2->getCurrentTransform(R2_0, T2_0);
+
+ relativeTransform(R1_0, T1_0, R2_0, T2_0, cnode.R, cnode.T);
cnode.enable_statistics = false;
cnode.num_max_contacts = num_max_contacts;
@@ -106,13 +112,10 @@ int conservativeAdvancement(const CollisionObject* o1,
MeshConservativeAdvancementTraversalNodeRSS node;
- initialize(node, *model1, *model2, R1_1, T1_1, R2_1, T2_1);
-
- InterpMotion<RSS> motion1(R1_1, T1_1, R1_2, T1_2, O1);
- InterpMotion<RSS> motion2(R2_1, T2_1, R2_2, T2_2, O2);
+ initialize(node, *model1, *model2, R1_0, T1_0, R2_0, T2_0);
- node.motion1 = &motion1;
- node.motion2 = &motion2;
+ node.motion1 = motion1;
+ node.motion2 = motion2;
do
@@ -133,7 +136,7 @@ int conservativeAdvancement(const CollisionObject* o1,
distanceRecurse(&node, 0, 0, NULL);
- if(node.delta_t < node.t_err)
+ if(node.delta_t <= node.t_err)
{
// std::cout << node.delta_t << " " << node.t_err << std::endl;
break;
@@ -160,4 +163,14 @@ int conservativeAdvancement(const CollisionObject* o1,
return 0;
}
+
+template int conservativeAdvancement<RSS>(const CollisionObject* o1,
+ MotionBase<RSS>* motion1,
+ const CollisionObject* o2,
+ MotionBase<RSS>* motion2,
+ int num_max_contacts, bool exhaustive, bool enable_contact,
+ std::vector<Contact>& contacts,
+ BVH_REAL& toc);
+
+
}
diff --git a/trunk/fcl/src/motion.cpp b/trunk/fcl/src/motion.cpp
index cc20b258..6dffab08 100644
--- a/trunk/fcl/src/motion.cpp
+++ b/trunk/fcl/src/motion.cpp
@@ -43,13 +43,13 @@ namespace fcl
template<>
BVH_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
{
- BVH_REAL c_proj_max = ((bv.Tr - reference_p).cross(angular_axis)).sqrLength();
+ BVH_REAL c_proj_max = ((t1.getQuatRotation().transform(bv.Tr - reference_p)).cross(angular_axis)).sqrLength();
BVH_REAL tmp;
- tmp = ((bv.Tr + bv.axis[0] * bv.l[0] - reference_p).cross(angular_axis)).sqrLength();
+ tmp = ((t1.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] - reference_p)).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((bv.Tr + bv.axis[1] * bv.l[1] - reference_p).cross(angular_axis)).sqrLength();
+ tmp = ((t1.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
- tmp = ((bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p).cross(angular_axis)).sqrLength();
+ tmp = ((t1.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1] - reference_p)).cross(angular_axis)).sqrLength();
if(tmp > c_proj_max) c_proj_max = tmp;
c_proj_max = sqrt(c_proj_max);
@@ -61,4 +61,27 @@ BVH_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) co
return mu;
}
+template<>
+BVH_REAL ScrewMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const
+{
+ BVH_REAL c_proj_max = ((t1.getQuatRotation().transform(bv.Tr)).cross(axis)).sqrLength();
+ BVH_REAL tmp;
+ tmp = ((t1.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0])).cross(axis)).sqrLength();
+ if(tmp > c_proj_max) c_proj_max = tmp;
+ tmp = ((t1.getQuatRotation().transform(bv.Tr + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
+ if(tmp > c_proj_max) c_proj_max = tmp;
+ tmp = ((t1.getQuatRotation().transform(bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1])).cross(axis)).sqrLength();
+ if(tmp > c_proj_max) c_proj_max = tmp;
+
+ c_proj_max = sqrt(c_proj_max);
+
+ BVH_REAL v_dot_n = axis.dot(n) * linear_vel;
+ BVH_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
+ BVH_REAL origin_proj = ((t1.getTranslation() - p).cross(axis)).length();
+
+ BVH_REAL mu = v_dot_n + w_cross_n * (c_proj_max + bv.r + origin_proj);
+
+ return mu;
+}
+
}
diff --git a/trunk/fcl/src/traversal_node_bvhs.cpp b/trunk/fcl/src/traversal_node_bvhs.cpp
index 1e1ed237..f8a27870 100644
--- a/trunk/fcl/src/traversal_node_bvhs.cpp
+++ b/trunk/fcl/src/traversal_node_bvhs.cpp
@@ -422,9 +422,11 @@ bool MeshConservativeAdvancementTraversalNode<OBB>::canStop(BVH_REAL c) const
BVH_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
BVH_REAL bound = bound1 + bound2;
- if(bound < c) bound = c;
- BVH_REAL cur_delta_t = c / bound;
+ BVH_REAL cur_delta_t;
+ if(bound <= c) cur_delta_t = 1;
+ else cur_delta_t = c / bound;
+
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
@@ -480,9 +482,11 @@ bool MeshConservativeAdvancementTraversalNode<RSS>::canStop(BVH_REAL c) const
BVH_REAL bound2 = motion2->computeMotionBound(this->model2->getBV(c2).bv, n_transformed);
BVH_REAL bound = bound1 + bound2;
- if(bound < c) bound = c;
- BVH_REAL cur_delta_t = c / bound;
+ BVH_REAL cur_delta_t;
+ if(bound <= c) cur_delta_t = 1;
+ else cur_delta_t = c / bound;
+
if(cur_delta_t < delta_t)
delta_t = cur_delta_t;
diff --git a/trunk/fcl/test/test_core_conservative_advancement.cpp b/trunk/fcl/test/test_core_conservative_advancement.cpp
index bf718d5f..71141cf5 100644
--- a/trunk/fcl/test/test_core_conservative_advancement.cpp
+++ b/trunk/fcl/test/test_core_conservative_advancement.cpp
@@ -43,22 +43,35 @@
using namespace fcl;
-bool CA_ccd_Test(const Transform& tf1, const Transform& tf2,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
- SplitMethodType split_method,
- bool use_COM,
- BVH_REAL& toc);
-
-bool interp_ccd_Test(const Transform& tf1, const Transform& tf2,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
- SplitMethodType split_method,
- unsigned int nsamples,
- bool use_COM,
- BVH_REAL& toc);
-
-unsigned int n_dcd_samples = 10;
+bool CA_linear_Test(const Transform& tf1, const Transform& tf2,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+ SplitMethodType split_method,
+ bool use_COM,
+ BVH_REAL& toc);
+
+bool CA_screw_Test(const Transform& tf1, const Transform& tf2,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+ SplitMethodType split_method,
+ BVH_REAL& toc);
+
+bool linear_interp_Test(const Transform& tf1, const Transform& tf2,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+ SplitMethodType split_method,
+ unsigned int nsamples,
+ bool use_COM,
+ BVH_REAL& toc);
+
+bool screw_interp_Test(const Transform& tf1, const Transform& tf2,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+ SplitMethodType split_method,
+ unsigned int nsamples,
+ BVH_REAL& toc);
+
+unsigned int n_dcd_samples = 100;
int main()
{
@@ -70,7 +83,7 @@ int main()
std::vector<Transform> transforms; // t0
std::vector<Transform> transforms2; // t1
BVH_REAL extents[] = {-3000, -3000, 0, 3000, 3000, 3000};
- BVH_REAL delta_trans[] = {10, 10, 10};
+ BVH_REAL delta_trans[] = {100, 100, 100};
int n = 100;
generateRandomTransform(extents, transforms, transforms2, delta_trans, 0.005 * 2 * 3.1415, n);
@@ -79,25 +92,33 @@ int main()
{
std::cout << i << std::endl;
BVH_REAL toc;
- bool res = CA_ccd_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, toc);
+ bool res = CA_linear_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, false, toc);
BVH_REAL toc2;
- bool res2 = CA_ccd_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, true, toc2);
+ bool res2 = CA_linear_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, true, toc2);
BVH_REAL toc3;
- bool res3 = interp_ccd_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, n_dcd_samples, false, toc3);
+ bool res3 = linear_interp_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, n_dcd_samples, false, toc3);
BVH_REAL toc4;
- bool res4 = interp_ccd_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, n_dcd_samples, true, toc4);
+ bool res4 = linear_interp_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, n_dcd_samples, true, toc4);
+
+ BVH_REAL toc5;
+ bool res5 = CA_screw_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, toc5);
+
+ BVH_REAL toc6;
+ bool res6 = screw_interp_Test(transforms[i], transforms2[i], p1, t1, p2, t2, SPLIT_METHOD_MEDIAN, n_dcd_samples, toc6);
if(res) std::cout << "yes "; else std::cout << "no ";
if(res2) std::cout << "yes "; else std::cout << "no ";
if(res3) std::cout << "yes "; else std::cout << "no ";
if(res4) std::cout << "yes "; else std::cout << "no ";
+ if(res5) std::cout << "yes "; else std::cout << "no ";
+ if(res6) std::cout << "yes "; else std::cout << "no ";
std::cout << std::endl;
- std::cout << toc << " " << toc2 << " " << toc3 << " " << toc4 << std::endl;
+ std::cout << toc << " " << toc2 << " " << toc3 << " " << toc4 << " " << toc5 << " " << toc6 << std::endl;
std::cout << std::endl;
}
@@ -106,12 +127,12 @@ int main()
}
-bool CA_ccd_Test(const Transform& tf1, const Transform& tf2,
- const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
- const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
- SplitMethodType split_method,
- bool use_COM,
- BVH_REAL& toc)
+bool CA_linear_Test(const Transform& tf1, const Transform& tf2,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+ SplitMethodType split_method,
+ bool use_COM,
+ BVH_REAL& toc)
{
BVHModel<RSS> m1;
BVHModel<RSS> m2;
@@ -149,15 +170,60 @@ bool CA_ccd_Test(const Transform& tf1, const Transform& tf2,
m2_ref *= (1.0 / vertices2.size());
}
- int b = conservativeAdvancement(&m1, tf1.R, tf1.T, tf2.R, tf2.T, m1_ref,
- &m2, R2, T2, R2, T2, m2_ref,
- 1, false, false, contacts, toc);
+ InterpMotion<RSS> motion1(tf1.R, tf1.T, tf2.R, tf2.T, m1_ref);
+ InterpMotion<RSS> motion2(R2, T2, R2, T2, m2_ref);
+
+ int b = conservativeAdvancement(&m1, &motion1,
+ &m2, &motion2,
+ 1, false, false, contacts, toc);
return (b > 0);
}
+bool CA_screw_Test(const Transform& tf1, const Transform& tf2,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+ SplitMethodType split_method,
+ BVH_REAL& toc)
+{
+ BVHModel<RSS> m1;
+ BVHModel<RSS> m2;
+
+ m1.bv_splitter.reset(new BVSplitter<RSS>(split_method));
+ m2.bv_splitter.reset(new BVSplitter<RSS>(split_method));
+
+ m1.beginModel();
+ m1.addSubModel(vertices1, triangles1);
+ m1.endModel();
-bool interp_ccd_Test(const Transform& tf1, const Transform& tf2,
+ m2.beginModel();
+ m2.addSubModel(vertices2, triangles2);
+ m2.endModel();
+
+ Vec3f R2[3];
+ Vec3f T2;
+ R2[0] = Vec3f(1, 0, 0);
+ R2[1] = Vec3f(0, 1, 0);
+ R2[2] = Vec3f(0, 0, 1);
+
+ std::vector<Contact> contacts;
+
+ Vec3f m1_ref;
+ Vec3f m2_ref;
+
+
+ ScrewMotion<RSS> motion1(tf1.R, tf1.T, tf2.R, tf2.T);
+ ScrewMotion<RSS> motion2(R2, T2, R2, T2);
+
+ int b = conservativeAdvancement(&m1, &motion1,
+ &m2, &motion2,
+ 1, false, false, contacts, toc);
+
+
+ return (b > 0);
+
+}
+bool linear_interp_Test(const Transform& tf1, const Transform& tf2,
const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
SplitMethodType split_method,
@@ -165,7 +231,6 @@ bool interp_ccd_Test(const Transform& tf1, const Transform& tf2,
bool use_COM,
BVH_REAL& toc)
{
-
BVHModel<RSS> m1;
BVHModel<RSS> m2;
@@ -232,5 +297,73 @@ bool interp_ccd_Test(const Transform& tf1, const Transform& tf2,
}
}
+ toc = 1;
+ return false;
+}
+
+bool screw_interp_Test(const Transform& tf1, const Transform& tf2,
+ const std::vector<Vec3f>& vertices1, const std::vector<Triangle>& triangles1,
+ const std::vector<Vec3f>& vertices2, const std::vector<Triangle>& triangles2,
+ SplitMethodType split_method,
+ unsigned int nsamples,
+ BVH_REAL& toc)
+{
+ BVHModel<RSS> m1;
+ BVHModel<RSS> m2;
+
+ m1.bv_splitter.reset(new BVSplitter<RSS>(split_method));
+ m2.bv_splitter.reset(new BVSplitter<RSS>(split_method));
+
+ m1.beginModel();
+ m1.addSubModel(vertices1, triangles1);
+ m1.endModel();
+
+ m2.beginModel();
+ m2.addSubModel(vertices2, triangles2);
+ m2.endModel();
+
+ Vec3f R2[3];
+ Vec3f T2;
+ R2[0] = Vec3f(1, 0, 0);
+ R2[1] = Vec3f(0, 1, 0);
+ R2[2] = Vec3f(0, 0, 1);
+
+ Vec3f m1_ref;
+ Vec3f m2_ref;
+
+
+ ScrewMotion<RSS> motion1(tf1.R, tf1.T, tf2.R, tf2.T);
+
+ for(unsigned int i = 0; i <= nsamples; ++i)
+ {
+ BVH_REAL curt = i / (BVH_REAL)nsamples;
+
+ Vec3f R[3];
+ Vec3f T;
+ motion1.integrate(curt);
+ motion1.getCurrentTransform(R, T);
+
+ m1.setTransform(R, T);
+ m2.setTransform(R2, T2);
+
+ MeshCollisionTraversalNodeRSS node;
+ if(!initialize(node, (const BVHModel<RSS>&)m1, (const BVHModel<RSS>&)m2))
+ std::cout << "initialize error" << std::endl;
+
+ node.enable_statistics = false;
+ node.num_max_contacts = 1;
+ node.exhaustive = false;
+ node.enable_contact = false;
+
+ collide(&node);
+
+ if(node.pairs.size() > 0)
+ {
+ toc = curt;
+ return true;
+ }
+ }
+
+ toc = 1;
return false;
}
--
GitLab