~choreonoid/choreonoid/debian

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

* OPCODE - Optimized Collision Detection

* Homepage: http://www.codercorner.com/Opcode.htm

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/**

* Contains code for optimized trees.

* \file OPC_OptimizedTree.h

* \author Pierre Terdiman

* \date March, 20, 2001

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

// Include Guard

#ifndef __OPC_OPTIMIZEDTREE_H__

#define __OPC_OPTIMIZEDTREE_H__

#ifdef __x86_64

#define EXWORD uqword

#else

#define EXWORD udword

#endif

//! Common interface for a node of an implicit tree

#define IMPLEMENT_IMPLICIT_NODE(base_class, volume) \

public: \

/* Constructor / Destructor */ \

inline_ base_class() : mData(0) {} \

inline_ ~base_class() {} \

/* Leaf test */ \

inline_ BOOL IsLeaf() const { return mData&1; } \

/* Data access */ \

inline_ const base_class* GetPos() const { return (base_class*)mData; } \

inline_ const base_class* GetNeg() const { return ((base_class*)mData)+1; } \

/* Modified by S-cubed, Inc. */ \

inline_ const base_class* GetB() const { return mB; } \

inline_ udword GetPrimitive() const { return (mData>>1); } \

/* Stats */ \

inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \

volume mAABB; \

EXWORD mData; \

/* Modified by S-cubed, Inc. */ \

base_class* mB;

//! Common interface for a node of a no-leaf tree

#define IMPLEMENT_NOLEAF_NODE(base_class, volume) \

public: \

/* Constructor / Destructor */ \

inline_ base_class() : mPosData(0), mNegData(0) {} \

inline_ ~base_class() {} \

/* Leaf tests */ \

inline_ BOOL HasPosLeaf() const { return mPosData&1; } \

inline_ BOOL HasNegLeaf() const { return mNegData&1; } \

/* Data access */ \

inline_ const base_class* GetPos() const { return (base_class*)mPosData; } \

inline_ const base_class* GetNeg() const { return (base_class*)mNegData; } \

/* Modified by S-cubed, Inc. */ \

inline_ const base_class* GetB() const { return mB; } \

inline_ udword GetPosPrimitive() const { return (mPosData>>1); } \

inline_ udword GetNegPrimitive() const { return (mNegData>>1); } \

/* Stats */ \

inline_ udword GetNodeSize() const { return SIZEOFOBJECT; } \

volume mAABB; \

EXWORD mPosData; \

EXWORD mNegData; \

/* Modified by S-cubed, Inc. */ \

base_class* mB;

class OPCODE_API AABBCollisionNode

{

IMPLEMENT_IMPLICIT_NODE(AABBCollisionNode, CollisionAABB)

inline_ float GetVolume() const { return mAABB.mExtents.x * mAABB.mExtents.y * mAABB.mExtents.z; }

inline_ float GetSize() const { return mAABB.mExtents.SquareMagnitude(); }

inline_ udword GetRadius() const

{

udword* Bits = (udword*)&mAABB.mExtents.x;

udword Max = Bits[0];

if(Bits[1]>Max) Max = Bits[1];

if(Bits[2]>Max) Max = Bits[2];

return Max;

}

// NB: using the square-magnitude or the true volume of the box, seems to yield better results

// (assuming UNC-like informed traversal methods). I borrowed this idea from PQP. The usual "size"

// otherwise, is the largest box extent. In SOLID that extent is computed on-the-fly each time it's

// needed (the best approach IMHO). In RAPID the rotation matrix is permuted so that Extent[0] is

// always the greatest, which saves looking for it at runtime. On the other hand, it yields matrices

// whose determinant is not 1, i.e. you can't encode them anymore as unit quaternions. Not a very

// good strategy.

};

100

class OPCODE_API AABBQuantizedNode

101

