All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines
Static Public Member Functions | Static Private Member Functions | Static Private Attributes
fcl::Intersect Class Reference

CCD intersect kernel among primitives. More...

#include <intersect.h>

List of all members.

Static Public Member Functions

static bool intersect_EE (const Vec3f &a0, const Vec3f &b0, const Vec3f &c0, const Vec3f &d0, const Vec3f &a1, const Vec3f &b1, const Vec3f &c1, const Vec3f &d1, FCL_REAL *collision_time, Vec3f *p_i, bool useNewton=true)
 CCD intersect between two edges [a0, b0] and [a1, b1] are points for one edge in time t0 and t1 [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1 p_i returns the coordinate of the collision point.
static bool intersect_EE_filtered (const Vec3f &a0, const Vec3f &b0, const Vec3f &c0, const Vec3f &d0, const Vec3f &a1, const Vec3f &b1, const Vec3f &c1, const Vec3f &d1, FCL_REAL *collision_time, Vec3f *p_i, bool useNewton=true)
 CCD intersect between two edges, using additional filter.
static bool intersect_Triangle (const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Vec3f &Q1, const Vec3f &Q2, const Vec3f &Q3, Vec3f *contact_points=NULL, unsigned int *num_contact_points=NULL, FCL_REAL *penetration_depth=NULL, Vec3f *normal=NULL)
 CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3].
static bool intersect_Triangle (const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Vec3f &Q1, const Vec3f &Q2, const Vec3f &Q3, const Matrix3f &R, const Vec3f &T, Vec3f *contact_points=NULL, unsigned int *num_contact_points=NULL, FCL_REAL *penetration_depth=NULL, Vec3f *normal=NULL)
static bool intersect_Triangle (const Vec3f &P1, const Vec3f &P2, const Vec3f &P3, const Vec3f &Q1, const Vec3f &Q2, const Vec3f &Q3, const Transform3f &tf, Vec3f *contact_points=NULL, unsigned int *num_contact_points=NULL, FCL_REAL *penetration_depth=NULL, Vec3f *normal=NULL)
static bool intersect_VE (const Vec3f &a0, const Vec3f &b0, const Vec3f &p0, const Vec3f &a1, const Vec3f &b1, const Vec3f &p1, const Vec3f &L)
 CCD intersect between one vertex and and one edge.
