$search

conservative_advancement.h

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2011, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef COLLISION_CHECKING_CONSERVATIVE_ADVANCEMENT_H
#define COLLISION_CHECKING_CONSERVATIVE_ADVANCEMENT_H

#include <limits>
#include "collision_checking/BVH_defs.h"
#include "collision_checking/BVH_model.h"
#include "collision_checking/BVH_front.h"

namespace collision_checking
{

class InterpMotion
{
  struct SimpleQuaternion
  {
    SimpleQuaternion()
    {
      data[0] = 1;
      data[1] = 0;
      data[2] = 0;
      data[3] = 0;
    }

    SimpleQuaternion(BVH_REAL a, BVH_REAL b, BVH_REAL c, BVH_REAL d)
    {
      data[0] = a; // w
      data[1] = b; // x
      data[2] = c; // y
      data[3] = d; // z
    }

    void fromRotation(const Vec3f R[3]);

    void toRotation(Vec3f R[3]) const;

    void fromAxisAngle(const Vec3f& axis, BVH_REAL angle);

    void toAxisAngle(Vec3f& axis, BVH_REAL& angle) const;

    BVH_REAL dot(const SimpleQuaternion& other) const;

    SimpleQuaternion operator + (const SimpleQuaternion& other) const;

    SimpleQuaternion operator - (const SimpleQuaternion& other) const;

    SimpleQuaternion operator * (const SimpleQuaternion& other) const;

    SimpleQuaternion operator - () const;

    SimpleQuaternion operator * (BVH_REAL t) const;

    SimpleQuaternion conj() const;

    SimpleQuaternion inverse() const;

    Vec3f transform(const Vec3f& v) const;

    BVH_REAL data[4];
  };

  struct SimpleTransform
  {
    Vec3f R[3];
    Vec3f T;

    SimpleQuaternion q;

    SimpleTransform()
    {
      R[0][0] = 1; R[1][1] = 1; R[2][2] = 1;
    }

    SimpleTransform(const Vec3f R_[3], const Vec3f& T_)
    {
      for(int i = 0; i < 3; ++i)
        R[i] = R_[i];
      T = T_;

      q.fromRotation(R_);
    }
  };

public:
  InterpMotion()
  {
    angular_axis = Vec3f(1, 0, 0);
    angular_vel = 0;

  }

  InterpMotion(const Vec3f R1[3], const Vec3f& T1,
               const Vec3f R2[3], const Vec3f& T2)
  {
    t1 = SimpleTransform(R1, T1);
    t2 = SimpleTransform(R2, T2);

    t = t1;

    computeVelocity();
  }

  InterpMotion(const Vec3f R1[3], const Vec3f& T1,
               const Vec3f R2[3], const Vec3f& T2,
               const Vec3f& O)
  {
    t1 = SimpleTransform(R1, T1 - MxV(R1, O));
    t2 = SimpleTransform(R2, T2 - MxV(R2, O));
    t = t1;

    computeVelocity();
  }


  bool integrate(double dt)
  {
    if(dt > 1) dt = 1;

    t.T = t1.T + linear_vel * dt;

    t.q = absoluteRotation(dt);
    t.q.toRotation(t.R);

    return true;
  }

  BVH_REAL computeMotionBound(const RSS& bv, const Vec3f& n) const;

  BVH_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const;

  void getCurrentTransformation(Vec3f R[3], Vec3f& T) const
  {
    for(int i = 0; i < 3; ++i)
    {
      R[i] = t.R[i];
    }

    T = t.T;
  }

  void computeVelocity()
  {
    linear_vel = t2.T - t1.T;
    SimpleQuaternion deltaq = t2.q * t1.q.inverse();
    deltaq.toAxisAngle(angular_axis, angular_vel);
  }

  SimpleQuaternion deltaRotation(BVH_REAL t) const
  {
    SimpleQuaternion res;
    res.fromAxisAngle(angular_axis, (BVH_REAL)(t * angular_vel));
    return res;
  }

  SimpleQuaternion absoluteRotation(BVH_REAL t) const
  {
    SimpleQuaternion delta_t = deltaRotation(t);
    return delta_t * t1.q;
  }

  SimpleTransform t1;

  SimpleTransform t2;

  SimpleTransform t;

  Vec3f linear_vel;

  BVH_REAL angular_vel;

  Vec3f angular_axis;
};

struct BVH_CAResult
{
  int num_bv_tests;

  int num_tri_tests;

  BVH_REAL query_time_seconds;

  BVH_REAL rel_err;
  BVH_REAL abs_err;

  BVH_REAL distance;
  Vec3f p1, p2;

  int qsize;

  int last_tri_id1;
  int last_tri_id2;

  BVH_REAL w;

  BVH_REAL toc;
  BVH_REAL t_err;

  BVH_REAL delta_t;

  InterpMotion motion1;
  InterpMotion motion2;

  BVH_CAResult(BVH_REAL w_ = 1)
  {
    num_bv_tests = 0;
    num_tri_tests = 0;

    query_time_seconds = 0;

    /* default queue size is 2 */
    qsize = 2;

    /* default last_tris are the first triangle on each model */
    last_tri_id1 = 0;
    last_tri_id2 = 0;

    rel_err = (BVH_REAL)0.01;
    abs_err = (BVH_REAL)0.01;

    distance = std::numeric_limits<BVH_REAL>::max();

    delta_t = 1;
    toc = 0;
    t_err = (BVH_REAL)0.001;

    w = w_;
  }

  ~BVH_CAResult() {}

  void resetRecord()
  {
    num_bv_tests = 0;
    num_tri_tests = 0;

    delta_t = 1;
    toc = 0;
  }
};



void conservativeAdvancementRecurse(BVNode<RSS>* tree1, BVNode<RSS>* tree2,
                                    const Vec3f R[3], const Vec3f& T,
                                    int b1, int b2,
                                    Vec3f* vertices1, Vec3f* vertices2,
                                    Triangle* tri_indices1, Triangle* tri_indices2,
                                    BVH_CAResult* res, BVHFrontList* front_list = NULL);


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,
                          const BVHModel<RSS>& model2, const Vec3f R2_1[3], const Vec3f& T2_1, const Vec3f R2_2[3], const Vec3f& T2_2,
                          BVH_CAResult* res, BVHFrontList* front_list = NULL);


}

#endif

---

collision_checking
Author(s): Jia Pan, Dinesh Manocha (UNC, Chapel Hill)
autogenerated on Fri Mar 1 14:56:47 2013