{

102

IMPLEMENT_IMPLICIT_NODE(AABBQuantizedNode, QuantizedAABB)

103

104

inline_ uword GetSize() const

105

{

106

const uword* Bits = mAABB.mExtents;

107

uword Max = Bits[0];

108

if(Bits[1]>Max) Max = Bits[1];

109

if(Bits[2]>Max) Max = Bits[2];

110

return Max;

111

}

112

// NB: for quantized nodes I don't feel like computing a square-magnitude with integers all

113

// over the place.......!

114

};

115

116

class OPCODE_API AABBNoLeafNode

117

{

118

IMPLEMENT_NOLEAF_NODE(AABBNoLeafNode, CollisionAABB)

119

};

120

121

class OPCODE_API AABBQuantizedNoLeafNode

122

{

123

IMPLEMENT_NOLEAF_NODE(AABBQuantizedNoLeafNode, QuantizedAABB)

124

};

125

126

//! Common interface for a collision tree

127

#define IMPLEMENT_COLLISION_TREE(base_class, node) \

128

public: \

129

/* Constructor / Destructor */ \

130

base_class(); \

131

virtual ~base_class(); \

132

/* Builds from a standard tree */ \

133

override(AABBOptimizedTree) bool Build(AABBTree* tree); \

134

/* Refits the tree */ \

135

override(AABBOptimizedTree) bool Refit(const MeshInterface* mesh_interface); \

136

/* Walks the tree */ \

137

override(AABBOptimizedTree) bool Walk(GenericWalkingCallback callback, void* user_data) const; \

138

/* Data access */ \

139

inline_ const node* GetNodes() const { return mNodes; } \

140

/* Stats */ \

141

override(AABBOptimizedTree) udword GetUsedBytes() const { return mNbNodes*sizeof(node); } \

142

private: \

143

node* mNodes;

144

145

typedef bool (*GenericWalkingCallback) (const void* current, void* user_data);

146

147

class OPCODE_API AABBOptimizedTree

148

{

149

public:

150

// Constructor / Destructor

151

AABBOptimizedTree() :

152

mNbNodes (0)

153

{}

154

virtual ~AABBOptimizedTree() {}

155

156

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

157

/**

158

* Builds the collision tree from a generic AABB tree.

159

* \param tree [in] generic AABB tree

160

* \return true if success

161

162

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

163

virtual bool Build(AABBTree* tree) = 0;

164

165

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

166

/**

167

* Refits the collision tree after vertices have been modified.

168

* \param mesh_interface [in] mesh interface for current model

169

* \return true if success

170

171

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

172

virtual bool Refit(const MeshInterface* mesh_interface) = 0;

173

174

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

175

/**

176

* Walks the tree and call the user back for each node.

177

* \param callback [in] walking callback

178

* \param user_data [in] callback's user data

179

* \return true if success

180

181

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

182

virtual bool Walk(GenericWalkingCallback callback, void* user_data) const = 0;

183

184

// Data access

185

virtual udword GetUsedBytes() const = 0;

186

inline_ udword GetNbNodes() const { return mNbNodes; }

187

188

protected:

189

udword mNbNodes;

190

};

191

192

class OPCODE_API AABBCollisionTree : public AABBOptimizedTree

193

{

194

IMPLEMENT_COLLISION_TREE(AABBCollisionTree, AABBCollisionNode)

195

};

196

197

class OPCODE_API AABBNoLeafTree : public AABBOptimizedTree

198

{

199

IMPLEMENT_COLLISION_TREE(AABBNoLeafTree, AABBNoLeafNode)

200

};

201

202

class OPCODE_API AABBQuantizedTree : public AABBOptimizedTree

203

{

204

IMPLEMENT_COLLISION_TREE(AABBQuantizedTree, AABBQuantizedNode)

205

206

public:

207

Point mCenterCoeff;

208

Point mExtentsCoeff;

209

};

210

211

class OPCODE_API AABBQuantizedNoLeafTree : public AABBOptimizedTree

212

{

213

IMPLEMENT_COLLISION_TREE(AABBQuantizedNoLeafTree, AABBQuantizedNoLeafNode)

214

215

public:

216

Point mCenterCoeff;

217

Point mExtentsCoeff;

218

};

219

220

#endif // __OPC_OPTIMIZEDTREE_H__

Older »