static bool intersect_VF (const Vec3f &a0, const Vec3f &b0, const Vec3f &c0, const Vec3f &p0, const Vec3f &a1, const Vec3f &b1, const Vec3f &c1, const Vec3f &p1, FCL_REAL *collision_time, Vec3f *p_i, bool useNewton=true)
 CCD intersect between one vertex and one face [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1 p0 and p1 are points for vertex in time t0 and t1 p_i returns the coordinate of the collision point.
static bool intersect_VF_filtered (const Vec3f &a0, const Vec3f &b0, const Vec3f &c0, const Vec3f &p0, const Vec3f &a1, const Vec3f &b1, const Vec3f &c1, const Vec3f &p1, FCL_REAL *collision_time, Vec3f *p_i, bool useNewton=true)
 CCD intersect between one vertex and one face, using additional filter.

Static Private Member Functions

static bool buildEdgePlane (const Vec3f &v1, const Vec3f &v2, const Vec3f &tn, Vec3f *n, FCL_REAL *t)
 build a plane pass through edge v1 and v2, normal is tn
static bool buildTrianglePlane (const Vec3f &v1, const Vec3f &v2, const Vec3f &v3, Vec3f *n, FCL_REAL *t)
 build a plane passed through triangle v1 v2 v3
static bool checkRootValidity_EE (const Vec3f &a0, const Vec3f &b0, const Vec3f &c0, const Vec3f &d0, const Vec3f &va, const Vec3f &vb, const Vec3f &vc, const Vec3f &vd, FCL_REAL t, Vec3f *q_i=NULL)
 Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time.
static bool checkRootValidity_VE (const Vec3f &a0, const Vec3f &b0, const Vec3f &p0, const Vec3f &va, const Vec3f &vb, const Vec3f &vp, FCL_REAL t)
 Check whether a root for VE intersection is valid.
static bool checkRootValidity_VF (const Vec3f &a0, const Vec3f &b0, const Vec3f &c0, const Vec3f &p0, const Vec3f &va, const Vec3f &vb, const Vec3f &vc, const Vec3f &vp, FCL_REAL t)
 Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t.
static void clipPolygonByPlane (Vec3f *polygon_points, unsigned int num_polygon_points, const Vec3f &n, FCL_REAL t, Vec3f clipped_points[], unsigned int *num_clipped_points)
 clip polygon by plane
static void clipSegmentByPlane (const Vec3f &v1, const Vec3f &v2, const Vec3f &n, FCL_REAL t, Vec3f *clipped_point)
 clip a line segment by plane
static void clipTriangleByTriangleAndEdgePlanes (const Vec3f &v1, const Vec3f &v2, const Vec3f &v3, const Vec3f &t1, const Vec3f &t2, const Vec3f &t3, const Vec3f &tn, FCL_REAL to, Vec3f clipped_points[], unsigned int *num_clipped_points, bool clip_triangle=false)
 clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to
static void computeCubicCoeff_EE (const Vec3f &a0, const Vec3f &b0, const Vec3f &c0, const Vec3f &d0, const Vec3f &va, const Vec3f &vb, const Vec3f &vc, const Vec3f &vd, FCL_REAL *a, FCL_REAL *b, FCL_REAL *c, FCL_REAL *d)
 Compute the cubic coefficients for EE intersection.
static void computeCubicCoeff_VE (const Vec3f &a0, const Vec3f &b0, const Vec3f &p0, const Vec3f &va, const Vec3f &vb, const Vec3f &vp, const Vec3f &L, FCL_REAL *a, FCL_REAL *b, FCL_REAL *c)
 Compute the cubic coefficients for VE intersection.
static void computeCubicCoeff_VF (const Vec3f &a0, const Vec3f &b0, const Vec3f &c0, const Vec3f &p0, const Vec3f &va, const Vec3f &vb, const Vec3f &vc, const Vec3f &vp, FCL_REAL *a, FCL_REAL *b, FCL_REAL *c, FCL_REAL *d)
 Compute the cubic coefficients for VF intersection See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.
static void computeDeepestPoints (Vec3f *clipped_points, unsigned int num_clipped_points, const Vec3f &n, FCL_REAL t, FCL_REAL *penetration_depth, Vec3f *deepest_points, unsigned int *num_deepest_points)
 compute the points which has deepest penetration depth
static FCL_REAL distanceToPlane (const Vec3f &n, FCL_REAL t, const Vec3f &v)
 distance of point v to a plane n * x - t = 0
static FCL_REAL gaussianCDF (FCL_REAL x)
 compute the cdf(x)
static bool insideLineSegment (const Vec3f &a, const Vec3f &b, const Vec3f &p)
 Check whether one point p is within a line segment [a, b].
static bool insideTriangle (const Vec3f &a, const Vec3f &b, const Vec3f &c, const Vec3f &p)
 Check whether one point p is within triangle [a, b, c].
static bool intersectPreFiltering (const Vec3f &a0, const Vec3f &b0, const Vec3f &c0, const Vec3f &d0, const Vec3f &a1, const Vec3f &b1, const Vec3f &c1, const Vec3f &d1)
 filter for intersection, works for both VF and EE
static bool isZero (FCL_REAL v)
 Check whether one value is zero.
static bool linelineIntersect (const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4, Vec3f *pa, Vec3f *pb, FCL_REAL *mua, FCL_REAL *mub)
 Calculate the line segment papb that is the shortest route between two lines p1p2 and p3p4. Calculate also the values of mua and mub where pa = p1 + mua (p2 - p1) pb = p3 + mub (p4 - p3) return FALSE if no solution exists.
static int project6 (const Vec3f &ax, const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &q1, const Vec3f &q2, const Vec3f &q3)
 Project function used in intersect_Triangle()
static bool sameSideOfPlane (const Vec3f &v1, const Vec3f &v2, const Vec3f &v3, const Vec3f &n, FCL_REAL t)
 check wether points v1, v2, v2 are on the same side of plane n * x - t = 0
static bool solveCubicWithIntervalNewton (const Vec3f &a0, const Vec3f &b0, const Vec3f &c0, const Vec3f &d0, const Vec3f &va, const Vec3f &vb, const Vec3f &vc, const Vec3f &vd, FCL_REAL &l, FCL_REAL &r, bool bVF, FCL_REAL coeffs[], Vec3f *data=NULL)
 Solve the cubic function using Newton method, also satisfies the interval restriction.
static bool solveSquare (FCL_REAL a, FCL_REAL b, FCL_REAL c, const Vec3f &a0, const Vec3f &b0, const Vec3f &c0, const Vec3f &d0, const Vec3f &va, const Vec3f &vb, const Vec3f &vc, const Vec3f &vd, bool bVF, FCL_REAL *ret)
 Solve a square function for EE intersection (with interval restriction)
static bool solveSquare (FCL_REAL a, FCL_REAL b, FCL_REAL c, const Vec3f &a0, const Vec3f &b0, const Vec3f &p0, const Vec3f &va, const Vec3f &vb, const Vec3f &vp)
 Solve a square function for VE intersection (with interval restriction)

Static Private Attributes

static const FCL_REAL CCD_RESOLUTION = 1e-7
static const FCL_REAL EPSILON = 1e-5
static const unsigned int MAX_TRIANGLE_CLIPS = 8
static const FCL_REAL NEAR_ZERO_THRESHOLD = 1e-7

Detailed Description

CCD intersect kernel among primitives.

Definition at line 71 of file intersect.h.


Member Function Documentation

bool fcl::Intersect::buildEdgePlane ( const Vec3f v1,
const Vec3f v2,
const Vec3f tn,
Vec3f n,
FCL_REAL t 
) [static, private]

build a plane pass through edge v1 and v2, normal is tn

Definition at line 1125 of file intersect.cpp.

bool fcl::Intersect::buildTrianglePlane ( const Vec3f v1,
const Vec3f v2,
const Vec3f v3,
Vec3f n,
FCL_REAL t 
) [static, private]

build a plane passed through triangle v1 v2 v3

Definition at line 1110 of file intersect.cpp.

bool fcl::Intersect::checkRootValidity_EE ( const Vec3f a0,
const Vec3f b0,
const Vec3f c0,
const Vec3f d0,
const Vec3f va,
const Vec3f vb,
const Vec3f vc,
const Vec3f vd,
FCL_REAL  t,
Vec3f q_i = NULL 
) [static, private]

Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time.

Definition at line 337 of file intersect.cpp.

bool fcl::Intersect::checkRootValidity_VE ( const Vec3f a0,
const Vec3f b0,
const Vec3f p0,
const Vec3f va,
const Vec3f vb,
const Vec3f vp,
FCL_REAL  t 
) [static, private]

Check whether a root for VE intersection is valid.

Definition at line 356 of file intersect.cpp.

bool fcl::Intersect::checkRootValidity_VF ( const Vec3f a0,
const Vec3f b0,
const Vec3f c0,
const Vec3f p0,
const Vec3f va,
const Vec3f vb,
const Vec3f vc,
const Vec3f vp,
FCL_REAL  t 
) [static, private]

Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t.

Definition at line 330 of file intersect.cpp.

void fcl::Intersect::clipPolygonByPlane ( Vec3f polygon_points,
unsigned int  num_polygon_points,
const Vec3f n,
FCL_REAL  t,
Vec3f  clipped_points[],
unsigned int *  num_clipped_points 
) [static, private]

clip polygon by plane

Definition at line 1015 of file intersect.cpp.

void fcl::Intersect::clipSegmentByPlane ( const Vec3f v1,
const Vec3f v2,
const Vec3f n,
FCL_REAL  t,
Vec3f clipped_point 
) [static, private]

clip a line segment by plane

Definition at line 1097 of file intersect.cpp.

void fcl::Intersect::clipTriangleByTriangleAndEdgePlanes ( const Vec3f v1,
const Vec3f v2,
const Vec3f v3,
const Vec3f t1,
const Vec3f t2,
const Vec3f t3,
const Vec3f tn,
FCL_REAL  to,
Vec3f  clipped_points[],
unsigned int *  num_clipped_points,
bool  clip_triangle = false 
) [static, private]

clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to

Definition at line 967 of file intersect.cpp.

void fcl::Intersect::computeCubicCoeff_EE ( const Vec3f a0,
const Vec3f b0,
const Vec3f c0,
const Vec3f d0,
const Vec3f va,
const Vec3f vb,
const Vec3f vc,
const Vec3f vd,
FCL_REAL a,
FCL_REAL b,
FCL_REAL c,
FCL_REAL d 
) [static, private]

Compute the cubic coefficients for EE intersection.

Definition at line 450 of file intersect.cpp.

void fcl::Intersect::computeCubicCoeff_VE ( const Vec3f a0,
const Vec3f b0,
const Vec3f p0,
const Vec3f va,
const Vec3f vb,
const Vec3f vp,
const Vec3f L,
FCL_REAL a,
FCL_REAL b,
FCL_REAL c 
) [static, private]

Compute the cubic coefficients for VE intersection.

Definition at line 471 of file intersect.cpp.

void fcl::Intersect::computeCubicCoeff_VF ( const Vec3f a0,
const Vec3f b0,
const Vec3f c0,
const Vec3f p0,
const Vec3f va,
const Vec3f vb,
const Vec3f vc,
const Vec3f vp,
FCL_REAL a,
FCL_REAL b,
FCL_REAL c,
FCL_REAL d 
) [static, private]

Compute the cubic coefficients for VF intersection See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.

Compute the cubic coefficients for VF case See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1.

Definition at line 428 of file intersect.cpp.

void fcl::Intersect::computeDeepestPoints ( Vec3f clipped_points,
unsigned int  num_clipped_points,
const Vec3f n,
FCL_REAL  t,
FCL_REAL penetration_depth,
Vec3f deepest_points,
unsigned int *  num_deepest_points 
) [static, private]

compute the points which has deepest penetration depth

Definition at line 929 of file intersect.cpp.

FCL_REAL fcl::Intersect::distanceToPlane ( const Vec3f n,
FCL_REAL  t,
const Vec3f v 
) [static, private]

distance of point v to a plane n * x - t = 0

Definition at line 1105 of file intersect.cpp.

static FCL_REAL fcl::Intersect::gaussianCDF ( FCL_REAL  x) [inline, static, private]

compute the cdf(x)

Definition at line 237 of file intersect.h.

bool fcl::Intersect::insideLineSegment ( const Vec3f a,
const Vec3f b,
const Vec3f p 
) [static, private]

Check whether one point p is within a line segment [a, b].

Definition at line 284 of file intersect.cpp.

bool fcl::Intersect::insideTriangle ( const Vec3f a,
const Vec3f b,
const Vec3f c,
const Vec3f p 
) [static, private]

Check whether one point p is within triangle [a, b, c].

Definition at line 267 of file intersect.cpp.

bool fcl::Intersect::intersect_EE ( const Vec3f a0,
const Vec3f b0,
const Vec3f c0,
const Vec3f d0,
const Vec3f a1,
const Vec3f b1,
const Vec3f c1,
const Vec3f d1,
FCL_REAL collision_time,
Vec3f p_i,
bool  useNewton = true 
) [static]

CCD intersect between two edges [a0, b0] and [a1, b1] are points for one edge in time t0 and t1 [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1 p_i returns the coordinate of the collision point.

if(isZero(a)) { return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false); }

Definition at line 554 of file intersect.cpp.

bool fcl::Intersect::intersect_EE_filtered ( const Vec3f a0,
const Vec3f b0,
const Vec3f c0,
const Vec3f d0,
const Vec3f a1,
const Vec3f b1,
const Vec3f c1,
const Vec3f d1,
FCL_REAL collision_time,
Vec3f p_i,
bool  useNewton = true 
) [static]

CCD intersect between two edges, using additional filter.

Definition at line 681 of file intersect.cpp.

bool fcl::Intersect::intersect_Triangle ( const Vec3f P1,
const Vec3f P2,
const Vec3f P3,
const Vec3f Q1,
const Vec3f Q2,
const Vec3f Q3,
Vec3f contact_points = NULL,
unsigned int *  num_contact_points = NULL,
FCL_REAL penetration_depth = NULL,
Vec3f normal = NULL 
) [static]

CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3].

