static void convexHullSupport( ref btVector3 localDirOrg, btVector3[] points, int numPoints, ref btVector3 localScaling, out btVector3 result )
{
btVector3 vec; localDirOrg.Mult( ref localScaling, out vec );
double maxDot;
long ptIndex = vec.maxDot( points, numPoints, out maxDot );
Debug.Assert( ptIndex >= 0 );
points[ptIndex].Mult( ref localScaling, out result );
}
public static void integrateTransform( ref btTransform curTrans, ref btVector3 linvel, ref btVector3 angvel
, double timeStep, out btTransform predictedTransform )
{
btVector3 tmp;
btVector3 tmp2;
linvel.Mult( timeStep, out tmp );
curTrans.getOrigin( out tmp2 );
tmp2.Add( ref tmp, out predictedTransform.m_origin );
#if QUATERNION_DERIVATIVE
btQuaternion predictedOrn = curTrans.getRotation();
predictedOrn += (angvel predictedOrn) * (timeStep 0.5);
predictedOrn.normalize();
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
btVector3 axis;
double fAngle = angvel.length();
//limit the angular motion
if( fAngle * timeStep > ANGULAR_MOTION_THRESHOLD )
{
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
}
if( fAngle < 0.001 )
{
// use Taylor's expansions of sync function
angvel.Mult( ( btScalar.BT_HALF * timeStep - ( timeStep * timeStep * timeStep ) * ( 0.020833333333 ) * fAngle * fAngle ), out axis );
}
else
{
// sync(fAngle) = sin(cfAngle)/t
angvel.Mult( ( btScalar.btSin( btScalar.BT_HALF * fAngle * timeStep ) / fAngle ), out axis );
}
btQuaternion dorn = new btQuaternion( axis.x, axis.y, axis.z, btScalar.btCos( fAngle * timeStep * 0.5 ) );
btQuaternion orn0;
curTrans.getRotation( out orn0 );
btQuaternion predictedOrn;
dorn.Mult( ref orn0, out predictedOrn );
predictedOrn.normalize();
#endif
btMatrix3x3.setRotation( out predictedTransform.m_basis, ref predictedOrn);
//predictedTransform.setRotation( ref predictedOrn );
}
public override void calculateLocalInertia( double mass, out btVector3 inertia )
{
btVector3 aabbMin, aabbMax;
getAabb( ref btTransform.Identity, out aabbMin, out aabbMax );
btVector3 tmp;
aabbMax.Sub( ref aabbMin, out tmp );
btVector3 halfExtents;
tmp.Mult( btScalar.BT_HALF, out halfExtents );
double margin = getMargin();
double lx = (double)( 2.0 ) * ( halfExtents.x + margin );
double ly = (double)( 2.0 ) * ( halfExtents.y + margin );
double lz = (double)( 2.0 ) * ( halfExtents.z + margin );
double x2 = lx * lx;
double y2 = ly * ly;
double z2 = lz * lz;
double scaledmass = mass * (double)( 0.08333333 );
tmp = new btVector3( y2 + z2, x2 + z2, x2 + y2 );
tmp.Mult( scaledmass, out inertia );
}
public btCylinderShape( ref btVector3 halfExtents )
{
m_upAxis = ( 1 );
setSafeMargin( ref halfExtents );
btVector3 margin = new btVector3( getMargin() );
btVector3 tmp;
halfExtents.Mult( ref m_localScaling, out tmp );
tmp.Sub( ref margin, out m_implicitShapeDimensions );
//m_implicitShapeDimensions = ( halfExtents * m_localScaling ) - margin;
m_shapeType = BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE;
}
///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
public void applyDamping( double timeStep )
{
//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
//#define USE_OLD_DAMPING_METHOD 1
#if USE_OLD_DAMPING_METHOD
m_linearVelocity *= GEN_clamped( ( (double)( 1.0) - timeStep * m_linearDamping ), (double)btScalar.BT_ZERO, (double)(double)( 1.0 ) );
m_angularVelocity *= GEN_clamped( ( (double)( 1.0) - timeStep * m_angularDamping ), (double)btScalar.BT_ZERO, (double)(double)( 1.0 ) );
#else
m_linearVelocity.Mult( btScalar.btPow( (double)( 1 ) - m_linearDamping, timeStep ), out m_linearVelocity );
m_angularVelocity.Mult( btScalar.btPow( (double)( 1 ) - m_angularDamping, timeStep ), out m_angularVelocity );
//m_linearVelocity *= btScalar.btPow( (double)( 1 ) - m_linearDamping, timeStep );
//m_angularVelocity *= btScalar.btPow( (double)( 1 ) - m_angularDamping, timeStep );
#endif
if( m_additionalDamping )
{
//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
if( ( m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr ) &
( m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr ) )
{
m_linearVelocity.Mult( m_additionalDampingFactor, out m_linearVelocity );
m_angularVelocity.Mult( m_additionalDampingFactor, out m_angularVelocity );
//m_angularVelocity *= m_additionalDampingFactor;
//m_linearVelocity *= m_additionalDampingFactor;
}
double speed = m_linearVelocity.length();
if( speed < m_linearDamping )
{
double dampVel = (double)( 0.005 );
if( speed > dampVel )
{
btVector3 dir; m_linearVelocity.normalized( out dir );
dir.Mult( dampVel, out dir );
m_linearVelocity.Sub( ref dir, out m_linearVelocity );
//m_linearVelocity -= dir * dampVel;
}
else
{
m_linearVelocity = btVector3.Zero;
}
}
double angSpeed = m_angularVelocity.length();
if( angSpeed < m_angularDamping )
{
double angDampVel = (double)( 0.005 );
if( angSpeed > angDampVel )
{
btVector3 dir; m_angularVelocity.normalized( out dir );
dir.Mult( angDampVel, out dir );
m_angularVelocity.Sub( ref dir, out m_angularVelocity );
//m_angularVelocity -= dir * angDampVel;
}
else
{
m_angularVelocity = btVector3.Zero;
}
}
}
}
public void evalEulerEqn( ref btVector3 w1, ref btVector3 w0, ref btVector3 T, double dt,
ref btMatrix3x3 I, out btVector3 result )
{
btVector3 Iw0;
btVector3 dtT;
btVector3 Iw1;
btVector3 cross;
T.Mult( dt, out dtT );
I.Apply( ref w0, out Iw0 );
dtT.Add( ref Iw0, out Iw0 );
// Iw0 = ( T * dt + I * w0 )
I.Apply( ref w1, out Iw1 );
w1.cross( ref Iw1, out cross );
cross.Mult( dt, out cross );
// cross = w1.cross( I * w1 ) * dt
Iw1.Add( ref cross, out Iw1 );
// Iw1 = Iw1 + cross
Iw1.Sub( ref Iw0, out result );
//btVector3 w2; = I * w1 + w1.cross( I * w1 ) * dt - ( T * dt + I * w0 );
//return w2;
}
//! apply the correction impulses for two bodies
//double solveAngularLimits(double timeStep,ref btVector3 axis, double jacDiagABInv,btRigidBody * body0, btRigidBody * body1);
internal double solveAngularLimits(
double timeStep, ref btVector3 axis, double jacDiagABInv,
btRigidBody body0, btRigidBody body1 )
{
if( needApplyTorques() == false ) return 0.0f;
double target_velocity = m_targetVelocity;
double maxMotorForce = m_maxMotorForce;
//current error correction
if( m_currentLimit != 0 )
{
target_velocity = -m_stopERP * m_currentLimitError / ( timeStep );
maxMotorForce = m_maxLimitForce;
}
maxMotorForce *= timeStep;
// current velocity difference
btVector3 angVelA = body0.getAngularVelocity();
btVector3 angVelB = body1.getAngularVelocity();
btVector3 vel_diff;
vel_diff = angVelA - angVelB;
double rel_vel = axis.dot( vel_diff );
// correction velocity
double motor_relvel = m_limitSoftness * ( target_velocity - m_damping * rel_vel );
if( motor_relvel < btScalar.SIMD_EPSILON && motor_relvel > -btScalar.SIMD_EPSILON )
{
return 0.0f;//no need for applying force
}
// correction impulse
double unclippedMotorImpulse = ( 1 + m_bounce ) * motor_relvel * jacDiagABInv;
// clip correction impulse
double clippedMotorImpulse;
///@todo: should clip against accumulated impulse
if( unclippedMotorImpulse > 0.0f )
{
clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
}
else
{
clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
}
// sort with accumulated impulses
double lo = (double)( -btScalar.BT_LARGE_FLOAT );
double hi = (double)( btScalar.BT_LARGE_FLOAT );
double oldaccumImpulse = m_accumulatedImpulse;
double sum = oldaccumImpulse + clippedMotorImpulse;
m_accumulatedImpulse = sum > hi ? btScalar.BT_ZERO : sum < lo ? btScalar.BT_ZERO : sum;
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
btVector3 motorImp; axis.Mult( clippedMotorImpulse, out motorImp );
body0.applyTorqueImpulse( ref motorImp );
btVector3 tmp;
motorImp.Invert( out tmp );
body1.applyTorqueImpulse( ref tmp );
return clippedMotorImpulse;
}
///setupRigidBody is only used internally by the constructor
public void setupRigidBody( btRigidBodyConstructionInfo constructionInfo )
{
m_internalType = CollisionObjectTypes.CO_RIGID_BODY;
m_linearVelocity = btVector3.Zero;
m_angularVelocity = btVector3.Zero;
m_angularFactor = btVector3.One;
m_linearFactor = btVector3.One;
m_gravity = btVector3.Zero;
m_gravity_acceleration = btVector3.Zero;
m_totalForce = btVector3.Zero;
m_totalTorque = btVector3.Zero;
setDamping( constructionInfo.m_linearDamping, constructionInfo.m_angularDamping );
m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
m_optionalMotionState = constructionInfo.m_motionState;
//m_contactSolverType = 0;
//m_frictionSolverType = 0;
m_additionalDamping = constructionInfo.m_additionalDamping;
m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
if( m_optionalMotionState != null )
{
m_optionalMotionState.getWorldTransform( out m_worldTransform );
}
else
{
m_worldTransform = constructionInfo.m_startWorldTransform;
}
m_interpolationWorldTransform = m_worldTransform;
m_interpolationLinearVelocity.setValue( 0, 0, 0 );
m_interpolationAngularVelocity.setValue( 0, 0, 0 );
//moved to btCollisionObject
m_friction = constructionInfo.m_friction;
m_rollingFriction = constructionInfo.m_rollingFriction;
m_restitution = constructionInfo.m_restitution;
setCollisionShape( constructionInfo.m_collisionShape );
m_debugBodyId = uniqueId++;
setMassProps( constructionInfo.m_mass, ref constructionInfo.m_localInertia );
updateInertiaTensor();
m_rigidbodyFlags = btRigidBodyFlags.BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY;
m_deltaLinearVelocity.setZero();
m_deltaAngularVelocity.setZero();
m_linearFactor.Mult( m_inverseMass, out m_invMass );
//m_invMass = m_inverseMass * m_linearFactor;
m_pushVelocity.setZero();
m_turnVelocity.setZero();
}
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
public void applyImpulse( ref btVector3 linearComponent, ref btVector3 angularComponent, double impulseMagnitude )
{
if( m_originalBody != null )
{
btVector3 tmp;
linearComponent.Mult( ref m_linearFactor, out tmp );
m_deltaLinearVelocity.AddScale( ref tmp, impulseMagnitude, out m_deltaLinearVelocity );
//m_deltaLinearVelocity += linearComponent * impulseMagnitude * m_linearFactor;
angularComponent.Mult( ref m_angularFactor, out tmp );
m_deltaAngularVelocity.AddScale( ref tmp, impulseMagnitude, out m_deltaAngularVelocity );
//if( m_deltaLinearVelocity.length() > 20 )
// Debugger.Break();
//m_deltaAngularVelocity += angularComponent * ( impulseMagnitude * m_angularFactor );
btScalar.Dbg( "apply impulse delta linear velocity is " + m_deltaLinearVelocity.ToString() );
}
}
protected void initSolverBody( btSolverBody solverBody, btRigidBody rb, double timeStep )
{
//btRigidBody rb = collisionObject != null ? btRigidBody.upcast( collisionObject ) : null;
solverBody.m_deltaLinearVelocity = btVector3.Zero;
solverBody.m_deltaAngularVelocity = btVector3.Zero;
solverBody.m_pushVelocity = btVector3.Zero;
solverBody.m_turnVelocity = btVector3.Zero;
solverBody.modified = false;
solverBody.pushed = false;
if( rb != null )
{
solverBody.m_worldTransform = rb.m_worldTransform;
btVector3 tmp = new btVector3( rb.getInvMass() );
btVector3 tmp2;
btVector3 tmp3;
rb.getLinearFactor( out tmp2 );
tmp.Mult( ref tmp2, out tmp3 );
solverBody.internalSetInvMass( ref tmp3 );
solverBody.m_originalBody = rb;
rb.getAngularFactor( out solverBody.m_angularFactor );
rb.getLinearFactor( out solverBody.m_linearFactor );
rb.getLinearVelocity( out solverBody.m_linearVelocity );
rb.getAngularVelocity( out solverBody.m_angularVelocity );
rb.m_totalForce.Mult( rb.m_inverseMass * timeStep, out solverBody.m_externalForceImpulse );
//= rb.getTotalForce() * rb.getInvMass() * timeStep;
rb.m_invInertiaTensorWorld.Apply( ref rb.m_totalTorque, out tmp );
tmp.Mult( timeStep, out solverBody.m_externalTorqueImpulse );
btScalar.Dbg( "Setup external impulses " + solverBody.m_externalForceImpulse + " " + solverBody.m_externalTorqueImpulse );
///solverBody.m_externalTorqueImpulse = rb.getTotalTorque() * rb.getInvInertiaTensorWorld() * timeStep;
}
else
{
solverBody.modified = false;
solverBody.m_worldTransform = btTransform.Identity;
solverBody.internalSetInvMass( ref btVector3.Zero );
solverBody.m_originalBody = null;
solverBody.m_angularFactor = btVector3.One;
solverBody.m_linearFactor = btVector3.One;
solverBody.m_linearVelocity = btVector3.Zero;
solverBody.m_angularVelocity = btVector3.Zero;
solverBody.m_externalForceImpulse = btVector3.Zero;
solverBody.m_externalTorqueImpulse = btVector3.Zero;
}
}
public override void localGetSupportingVertexWithoutMargin( ref btVector3 vec, out btVector3 result )
{
result = btVector3.Zero;
double maxDot = (double)( -btScalar.BT_LARGE_FLOAT );
// Here we take advantage of dot(a, b*c) = dot(a*b, c). Note: This is true mathematically, but not numerically.
if( 0 < m_unscaledPoints.Count )
{
btVector3 scaled; vec.Mult(ref m_localScaling, out scaled );
int index = (int)scaled.maxDot( m_unscaledPoints.InternalArray, m_unscaledPoints.Count, out maxDot ); // FIXME: may violate encapsulation of m_unscaledPoints
m_unscaledPoints[index].Mult( ref m_localScaling, out result );
}
//return supVec;
}
public void applyPushImpulse( ref btVector3 linearComponent, ref btVector3 angularComponent, double impulseMagnitude )
{
if( m_originalBody != null )
{
btVector3 tmp;
pushed = true;
linearComponent.Mult( ref m_linearFactor, out tmp );
m_pushVelocity.AddScale( ref tmp, impulseMagnitude, out m_pushVelocity );
//m_pushVelocity += linearComponent * impulseMagnitude * m_linearFactor;
angularComponent.Mult( ref m_angularFactor, out tmp );
m_turnVelocity.AddScale( ref tmp, impulseMagnitude, out m_turnVelocity );
//m_turnVelocity += angularComponent * ( impulseMagnitude * m_angularFactor );
}
}
static public double capsuleCapsuleDistance(
out btVector3 normalOnB,
out btVector3 pointOnB,
double capsuleLengthA,
double capsuleRadiusA,
double capsuleLengthB,
double capsuleRadiusB,
int capsuleAxisA,
int capsuleAxisB,
ref btTransform transformA,
ref btTransform transformB,
double distanceThreshold )
{
btVector3 directionA; transformA.m_basis.getColumn( capsuleAxisA, out directionA );
btVector3 translationA; transformA.getOrigin( out translationA );
btVector3 directionB; transformB.m_basis.getColumn( capsuleAxisB, out directionB );
btVector3 translationB; transformB.getOrigin( out translationB );
// translation between centers
btVector3 translation; translationB.Sub( ref translationA, out translation );
// compute the closest points of the capsule line segments
btVector3 ptsVector; // the vector between the closest points
btVector3 offsetA, offsetB; // offsets from segment centers to their closest points
double tA, tB; // parameters on line segment
segmentsClosestPoints( out ptsVector, out offsetA, out offsetB, out tA, out tB, ref translation,
ref directionA, capsuleLengthA, ref directionB, capsuleLengthB );
double distance = ptsVector.length() - capsuleRadiusA - capsuleRadiusB;
if( distance > distanceThreshold )
{
pointOnB = btVector3.Zero;
normalOnB = btVector3.xAxis;
return distance;
}
double lenSqr = ptsVector.length2();
if( lenSqr <= ( btScalar.SIMD_EPSILON * btScalar.SIMD_EPSILON ) )
{
//degenerate case where 2 capsules are likely at the same location: take a vector tangential to 'directionA'
btVector3 q;
btVector3.btPlaneSpace1( ref directionA, out normalOnB, out q );
}
else
{
// compute the contact normal
ptsVector.Mult( -btScalar.btRecipSqrt( lenSqr ), out normalOnB );
}
btVector3 tmp;
btVector3 tmp2;
translationB.Add( ref offsetB, out tmp );
normalOnB.Mult( capsuleRadiusB, out tmp2 );
tmp.Add( ref tmp2, out pointOnB );
//pointOnB = transformB.getOrigin() + offsetB + normalOnB * capsuleRadiusB;
return distance;
}
public virtual void drawAabb( ref btVector3 from, ref btVector3 to, ref btVector3 color )
{
btVector3 halfExtents; to.SubAndScale( ref from, 0.5f, out halfExtents );
btVector3 center; to.AddAndScale( ref from, 0.5f, out center );
int i, j;
btVector3 edgecoord = new btVector3( 1f, 1f, 1f ), pa, pb;
for( i = 0; i < 4; i++ )
{
for( j = 0; j < 3; j++ )
{
edgecoord.Mult( ref halfExtents, out pa );
pa.Add( ref center, out pa );
int othercoord = j % 3;
edgecoord[othercoord] *= -1f;
edgecoord.Mult( ref halfExtents, out pb );
pb.Add( ref center, out pb );
drawLine( ref pa, ref pb, ref color );
}
edgecoord.setValue( -1, -1, -1 );
if( i < 3 )
edgecoord[i] *= -1;
}
}
static public void segmentsClosestPoints(
out btVector3 ptsVector,
out btVector3 offsetA,
out btVector3 offsetB,
out double tA, out double tB,
ref btVector3 translation,
ref btVector3 dirA, double hlenA,
ref btVector3 dirB, double hlenB )
{
// compute the parameters of the closest points on each line segment
double dirA_dot_dirB = btVector3.btDot( ref dirA, ref dirB );
double dirA_dot_trans = btVector3.btDot( ref dirA, ref translation );
double dirB_dot_trans = btVector3.btDot( ref dirB, ref translation );
double denom = 1.0f - dirA_dot_dirB * dirA_dot_dirB;
if( denom == 0.0f )
{
tA = 0.0f;
}
else
{
tA = ( dirA_dot_trans - dirB_dot_trans * dirA_dot_dirB ) / denom;
if( tA < -hlenA )
tA = -hlenA;
else if( tA > hlenA )
tA = hlenA;
}
tB = tA * dirA_dot_dirB - dirB_dot_trans;
if( tB < -hlenB )
{
tB = -hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if( tA < -hlenA )
tA = -hlenA;
else if( tA > hlenA )
tA = hlenA;
}
else if( tB > hlenB )
{
tB = hlenB;
tA = tB * dirA_dot_dirB + dirA_dot_trans;
if( tA < -hlenA )
tA = -hlenA;
else if( tA > hlenA )
tA = hlenA;
}
// compute the closest points relative to segment centers.
dirA.Mult( tA, out offsetA );
dirB.Mult( tB, out offsetB );
btVector3 tmp;
translation.Sub( ref offsetA, out tmp );
tmp.Add( ref offsetB, out ptsVector );
//ptsVector = translation - offsetA + offsetB;
}
/*@brief Return a rotated version of this vector
@param wAxis The axis to rotate about
@param angle The angle to rotate by */
public void rotate( ref btVector3 wAxis, double angle, out btVector3 result )
{
btVector3 o;
wAxis.Mult( wAxis.dot( ref this ), out o );
btVector3 _x;
this.Sub( ref o, out _x );
btVector3 _y;
wAxis.cross( ref this, out _y );
btVector3 tmp;
btVector3 tmp2;
_x.Mult( btScalar.btCos( angle ), out tmp );
o.Add( ref tmp, out tmp2 );
_y.Mult( btScalar.btSin( angle ), out tmp );
tmp2.Add( ref tmp, out result );
}
///a btBox2dShape is a flat 2D box in the X-Y plane (Z extents are zero)
btBox2dShape( ref btVector3 boxHalfExtents )
{
m_centroid = btVector3.Zero;
m_vertices[0].setValue( -boxHalfExtents.x, -boxHalfExtents.y, 0 );
m_vertices[1].setValue( boxHalfExtents.x, -boxHalfExtents.y, 0 );
m_vertices[2].setValue( boxHalfExtents.x, boxHalfExtents.y, 0 );
m_vertices[3].setValue( -boxHalfExtents.x, boxHalfExtents.y, 0 );
m_normals[0].setValue( 0, -1, 0 );
m_normals[1].setValue( 1, 0, 0 );
m_normals[2].setValue( 0, 1, 0 );
m_normals[3].setValue( -1, 0, 0 );
double minDimension = boxHalfExtents.x;
if( minDimension > boxHalfExtents.y )
minDimension = boxHalfExtents.y;
setSafeMargin( minDimension );
m_shapeType = BroadphaseNativeTypes.BOX_2D_SHAPE_PROXYTYPE;
btVector3 margin = new btVector3( getMargin(), getMargin(), getMargin() );
btVector3 tmp;
boxHalfExtents.Mult( ref m_localScaling, out tmp );
tmp.Sub( ref margin, out m_implicitShapeDimensions );
}
///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
///result is conservative
public void calculateTemporalAabb( ref btTransform curTrans, ref btVector3 linvel, ref btVector3 angvel, double timeStep
, out btVector3 temporalAabbMin, out btVector3 temporalAabbMax )
{
//start with static aabb
getAabb( ref curTrans, out temporalAabbMin, out temporalAabbMax );
double temporalAabbMaxx = temporalAabbMax.x;
double temporalAabbMaxy = temporalAabbMax.y;
double temporalAabbMaxz = temporalAabbMax.z;
double temporalAabbMinx = temporalAabbMin.x;
double temporalAabbMiny = temporalAabbMin.y;
double temporalAabbMinz = temporalAabbMin.z;
// add linear motion
btVector3 linMotion;
linvel.Mult( timeStep, out linMotion );
///@todo: simd would have a vector max/min operation, instead of per-element access
if( linMotion.x > btScalar.BT_ZERO )
temporalAabbMaxx += linMotion.x;
else
temporalAabbMinx += linMotion.x;
if( linMotion.y > btScalar.BT_ZERO )
temporalAabbMaxy += linMotion.y;
else
temporalAabbMiny += linMotion.y;
if( linMotion.z > btScalar.BT_ZERO )
temporalAabbMaxz += linMotion.z;
else
temporalAabbMinz += linMotion.z;
//add conservative angular motion
double angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
btVector3 angularMotion3d = new btVector3( angularMotion, angularMotion, angularMotion );
temporalAabbMin = new btVector3( temporalAabbMinx, temporalAabbMiny, temporalAabbMinz );
temporalAabbMax = new btVector3( temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz );
temporalAabbMin.Sub( ref angularMotion3d, out temporalAabbMin );
temporalAabbMax.Add( ref angularMotion3d, out temporalAabbMax );
}
/*@brief Return the vector scaled by s */
public static void Mult( double s, ref btVector3 v, out btVector3 result )
{
v.Mult( s, out result );
}
public virtual void drawPlane( ref btVector3 planeNormal, double planeConst, ref btTransform transform, ref btVector3 color )
{
btVector3 planeOrigin; planeNormal.Mult( planeConst, out planeOrigin );
btVector3 vec0, vec1;
btVector3.btPlaneSpace1( ref planeNormal, out vec0, out vec1 );
double vecLen = 100;
vec0.Mult( vecLen, out vec0 );
vec1.Mult( vecLen, out vec1 );
btVector3 pt0; planeOrigin.Add( ref vec0, out pt0 );
btVector3 pt1; planeOrigin.Sub( ref vec0, out pt1 );
btVector3 pt2; planeOrigin.Add( ref vec1, out pt2 );
btVector3 pt3; planeOrigin.Sub( ref vec1, out pt3 );
transform.Apply( ref pt0, out pt0 );
transform.Apply( ref pt1, out pt1 );
transform.Apply( ref pt2, out pt2 );
transform.Apply( ref pt3, out pt3 );
drawLine( ref pt0, ref pt1, ref color );
drawLine( ref pt2, ref pt3, ref color );
}
btBoxShape( ref btVector3 boxHalfExtents )
{
m_shapeType = BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE;
setSafeMargin( ref boxHalfExtents );
btVector3 margin = new btVector3( getMargin(), getMargin(), getMargin() );
boxHalfExtents.Mult( ref m_localScaling, out m_implicitShapeDimensions );
m_implicitShapeDimensions.Sub( ref margin, out m_implicitShapeDimensions );
//m_implicitShapeDimensions = ( boxHalfExtents * m_localScaling ) - margin;
}
