~ubuntu-branches/ubuntu/vivid/emscripten/vivid

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.

2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.

3. This notice may not be removed or altered from any source distribution.

#include "btGjkConvexCast.h"

#include "BulletCollision/CollisionShapes/btSphereShape.h"

#include "btGjkPairDetector.h"

#include "btPointCollector.h"

#include "LinearMath/btTransformUtil.h"

#ifdef BT_USE_DOUBLE_PRECISION

#define MAX_ITERATIONS 64

#else

#define MAX_ITERATIONS 32

#endif

btGjkConvexCast::btGjkConvexCast(const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver)

:m_simplexSolver(simplexSolver),

m_convexA(convexA),

m_convexB(convexB)

{

}

bool btGjkConvexCast::calcTimeOfImpact(

const btTransform& fromA,

const btTransform& toA,

const btTransform& fromB,

const btTransform& toB,

CastResult& result)

{

m_simplexSolver->reset();

/// compute linear velocity for this interval, to interpolate

//assume no rotation/angular velocity, assert here?

btVector3 linVelA,linVelB;

linVelA = toA.getOrigin()-fromA.getOrigin();

linVelB = toB.getOrigin()-fromB.getOrigin();

btScalar radius = btScalar(0.001);

btScalar lambda = btScalar(0.);

btVector3 v(1,0,0);

int maxIter = MAX_ITERATIONS;

btVector3 n;

n.setValue(btScalar(0.),btScalar(0.),btScalar(0.));

bool hasResult = false;

btVector3 c;

btVector3 r = (linVelA-linVelB);

btScalar lastLambda = lambda;

//btScalar epsilon = btScalar(0.001);

int numIter = 0;

//first solution, using GJK

btTransform identityTrans;

identityTrans.setIdentity();

// result.drawCoordSystem(sphereTr);

btPointCollector pointCollector;

btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,0);//m_penetrationDepthSolver);

btGjkPairDetector::ClosestPointInput input;

//we don't use margins during CCD

// gjk.setIgnoreMargin(true);

input.m_transformA = fromA;

input.m_transformB = fromB;

gjk.getClosestPoints(input,pointCollector,0);

hasResult = pointCollector.m_hasResult;

c = pointCollector.m_pointInWorld;

if (hasResult)

{

btScalar dist;

dist = pointCollector.m_distance;

n = pointCollector.m_normalOnBInWorld;

100

101

102

103

//not close enough

104

while (dist > radius)

105

{

106

numIter++;

107

if (numIter > maxIter)

108

{

109

return false; //todo: report a failure

110

}

111

btScalar dLambda = btScalar(0.);

112

113

btScalar projectedLinearVelocity = r.dot(n);

114

115

dLambda = dist / (projectedLinearVelocity);

116

117

lambda = lambda - dLambda;

118

119

if (lambda > btScalar(1.))

120

return false;

121

122

if (lambda < btScalar(0.))

123

return false;

124

125

//todo: next check with relative epsilon

126

if (lambda <= lastLambda)

127

{

128

return false;

129

//n.setValue(0,0,0);

130

break;

131

}

132

lastLambda = lambda;

133

134

//interpolate to next lambda

135

result.DebugDraw( lambda );

136

input.m_transformA.getOrigin().setInterpolate3(fromA.getOrigin(),toA.getOrigin(),lambda);

137

input.m_transformB.getOrigin().setInterpolate3(fromB.getOrigin(),toB.getOrigin(),lambda);

138

139

gjk.getClosestPoints(input,pointCollector,0);

140

if (pointCollector.m_hasResult)

141

{

142

if (pointCollector.m_distance < btScalar(0.))

143

{

144

result.m_fraction = lastLambda;

145

n = pointCollector.m_normalOnBInWorld;

146

result.m_normal=n;

147

result.m_hitPoint = pointCollector.m_pointInWorld;

148

return true;

149

}

150

c = pointCollector.m_pointInWorld;

151

n = pointCollector.m_normalOnBInWorld;

152

dist = pointCollector.m_distance;

153

} else

154

{

155

//??

156

return false;

157

}

158

159

}

160

161

//is n normalized?

162

//don't report time of impact for motion away from the contact normal (or causes minor penetration)

163

if (n.dot(r)>=-result.m_allowedPenetration)

164

return false;

165

166

result.m_fraction = lambda;

167

result.m_normal = n;

168

result.m_hitPoint = c;

169

return true;

170

}

171

172

return false;

173

174

175

}

176

Older »