Definition at line 809 of file intersect.cpp.

bool fcl::Intersect::intersect_Triangle ( const Vec3f P1,
const Vec3f P2,
const Vec3f P3,
const Vec3f Q1,
const Vec3f Q2,
const Vec3f Q3,
const Matrix3f R,
const Vec3f T,
Vec3f contact_points = NULL,
unsigned int *  num_contact_points = NULL,
FCL_REAL penetration_depth = NULL,
Vec3f normal = NULL 
) [static]

Definition at line 693 of file intersect.cpp.

bool fcl::Intersect::intersect_Triangle ( const Vec3f P1,
const Vec3f P2,
const Vec3f P3,
const Vec3f Q1,
const Vec3f Q2,
const Vec3f Q3,
const Transform3f tf,
Vec3f contact_points = NULL,
unsigned int *  num_contact_points = NULL,
FCL_REAL penetration_depth = NULL,
Vec3f normal = NULL 
) [static]

Definition at line 708 of file intersect.cpp.

bool fcl::Intersect::intersect_VE ( const Vec3f a0,
const Vec3f b0,
const Vec3f p0,
const Vec3f a1,
const Vec3f b1,
const Vec3f p1,
const Vec3f L 
) [static]

CCD intersect between one vertex and and one edge.

Definition at line 619 of file intersect.cpp.

