collision_checking: collision_checking: conservative

00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00037 #include "collision_checking/conservative_advancement.h"
00038 #include "collision_checking/intersect.h"
00039 #include "collision_checking/collision.h"
00040 
00042 namespace collision_checking
00043 {
00044 
00045 void InterpMotion::SimpleQuaternion::fromRotation(const Vec3f R[3])
00046 {
00047   const int next[3] = {1, 2, 0};
00048 
00049   BVH_REAL trace = R[0][0] + R[1][1] + R[2][2];
00050   BVH_REAL root;
00051 
00052   if(trace > 0.0)
00053   {
00054     // |w| > 1/2, may as well choose w > 1/2
00055     root = sqrt(trace + 1.0);  // 2w
00056     data[0] = 0.5 * root;
00057     root = 0.5 / root;  // 1/(4w)
00058     data[1] = (R[2][1] - R[1][2])*root;
00059     data[2] = (R[0][2] - R[2][0])*root;
00060     data[3] = (R[1][0] - R[0][1])*root;
00061   }
00062   else
00063   {
00064     // |w| <= 1/2
00065     int i = 0;
00066     if(R[1][1] > R[0][0])
00067     {
00068         i = 1;
00069     }
00070     if(R[2][2] > R[i][i])
00071     {
00072         i = 2;
00073     }
00074     int j = next[i];
00075     int k = next[j];
00076 
00077     root = sqrt(R[i][i] - R[j][j] - R[k][k] + 1.0);
00078     BVH_REAL* quat[3] = { &data[1], &data[2], &data[3] };
00079     *quat[i] = 0.5 * root;
00080     root = 0.5 / root;
00081     data[0] = (R[k][j] - R[j][k]) * root;
00082     *quat[j] = (R[j][i] + R[i][j]) * root;
00083     *quat[k] = (R[k][i] + R[i][k]) * root;
00084   }
00085 }
00086 
00087 void InterpMotion::SimpleQuaternion::toRotation(Vec3f R[3]) const
00088 {
00089   BVH_REAL twoX  = 2.0*data[1];
00090   BVH_REAL twoY  = 2.0*data[2];
00091   BVH_REAL twoZ  = 2.0*data[3];
00092   BVH_REAL twoWX = twoX*data[0];
00093   BVH_REAL twoWY = twoY*data[0];
00094   BVH_REAL twoWZ = twoZ*data[0];
00095   BVH_REAL twoXX = twoX*data[1];
00096   BVH_REAL twoXY = twoY*data[1];
00097   BVH_REAL twoXZ = twoZ*data[1];
00098   BVH_REAL twoYY = twoY*data[2];
00099   BVH_REAL twoYZ = twoZ*data[2];
00100   BVH_REAL twoZZ = twoZ*data[3];
00101 
00102   R[0] = Vec3f(1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY);
00103   R[1] = Vec3f(twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX);
00104   R[2] = Vec3f(twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY));
00105 }
00106 
00107 void InterpMotion::SimpleQuaternion::fromAxisAngle(const Vec3f& axis, BVH_REAL angle)
00108 {
00109   BVH_REAL half_angle = 0.5 * angle;
00110   BVH_REAL sn = sin((double)half_angle);
00111   data[0] = cos((double)half_angle);
00112   data[1] = sn * axis[0];
00113   data[2] = sn * axis[1];
00114   data[3] = sn * axis[2];
00115 }
00116 
00117 void InterpMotion::SimpleQuaternion::toAxisAngle(Vec3f& axis, BVH_REAL& angle) const
00118 {
00119   double sqr_length = data[1] * data[1] + data[2] * data[2] + data[3] * data[3];
00120   if(sqr_length > 0)
00121   {
00122     angle = 2.0 * acos((double)data[0]);
00123     double inv_length = 1.0 / sqrt(sqr_length);
00124     axis[0] = inv_length * data[1];
00125     axis[1] = inv_length * data[2];
00126     axis[2] = inv_length * data[3];
00127   }
00128   else
00129   {
00130     angle = 0;
00131     axis[0] = 1;
00132     axis[1] = 0;
00133     axis[2] = 0;
00134   }
00135 }
00136 
00137 BVH_REAL InterpMotion::SimpleQuaternion::dot(const SimpleQuaternion& other) const
00138 {
00139   return data[0] * other.data[0] + data[1] * other.data[1] + data[2] * other.data[2] + data[3] * other.data[3];
00140 }
00141 
00142 InterpMotion::SimpleQuaternion InterpMotion::SimpleQuaternion::operator + (const SimpleQuaternion& other) const
00143 {
00144   return SimpleQuaternion(data[0] + other.data[0], data[1] + other.data[1],
00145                           data[2] + other.data[2], data[3] + other.data[3]);
00146 }
00147 
00148 InterpMotion::SimpleQuaternion InterpMotion::SimpleQuaternion::operator - (const SimpleQuaternion& other) const
00149 {
00150   return SimpleQuaternion(data[0] - other.data[0], data[1] - other.data[1],
00151                           data[2] - other.data[2], data[3] - other.data[3]);
00152 }
00153 
00154 InterpMotion::SimpleQuaternion InterpMotion::SimpleQuaternion::operator * (const SimpleQuaternion& other) const
00155 {
00156   return SimpleQuaternion(data[0] * other.data[0] - data[1] * other.data[1] - data[2] * other.data[2] - data[3] * other.data[3],
00157                           data[0] * other.data[1] + data[1] * other.data[0] + data[2] * other.data[3] - data[3] * other.data[2],
00158                           data[0] * other.data[2] - data[1] * other.data[3] + data[2] * other.data[0] + data[3] * other.data[1],
00159                           data[0] * other.data[3] + data[1] * other.data[2] - data[2] * other.data[1] + data[3] * other.data[0]);
00160 }
00161 
00162 InterpMotion::SimpleQuaternion InterpMotion::SimpleQuaternion::operator - () const
00163 {
00164   return SimpleQuaternion(-data[0], -data[1], -data[2], -data[3]);
00165 }
00166 
00167 InterpMotion::SimpleQuaternion InterpMotion::SimpleQuaternion::operator * (BVH_REAL t) const
00168 {
00169   return SimpleQuaternion(data[0] * t, data[1] * t, data[2] * t, data[3] * t);
00170 }
00171 
00172 InterpMotion::SimpleQuaternion InterpMotion::SimpleQuaternion::conj() const
00173 {
00174   return SimpleQuaternion(data[0], -data[1], -data[2], -data[3]);
00175 }
00176 
00177 InterpMotion::SimpleQuaternion InterpMotion::SimpleQuaternion::inverse() const
00178 {
00179   double sqr_length = data[0] * data[0] + data[1] * data[1] + data[2] * data[2] + data[3] * data[3];
00180   if(sqr_length > 0)
00181   {
00182     double inv_length = 1.0 / sqrt(sqr_length);
00183     return SimpleQuaternion(data[0] * inv_length, -data[1] * inv_length, -data[2] * inv_length, -data[3] * inv_length);
00184   }
00185   else
00186   {
00187     return SimpleQuaternion(data[0], -data[1], -data[2], -data[3]);
00188   }
00189 }
00190 
00191 Vec3f InterpMotion::SimpleQuaternion::transform(const Vec3f& v) const
00192 {
00193   SimpleQuaternion r = (*this) * SimpleQuaternion(0, v[0], v[1], v[2]) * (this->conj());
00194   return Vec3f(r.data[1], r.data[2], r.data[3]);
00195 }
00196 
00197 BVH_REAL InterpMotion::computeMotionBound(const RSS& bv, const Vec3f& n) const
00198 {
00199   BVH_REAL c_proj_max = (bv.Tr.cross(angular_axis)).sqrLength();
00200   BVH_REAL tmp;
00201   tmp = ((bv.Tr + bv.axis[0] * bv.l[0]).cross(angular_axis)).sqrLength();
00202   if(tmp > c_proj_max) c_proj_max = tmp;
00203   tmp = ((bv.Tr + bv.axis[1] * bv.l[1]).cross(angular_axis)).sqrLength();
00204   if(tmp > c_proj_max) c_proj_max = tmp;
00205   tmp = ((bv.Tr + bv.axis[0] * bv.l[0] + bv.axis[1] * bv.l[1]).cross(angular_axis)).sqrLength();
00206   if(tmp > c_proj_max) c_proj_max = tmp;
00207 
00208   c_proj_max = sqrt(c_proj_max);
00209 
00210   BVH_REAL v_dot_n = linear_vel.dot(n);
00211   BVH_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
00212   BVH_REAL mu = v_dot_n + w_cross_n * (bv.r + c_proj_max);
00213 
00214   return mu;
00215 }
00216 
00217 BVH_REAL InterpMotion::computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
00218 {
00219   BVH_REAL c_proj_max = (a.cross(angular_axis)).sqrLength();
00220   BVH_REAL tmp;
00221   tmp = (b.cross(angular_axis)).sqrLength();
00222   if(tmp > c_proj_max) c_proj_max = tmp;
00223   tmp = (c.cross(angular_axis)).sqrLength();
00224   if(tmp > c_proj_max) c_proj_max = tmp;
00225 
00226   c_proj_max = sqrt(c_proj_max);
00227 
00228   BVH_REAL v_dot_n = linear_vel.dot(n);
00229   BVH_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
00230   BVH_REAL mu = v_dot_n + w_cross_n * c_proj_max;
00231 
00232   return mu;
00233 }
00234 
00235 
00236 void conservativeAdvancementRecurse(BVNode<RSS>* tree1, BVNode<RSS>* tree2,
00237                                     const Vec3f R[3], const Vec3f& T,
00238                                     int b1, int b2,
00239                                     Vec3f* vertices1, Vec3f* vertices2,
00240                                     Triangle* tri_indices1, Triangle* tri_indices2,
00241                                     BVH_CAResult* res, BVHFrontList* front_list)
00242 {
00243   BVNode<RSS>* node1 = tree1 + b1;
00244   BVNode<RSS>* node2 = tree2 + b2;
00245 
00246   bool l1 = node1->isLeaf();
00247   bool l2 = node2->isLeaf();
00248 
00249   if(l1 && l2)
00250   {
00251     if(front_list) front_list->push_back(BVHFrontNode(b1, b2));
00252 
00253     res->num_tri_tests++;
00254 
00255     const Triangle& tri_id1 = tri_indices1[-node1->first_child - 1];
00256     const Triangle& tri_id2 = tri_indices2[-node2->first_child - 1];
00257 
00258     const Vec3f& p1 = vertices1[tri_id1[0]];
00259     const Vec3f& p2 = vertices1[tri_id1[1]];
00260     const Vec3f& p3 = vertices1[tri_id1[2]];
00261 
00262     const Vec3f& q1 = vertices2[tri_id2[0]];
00263     const Vec3f& q2 = vertices2[tri_id2[1]];
00264     const Vec3f& q3 = vertices2[tri_id2[2]];
00265 
00266     // nearest point pair
00267     Vec3f P1, P2;
00268 
00269     BVH_REAL d = TriangleDistance::triDistance(p1, p2, p3, q1, q2, q3,
00270                                                R, T,
00271                                                P1, P2);
00272 
00273     if(d < res->distance)
00274     {
00275       res->distance = d;
00276 
00277       res->p1 = P1;
00278       res->p2 = P2;
00279 
00280       res->last_tri_id1 = -node1->first_child - 1;
00281       res->last_tri_id2 = -node2->first_child - 1;
00282     }
00283 
00284 
00286     Vec3f n = P2 - P1;
00288     Vec3f n_transformed = MxV(res->motion1.t.R, n);
00289     n_transformed.normalize();
00290     BVH_REAL bound1 = res->motion1.computeMotionBound(p1, p2, p3, n_transformed);
00291     BVH_REAL bound2 = res->motion2.computeMotionBound(q1, q2, q3, n_transformed);
00292 
00293     BVH_REAL delta_t = d / (bound1 + bound2);
00294 
00295     if(delta_t < res->delta_t)
00296       res->delta_t = delta_t;
00297   }
00298 
00299   BVH_REAL sz1 = node1->bv.size();
00300   BVH_REAL sz2 = node2->bv.size();
00301 
00302   int a1, a2, c1, c2;
00303 
00304   if(l2 || (!l1 && (sz1 > sz2)))
00305   {
00306     a1 = node1->first_child;
00307     a2 = b2;
00308     c1 = node1->first_child + 1;
00309     c2 = b2;
00310   }
00311   else
00312   {
00313     a1 = b1;
00314     a2 = node2->first_child;
00315     c1 = b1;
00316     c2 = node2->first_child + 1;
00317   }
00318 
00319   res->num_bv_tests += 2;
00320 
00321   // all in RSS1's local coordinate
00322   Vec3f P11, P12;
00323   Vec3f P21, P22;
00324 
00325   BVH_REAL d1 = distance(R, T, (tree1 + a1)->bv, (tree2 + a2)->bv, &P11, &P21);
00326   BVH_REAL d2 = distance(R, T, (tree1 + c1)->bv, (tree2 + c2)->bv, &P12, &P22);
00327 
00328 
00329   if(d2 < d1)
00330   {
00331     if((d2 < res->w * (res->distance - res->abs_err)) || (d2 * (1 + res->rel_err) < res->w * res->distance))
00332     {
00333       conservativeAdvancementRecurse(tree1, tree2, R, T, c1, c2, vertices1, vertices2, tri_indices1, tri_indices2, res, front_list);
00334     }
00335     else
00336     {
00338       Vec3f n = P22 - P12;
00340       Vec3f n_transformed = (tree1 + c1)->bv.axis[0] * n[0] + (tree1 + c1)->bv.axis[1] * n[1] + (tree1 + c1)->bv.axis[2] * n[2];
00341       n_transformed = MxV(R, n_transformed);
00342       n_transformed.normalize();
00343 
00344       BVH_REAL bound1 = res->motion1.computeMotionBound((tree1 + c1)->bv, n_transformed);
00345       BVH_REAL bound2 = res->motion2.computeMotionBound((tree2 + c2)->bv, n_transformed);
00346 
00347       BVH_REAL delta_t = d2 / (bound1 + bound2);
00348 
00349       if(delta_t < res->delta_t) res->delta_t = delta_t;
00350       if(front_list) front_list->push_back(BVHFrontNode(c1, c2));
00351     }
00352 
00353     if((d1 < res->w * (res->distance - res->abs_err)) || (d1 * (1 + res->rel_err) < res->w * res->distance))
00354     {
00355       conservativeAdvancementRecurse(tree1, tree2, R, T, a1, a2, vertices1, vertices2, tri_indices1, tri_indices2, res, front_list);
00356     }
00357     else
00358     {
00360       Vec3f n = P21 - P11;
00362       Vec3f n_transformed = (tree1 + a1)->bv.axis[0] * n[0] + (tree1 + a1)->bv.axis[1] * n[1] + (tree1 + a1)->bv.axis[2] * n[2];
00363       n_transformed = MxV(R, n_transformed);
00364       n_transformed.normalize();
00365 
00366       BVH_REAL bound1 = res->motion1.computeMotionBound((tree1 + a1)->bv, n_transformed);
00367       BVH_REAL bound2 = res->motion2.computeMotionBound((tree2 + a2)->bv, n_transformed);
00368 
00369       BVH_REAL delta_t = d1 / (bound1 + bound2);
00370 
00371       if(delta_t < res->delta_t) res->delta_t = delta_t;      if(delta_t < res->delta_t) res->delta_t = delta_t;
00372       if(front_list) front_list->push_back(BVHFrontNode(a1, a2));
00373     }
00374   }
00375   else
00376   {
00377     if((d1 < res->w * (res->distance - res->abs_err)) || (d1 * (1 + res->rel_err) < res->w * res->distance))
00378     {
00379       conservativeAdvancementRecurse(tree1, tree2, R, T, a1, a2, vertices1, vertices2, tri_indices1, tri_indices2, res, front_list);
00380     }
00381     else
00382     {
00384       Vec3f n = P21 - P11;
00386       Vec3f n_transformed = (tree1 + a1)->bv.axis[0] * n[0] + (tree1 + a1)->bv.axis[1] * n[1] + (tree1 + a1)->bv.axis[2] * n[2];
00387       n_transformed = MxV(R, n_transformed);
00388       n_transformed.normalize();
00389 
00390       BVH_REAL bound1 = res->motion1.computeMotionBound((tree1 + a1)->bv, n_transformed);
00391       BVH_REAL bound2 = res->motion2.computeMotionBound((tree2 + a2)->bv, n_transformed);
00392 
00393       BVH_REAL delta_t = d1 / (bound1 + bound2);
00394 
00395       if(delta_t < res->delta_t) res->delta_t = delta_t;
00396       if(front_list) front_list->push_back(BVHFrontNode(a1, a2));
00397     }
00398 
00399     if((d2 < res->w * (res->distance - res->abs_err)) || (d2 * (1 + res->rel_err) < res->w * res->distance))
00400     {
00401       conservativeAdvancementRecurse(tree1, tree2, R, T, c1, c2, vertices1, vertices2, tri_indices1, tri_indices2, res, front_list);
00402     }
00403     else
00404     {
00406       Vec3f n = P22 - P12;
00408       Vec3f n_transformed = (tree1 + c1)->bv.axis[0] * n[0] + (tree1 + c1)->bv.axis[1] * n[1] + (tree1 + c1)->bv.axis[2] * n[2];
00409       n_transformed = MxV(R, n_transformed);
00410       n_transformed.normalize();
00411 
00412       BVH_REAL bound1 = res->motion1.computeMotionBound((tree1 + c1)->bv, n_transformed);
00413       BVH_REAL bound2 = res->motion2.computeMotionBound((tree2 + c2)->bv, n_transformed);
00414 
00415       BVH_REAL delta_t = d2 / (bound1 + bound2);
00416 
00417       if(delta_t < res->delta_t) res->delta_t = delta_t;
00418       if(front_list) front_list->push_back(BVHFrontNode(c1, c2));
00419     }
00420   }
00421 }
00422 
00423 
00424 void continuousCollide_CA(const BVHModel<RSS>& model1, const Vec3f R1_1[3], const Vec3f& T1_1, const Vec3f R1_2[3], const Vec3f& T1_2,
00425                           const BVHModel<RSS>& model2, const Vec3f R2_1[3], const Vec3f& T2_1, const Vec3f R2_2[3], const Vec3f& T2_2,
00426                           BVH_CAResult* res, BVHFrontList* front_list)
00427 {
00428   res->motion1 = InterpMotion(R1_1, T1_1, R1_2, T1_2);
00429   res->motion2 = InterpMotion(R2_1, T2_1, R2_2, T2_2);
00430 
00431   BVH_CollideResult c_res;
00432   collide(model1, R1_1, T1_1, model2, R2_1, T2_1, &c_res);
00433 
00434   if(c_res.numPairs() > 0)
00435   {
00436     res->toc = 0;
00437     return;
00438   }
00439 
00440   do
00441   {
00442     Vec3f R1_t[3];
00443     Vec3f R2_t[3];
00444     Vec3f T1_t;
00445     Vec3f T2_t;
00446 
00447     res->motion1.getCurrentTransformation(R1_t, T1_t);
00448     res->motion2.getCurrentTransformation(R2_t, T2_t);
00449 
00450     // compute the transformation from 1 to 2
00451     Vec3f R1_col[3];
00452     R1_col[0] = Vec3f(R1_t[0][0], R1_t[1][0], R1_t[2][0]);
00453     R1_col[1] = Vec3f(R1_t[0][1], R1_t[1][1], R1_t[2][1]);
00454     R1_col[2] = Vec3f(R1_t[0][2], R1_t[1][2], R1_t[2][2]);
00455 
00456     Vec3f R2_col[3];
00457     R2_col[0] = Vec3f(R2_t[0][0], R2_t[1][0], R2_t[2][0]);
00458     R2_col[1] = Vec3f(R2_t[0][1], R2_t[1][1], R2_t[2][1]);
00459     R2_col[2] = Vec3f(R2_t[0][2], R2_t[1][2], R2_t[2][2]);
00460 
00461     Vec3f R[3];
00462     R[0] = Vec3f(R1_col[0].dot(R2_col[0]), R1_col[0].dot(R2_col[1]), R1_col[0].dot(R2_col[2]));
00463     R[1] = Vec3f(R1_col[1].dot(R2_col[0]), R1_col[1].dot(R2_col[1]), R1_col[1].dot(R2_col[2]));
00464     R[2] = Vec3f(R1_col[2].dot(R2_col[0]), R1_col[2].dot(R2_col[1]), R1_col[2].dot(R2_col[2]));
00465 
00466     Vec3f Ttemp = T2_t - T1_t;
00467     Vec3f T(R1_col[0].dot(Ttemp), R1_col[1].dot(Ttemp), R1_col[2].dot(Ttemp));
00468 
00469     res->delta_t = 1;
00470     res->distance = std::numeric_limits<BVH_REAL>::max();
00471     conservativeAdvancementRecurse(model1.bvs, model2.bvs, R, T, 0, 0, model1.vertices, model2.vertices, model1.tri_indices, model2.tri_indices, res, front_list);
00472 
00473     if(res->delta_t < res->t_err)
00474     {
00475       return;
00476     }
00477 
00478     res->toc += res->delta_t;
00479 
00480     if(res->toc > 1)
00481     {
00482       res->toc = 1;
00483       return;
00484     }
00485 
00486     res->motion1.integrate(res->toc);
00487     res->motion2.integrate(res->toc);
00488   }
00489   while(1);
00490 }
00491 
00492 }
conservative_advancement.cpp
