00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00037 #include "fcl/BV/AABB.h"
00038
00039 #include <limits>
00040 #include <iostream>
00041
00042 namespace fcl
00043 {
00044
00045 AABB::AABB() : min_(std::numeric_limits<FCL_REAL>::max()),
00046 max_(-std::numeric_limits<FCL_REAL>::max())
00047 {
00048 }
00049
00050 FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
00051 {
00052 FCL_REAL result = 0;
00053 for(std::size_t i = 0; i < 3; ++i)
00054 {
00055 const FCL_REAL& amin = min_[i];
00056 const FCL_REAL& amax = max_[i];
00057 const FCL_REAL& bmin = other.min_[i];
00058 const FCL_REAL& bmax = other.max_[i];
00059
00060 if(amin > bmax)
00061 {
00062 FCL_REAL delta = bmax - amin;
00063 result += delta * delta;
00064 if(P && Q)
00065 {
00066 (*P)[i] = amin;
00067 (*Q)[i] = bmax;
00068 }
00069 }
00070 else if(bmin > amax)
00071 {
00072 FCL_REAL delta = amax - bmin;
00073 result += delta * delta;
00074 if(P && Q)
00075 {
00076 (*P)[i] = amax;
00077 (*Q)[i] = bmin;
00078 }
00079 }
00080 else
00081 {
00082 if(P && Q)
00083 {
00084 if(bmin >= amin)
00085 {
00086 FCL_REAL t = 0.5 * (amax + bmin);
00087 (*P)[i] = t;
00088 (*Q)[i] = t;
00089 }
00090 else
00091 {
00092 FCL_REAL t = 0.5 * (amin + bmax);
00093 (*P)[i] = t;
00094 (*Q)[i] = t;
00095 }
00096 }
00097 }
00098 }
00099
00100 return std::sqrt(result);
00101 }
00102
00103 FCL_REAL AABB::distance(const AABB& other) const
00104 {
00105 FCL_REAL result = 0;
00106 for(std::size_t i = 0; i < 3; ++i)
00107 {
00108 const FCL_REAL& amin = min_[i];
00109 const FCL_REAL& amax = max_[i];
00110 const FCL_REAL& bmin = other.min_[i];
00111 const FCL_REAL& bmax = other.max_[i];
00112
00113 if(amin > bmax)
00114 {
00115 FCL_REAL delta = bmax - amin;
00116 result += delta * delta;
00117 }
00118 else if(bmin > amax)
00119 {
00120 FCL_REAL delta = amax - bmin;
00121 result += delta * delta;
00122 }
00123 }
00124
00125 return std::sqrt(result);
00126 }
00127
00128 }