bool fcl::Intersect::intersect_VF ( const Vec3f a0,
const Vec3f b0,
const Vec3f c0,
const Vec3f p0,
const Vec3f a1,
const Vec3f b1,
const Vec3f c1,
const Vec3f p1,
FCL_REAL collision_time,
Vec3f p_i,
bool  useNewton = true 
) [static]

CCD intersect between one vertex and one face [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1 p0 and p1 are points for vertex in time t0 and t1 p_i returns the coordinate of the collision point.

if(isZero(a)) { return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time); }

Definition at line 490 of file intersect.cpp.

bool fcl::Intersect::intersect_VF_filtered ( const Vec3f a0,
const Vec3f b0,
const Vec3f c0,
const Vec3f p0,
const Vec3f a1,
const Vec3f b1,
const Vec3f c1,
const Vec3f p1,
FCL_REAL collision_time,
Vec3f p_i,
bool  useNewton = true 
) [static]

CCD intersect between one vertex and one face, using additional filter.

Definition at line 669 of file intersect.cpp.

bool fcl::Intersect::intersectPreFiltering ( const Vec3f a0,
const Vec3f b0,
const Vec3f c0,
const Vec3f d0,
const Vec3f a1,
const Vec3f b1,
const Vec3f c1,
const Vec3f d1 
) [static, private]

filter for intersection, works for both VF and EE

Prefilter for intersection, works for both VF and EE.

Definition at line 640 of file intersect.cpp.

bool fcl::Intersect::isZero ( FCL_REAL  v) [inline, static, private]

Check whether one value is zero.

Definition at line 181 of file intersect.cpp.

bool fcl::Intersect::linelineIntersect ( const Vec3f p1,
const Vec3f p2,
const Vec3f p3,
const Vec3f p4,
Vec3f pa,
Vec3f pb,
FCL_REAL mua,
FCL_REAL mub 
) [static, private]

Calculate the line segment papb that is the shortest route between two lines p1p2 and p3p4. Calculate also the values of mua and mub where pa = p1 + mua (p2 - p1) pb = p3 + mub (p4 - p3) return FALSE if no solution exists.

Calculate the line segment papb that is the shortest route between two lines p1p2 and p3p4. Calculate also the values of mua and mub where pa = p1 + mua (p2 - p1) pb = p3 + mub (p4 - p3) Return FALSE if no solution exists.

Definition at line 294 of file intersect.cpp.

int fcl::Intersect::project6 ( const Vec3f ax,
const Vec3f p1,
const Vec3f p2,
const Vec3f p3,
const Vec3f q1,
const Vec3f q2,
const Vec3f q3 
) [static, private]

Project function used in intersect_Triangle()

Definition at line 1150 of file intersect.cpp.

bool fcl::Intersect::sameSideOfPlane ( const Vec3f v1,
const Vec3f v2,
const Vec3f v3,
const Vec3f n,
FCL_REAL  t 
) [static, private]

check wether points v1, v2, v2 are on the same side of plane n * x - t = 0

Definition at line 1140 of file intersect.cpp.

bool fcl::Intersect::solveCubicWithIntervalNewton ( const Vec3f a0,
const Vec3f b0,
const Vec3f c0,
const Vec3f d0,
const Vec3f va,
const Vec3f vb,
const Vec3f vc,
const Vec3f vd,
FCL_REAL l,
FCL_REAL r,
bool  bVF,
FCL_REAL  coeffs[],
Vec3f data = NULL 
) [static, private]

Solve the cubic function using Newton method, also satisfies the interval restriction.

data: only used for EE, return the intersect point

Definition at line 187 of file intersect.cpp.

bool fcl::Intersect::solveSquare ( FCL_REAL  a,
FCL_REAL  b,
FCL_REAL  c,
const Vec3f a0,
const Vec3f b0,
const Vec3f c0,
const Vec3f d0,
const Vec3f va,
const Vec3f vb,
const Vec3f vc,
const Vec3f vd,
bool  bVF,
FCL_REAL ret 
) [static, private]

Solve a square function for EE intersection (with interval restriction)

Definition at line 363 of file intersect.cpp.

bool fcl::Intersect::solveSquare ( FCL_REAL  a,
FCL_REAL  b,
FCL_REAL  c,
const Vec3f a0,
const Vec3f b0,
const Vec3f p0,
const Vec3f va,
const Vec3f vb,
const Vec3f vp 
) [static, private]

Solve a square function for VE intersection (with interval restriction)

Definition at line 399 of file intersect.cpp.


Member Data Documentation

const FCL_REAL fcl::Intersect::CCD_RESOLUTION = 1e-7 [static, private]

Definition at line 245 of file intersect.h.

const FCL_REAL fcl::Intersect::EPSILON = 1e-5 [static, private]

Definition at line 243 of file intersect.h.

const unsigned int fcl::Intersect::MAX_TRIANGLE_CLIPS = 8 [static, private]

Definition at line 246 of file intersect.h.

const FCL_REAL fcl::Intersect::NEAR_ZERO_THRESHOLD = 1e-7 [static, private]

Definition at line 244 of file intersect.h.


The documentation for this class was generated from the following files:
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:31