public override void SetLocalScaling(ref IndexedVector3 scaling)
{
for (int i = 0; i < m_children.Count; i++)
{
IndexedMatrix childTrans = GetChildTransform(i);
IndexedVector3 childScale = m_children[i].m_childShape.GetLocalScaling();
// childScale = childScale * (childTrans.getBasis() * scaling);
childScale = childScale * scaling / m_localScaling;
m_children[i].m_childShape.SetLocalScaling(ref childScale);
childTrans._origin = ((childTrans._origin) * scaling);
UpdateChildTransform(i, ref childTrans, false);
}
m_localScaling = scaling;
RecalculateLocalAabb();
}
public void RenderDebugLines(GameTime gameTime, ref IndexedMatrix view, ref IndexedMatrix projection)
{
m_debugEffect.World = Matrix.Identity;
m_debugEffect.View = view.ToMatrix();
m_debugEffect.Projection = projection.ToMatrixProjection();
if (m_lineIndex > 0)
{
foreach (EffectPass pass in m_debugEffect.CurrentTechnique.Passes)
{
pass.Apply();
m_game.GraphicsDevice.DrawUserPrimitives <VertexPositionColor>(PrimitiveType.LineList, m_lineVertices, 0, m_lineIndex / 2);
}
}
m_lineIndex = 0;
}
private void CreateScene5(int dim)
{
BoxShape boxShape = new BoxShape(new IndexedVector3(0.5f));
float mass = 1.0f;
for (int x = 0; x < dim; x++)
{
for (int e = x; e < dim; e++)
{
//IndexedVector3 pos = new IndexedVector3(e - 0.5f * x, x * 1.01f - 14, 25);
IndexedVector3 pos = new IndexedVector3(e - 0.5f * x, x * 1.0f, 0);
RigidBody rb = LocalCreateRigidBody(mass, IndexedMatrix.CreateTranslation(pos), boxShape);
//m_dynamicsWorld.AddRigidBody(rb);
}
}
}
public virtual void Project(ref IndexedMatrix trans, ref IndexedVector3 dir, ref float min, ref float max)
{
IndexedVector3 localAxis = dir * trans._basis;
IndexedVector3 vtx1 = trans * LocalGetSupportingVertex(localAxis);
IndexedVector3 vtx2 = trans * LocalGetSupportingVertex(-localAxis);
min = vtx1.Dot(dir);
max = vtx2.Dot(dir);
if (min > max)
{
float tmp = min;
min = max;
max = tmp;
}
}
public override void GetAabb(ref IndexedMatrix trans, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
IndexedVector3 halfExtents = GetHalfExtentsWithoutMargin();
AabbUtil2.TransformAabb(ref halfExtents, GetMargin(), ref trans, out aabbMin, out aabbMax);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxShape)
{
BulletGlobals.g_streamWriter.WriteLine("box::getAabb");
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "halfExtentWithout", GetHalfExtentsWithoutMargin());
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transform", trans);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "outMin", aabbMin);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "outMax", aabbMax);
}
#endif
}
public override void GetAabb(ref IndexedMatrix trans, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
IndexedVector3 halfExtents = new IndexedVector3(GetRadius());
halfExtents[m_upAxis] = GetRadius() + GetHalfHeight();
halfExtents += new IndexedVector3(GetMargin());
IndexedBasisMatrix abs_b = trans._basis.Absolute();
IndexedVector3 center = trans._origin;
IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents),
abs_b._el1.Dot(ref halfExtents),
abs_b._el2.Dot(ref halfExtents));
aabbMin = center - extent;
aabbMax = center + extent;
}
public void DrawBox(ref IndexedVector3 boxMin, ref IndexedVector3 boxMax, ref IndexedMatrix transform, ref IndexedVector3 color, float alpha)
{
DrawLine(transform * (new IndexedVector3(boxMin.X, boxMin.Y, boxMin.Z)), transform * (new IndexedVector3(boxMax.X, boxMin.Y, boxMin.Z)), color);
DrawLine(transform * (new IndexedVector3(boxMax.X, boxMin.Y, boxMin.Z)), transform * (new IndexedVector3(boxMax.X, boxMax.Y, boxMin.Z)), color);
DrawLine(transform * (new IndexedVector3(boxMax.X, boxMax.Y, boxMin.Z)), transform * (new IndexedVector3(boxMin.X, boxMax.Y, boxMin.Z)), color);
DrawLine(transform * (new IndexedVector3(boxMin.X, boxMax.Y, boxMin.Z)), transform * (new IndexedVector3(boxMin.X, boxMin.Y, boxMin.Z)), color);
DrawLine(transform * (new IndexedVector3(boxMin.X, boxMin.Y, boxMin.Z)), transform * (new IndexedVector3(boxMin.X, boxMin.Y, boxMax.Z)), color);
DrawLine(transform * (new IndexedVector3(boxMax.X, boxMin.Y, boxMin.Z)), transform * (new IndexedVector3(boxMax.X, boxMin.Y, boxMax.Z)), color);
DrawLine(transform * (new IndexedVector3(boxMax.X, boxMax.Y, boxMin.Z)), transform * (new IndexedVector3(boxMax.X, boxMax.Y, boxMax.Z)), color);
DrawLine(transform * (new IndexedVector3(boxMin.X, boxMax.Y, boxMin.Z)), transform * (new IndexedVector3(boxMin.X, boxMax.Y, boxMax.Z)), color);
DrawLine(transform * (new IndexedVector3(boxMin.X, boxMin.Y, boxMax.Z)), transform * (new IndexedVector3(boxMax.X, boxMin.Y, boxMax.Z)), color);
DrawLine(transform * (new IndexedVector3(boxMax.X, boxMin.Y, boxMax.Z)), transform * (new IndexedVector3(boxMax.X, boxMax.Y, boxMax.Z)), color);
DrawLine(transform * (new IndexedVector3(boxMax.X, boxMax.Y, boxMax.Z)), transform * (new IndexedVector3(boxMin.X, boxMax.Y, boxMax.Z)), color);
DrawLine(transform * (new IndexedVector3(boxMin.X, boxMax.Y, boxMax.Z)), transform * (new IndexedVector3(boxMin.X, boxMin.Y, boxMax.Z)), color);
}
public static void IntegrateTransform(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep,out IndexedMatrix predictedTransform)
{
predictedTransform = IndexedMatrix.CreateTranslation(curTrans._origin + linvel * timeStep);
// #define QUATERNION_DERIVATIVE
#if QUATERNION_DERIVATIVE
IndexedVector3 pos;
IndexedQuaternion predictedOrn;
IndexedVector3 scale;
curTrans.Decompose(ref scale, ref predictedOrn, ref pos);
predictedOrn += (angvel * predictedOrn) * (timeStep * .5f));
predictedOrn.Normalize();
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
IndexedVector3 axis;
float fAngle = angvel.Length();
//limit the angular motion
if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
{
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
}
if ( fAngle < 0.001f )
{
// use Taylor's expansions of sync function
axis = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel*( (float)Math.Sin(0.5f*fAngle*timeStep)/fAngle );
}
IndexedQuaternion dorn = new IndexedQuaternion(axis.X,axis.Y,axis.Z,(float)Math.Cos( fAngle*timeStep*.5f) );
IndexedQuaternion orn0 = curTrans.GetRotation();
IndexedQuaternion predictedOrn = dorn * orn0;
predictedOrn.Normalize();
#endif
IndexedMatrix newMatrix = IndexedMatrix.CreateFromQuaternion(predictedOrn);
predictedTransform._basis = newMatrix._basis;
}
/// continuous collision detection needs prediction
public void PredictIntegratedTransform(float timeStep, out IndexedMatrix predictedTransform)
{
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugRigidBody)
{
BulletGlobals.g_streamWriter.WriteLine("[{0}] predictIntegratedTransform pre", (String)m_userObjectPointer);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, m_worldTransform);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "LinearVel", m_linearVelocity);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "AngularVel", m_angularVelocity);
}
TransformUtil.IntegrateTransform(ref m_worldTransform, ref m_linearVelocity, ref m_angularVelocity, timeStep, out predictedTransform);
MathUtil.SanityCheckVector(m_worldTransform._basis[1]);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugRigidBody)
{
BulletGlobals.g_streamWriter.WriteLine("[{0}] predictIntegratedTransform post", (String)m_userObjectPointer);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, predictedTransform);
}
}
public SimMotionState(CollisionWorld pWorld, uint id, IndexedMatrix starTransform, object frameUpdates)
{
m_properties = new EntityProperties()
{
ID = id,
Position = starTransform._origin,
Rotation = starTransform.GetRotation()
};
m_lastProperties = new EntityProperties()
{
ID = id,
Position = starTransform._origin,
Rotation = starTransform.GetRotation()
};
m_world = pWorld;
m_xform = starTransform;
}
public static float CapsuleCapsuleDistance(
out IndexedVector3 normalOnB,
out IndexedVector3 pointOnB,
float capsuleLengthA,
float capsuleRadiusA,
float capsuleLengthB,
float capsuleRadiusB,
int capsuleAxisA,
int capsuleAxisB,
IndexedMatrix transformA,
IndexedMatrix transformB,
float distanceThreshold)
{
return(CapsuleCapsuleDistance(out normalOnB, out pointOnB, capsuleLengthA,
capsuleRadiusA, capsuleLengthB, capsuleRadiusB, capsuleAxisA, capsuleAxisB,
ref transformA, ref transformB, distanceThreshold));
}
public override void GetAabb(ref IndexedMatrix t, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
#if true
base.GetAabb(ref t, out aabbMin, out aabbMax);
#else
aabbMin = MathUtil.MAX_VECTOR;
aabbMax = MathUtil.MIN_VECTOR;
//just transform the vertices in worldspace, and take their AABB
for (int i = 0; i < m_numVertices; i++)
{
IndexedVector3 worldVertex = IndexedVector3.Transformt(m_vertices[i], t);
MathUtil.vectorMin(ref worldVertex, ref aabbMin);
MathUtil.vectorMin(ref worldVertex, ref aabbMax);
}
#endif
}
protected void GImpactVsShapeFindPairs(
ref IndexedMatrix trans0,
ref IndexedMatrix trans1,
GImpactShapeInterface shape0,
CollisionShape shape1,
ObjectArray <int> collided_primitives)
{
AABB boxshape = new AABB();
if (shape0.HasBoxSet())
{
IndexedMatrix trans1to0 = trans0.Inverse();
//trans1to0 *= trans1;
trans1to0 = trans1to0 * trans1;
//trans1to0 = MathUtil.BulletMatrixMultiply(trans1,trans1to0);
shape1.GetAabb(ref trans1to0, out boxshape.m_min, out boxshape.m_max);
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "GImpactAglo::GImpactVsShapeFindPairs trans1to0", trans1to0);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box min", boxshape.m_min);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box max", boxshape.m_max);
}
#endif
shape0.GetBoxSet().BoxQuery(ref boxshape, collided_primitives);
}
else
{
shape1.GetAabb(ref trans1, out boxshape.m_min, out boxshape.m_max);
AABB boxshape0 = new AABB();
int i = shape0.GetNumChildShapes();
while (i-- != 0)
{
shape0.GetChildAabb(i, ref trans0, out boxshape0.m_min, out boxshape0.m_max);
if (boxshape.HasCollision(ref boxshape0))
{
collided_primitives.Add(i);
}
}
}
}
public override void Process(DbvtNode leaf)
{
int index = leaf.dataAsInt;
CompoundShape compoundShape = (CompoundShape)(m_compoundColObj.GetCollisionShape());
CollisionShape childShape = compoundShape.GetChildShape(index);
if (m_dispatchInfo.getDebugDraw() != null && (((m_dispatchInfo.getDebugDraw().GetDebugMode() & DebugDrawModes.DBG_DrawAabb)) != 0))
{
IndexedVector3 worldAabbMin;
IndexedVector3 worldAabbMax;
IndexedMatrix orgTrans = m_compoundColObj.GetWorldTransform();
AabbUtil2.TransformAabb(leaf.volume.Mins(), leaf.volume.Maxs(), 0f, ref orgTrans, out worldAabbMin, out worldAabbMax);
m_dispatchInfo.getDebugDraw().DrawAabb(worldAabbMin, worldAabbMax, new IndexedVector3(1, 0, 0));
}
ProcessChildShape(childShape, index);
}
public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
m_dispatchInfoPtr = dispatchInfo;
m_collisionMarginTriangle = collisionMarginTriangle;
m_resultOut = resultOut;
//recalc aabbs
//IndexedMatrix convexInTriangleSpace = MathUtil.bulletMatrixMultiply(IndexedMatrix.Invert(m_triBody.getWorldTransform()) , m_convexBody.getWorldTransform());
IndexedMatrix convexInTriangleSpace = m_triBody.GetWorldTransform().Inverse() * m_convexBody.GetWorldTransform();
CollisionShape convexShape = m_convexBody.GetCollisionShape();
convexShape.GetAabb(ref convexInTriangleSpace, out m_aabbMin, out m_aabbMax);
float extraMargin = collisionMarginTriangle;
IndexedVector3 extra = new IndexedVector3(extraMargin);
m_aabbMax += extra;
m_aabbMin -= extra;
}
public override void CalculateLocalInertia(float mass, out IndexedVector3 inertia)
{
//approximation: take the inertia from the aabb for now
IndexedMatrix ident = IndexedMatrix.Identity;
IndexedVector3 aabbMin;
IndexedVector3 aabbMax;
GetAabb(ref ident, out aabbMin, out aabbMax);
IndexedVector3 halfExtents = (aabbMax - aabbMin) * .5f;
float lx = 2f * (halfExtents.X);
float ly = 2f * (halfExtents.Y);
float lz = 2f * (halfExtents.Z);
inertia = new IndexedVector3(mass / (12.0f) * (ly * ly + lz * lz),
mass / (12.0f) * (lx * lx + lz * lz),
mass / (12.0f) * (lx * lx + ly * ly));
}
public override void SetAxis(ref IndexedVector3 axis1, ref IndexedVector3 axis2)
{
IndexedVector3 zAxis = IndexedVector3.Normalize(axis1);
IndexedVector3 yAxis = IndexedVector3.Normalize(axis2);
IndexedVector3 xAxis = IndexedVector3.Cross(yAxis, zAxis); // we want right coordinate system
IndexedMatrix frameInW = IndexedMatrix.Identity;
frameInW._basis = new IndexedBasisMatrix(xAxis.X, yAxis.X, zAxis.X,
xAxis.Y, yAxis.Y, zAxis.Y,
xAxis.Z, yAxis.Z, zAxis.Z);
// now get constraint frame in local coordinate systems
m_frameInA = m_rbA.GetCenterOfMassTransform().Inverse() * frameInW;
m_frameInB = m_rbB.GetCenterOfMassTransform().Inverse() * frameInW;
CalculateTransforms();
}
public void ShootTrimesh(IndexedVector3 startPosition, IndexedVector3 destination)
{
if (m_dynamicsWorld != null)
{
float mass = 4.0f;
IndexedMatrix startTransform = IndexedMatrix.Identity;
startTransform._origin = new IndexedVector3(startPosition);
RigidBody body = LocalCreateRigidBody(mass, startTransform, m_trimeshShape2);
IndexedVector3 linVel = new IndexedVector3(destination - startPosition);
linVel.Normalize();
linVel *= m_ShootBoxInitialSpeed * 0.25f;
body.SetLinearVelocity(ref linVel);
body.SetAngularVelocity(IndexedVector3.Zero);
}
}
static void FindIncidentEdge(ClipVertex[] c,
Box2dShape poly1, ref IndexedMatrix xf1, int edge1,
Box2dShape poly2, ref IndexedMatrix xf2)
{
IndexedVector3[] normals1 = poly1.GetNormals();
int count2 = poly2.GetVertexCount();
IndexedVector3[] vertices2 = poly2.GetVertices();
IndexedVector3[] normals2 = poly2.GetNormals();
Debug.Assert(0 <= edge1 && edge1 < poly1.GetVertexCount());
// Get the normal of the reference edge in poly2's frame.
IndexedVector3 normal1 = xf2._basis.Transpose() * (xf1._basis * normals1[edge1]);
// Find the incident edge on poly2.
int index = 0;
float minDot = MathUtil.BT_LARGE_FLOAT;
for (int i = 0; i < count2; ++i)
{
float dot = normal1.Dot(normals2[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c[0].v = xf2 * vertices2[i1];
// c[0].id.features.referenceEdge = (unsigned char)edge1;
// c[0].id.features.incidentEdge = (unsigned char)i1;
// c[0].id.features.incidentVertex = 0;
c[1].v = xf2 * vertices2[i2];
// c[1].id.features.referenceEdge = (unsigned char)edge1;
// c[1].id.features.incidentEdge = (unsigned char)i2;
// c[1].id.features.incidentVertex = 1;
}
public void SetWorldTransform(ref IndexedMatrix worldTrans, bool force)
{
m_xform = worldTrans;
// Put the new transform into m_properties
m_properties.Position = m_xform._origin;
m_properties.Rotation = m_xform.GetRotation();
// A problem with stock Bullet is that we don't get an event when an object is deactivated.
// This means that the last non-zero values for linear and angular velocity
// are left in the viewer who does dead reconning and the objects look like
// they float off.
// BulletSim ships with a patch to Bullet which creates such an event.
m_properties.Velocity = Rigidbody.GetLinearVelocity();
m_properties.AngularVelocity = Rigidbody.GetAngularVelocity();
if (force
|| !AlmostEqual(ref m_lastProperties.Position, ref m_properties.Position, POSITION_TOLERANCE) ||
!AlmostEqual(ref m_properties.Rotation, ref m_lastProperties.Rotation, ROTATION_TOLERANCE)
// If the Velocity and AngularVelocity are zero, most likely the object has
// been deactivated. If they both are zero and they have become zero recently,
// make sure a property update is sent so the zeros make it to the viewer.
|| ((m_properties.Velocity == ZeroVect && m_properties.AngularVelocity == ZeroVect)
&&
(m_properties.Velocity != m_lastProperties.Velocity ||
m_properties.AngularVelocity != m_lastProperties.AngularVelocity))
// If Velocity and AngularVelocity are non-zero but have changed, send an update.
|| !AlmostEqual(ref m_properties.Velocity, ref m_lastProperties.Velocity, VELOCITY_TOLERANCE)
||
!AlmostEqual(ref m_properties.AngularVelocity, ref m_lastProperties.AngularVelocity,
ANGULARVELOCITY_TOLERANCE)
)
{
// Add this update to the list of updates for this frame.
m_lastProperties = m_properties;
if (m_world.LastEntityProperty < m_world.UpdatedObjects.Length)
{
m_world.UpdatedObjects[m_world.LastEntityProperty++] = (m_properties);
}
//(*m_updatesThisFrame)[m_properties.ID] = &m_properties;
}
}
public static bool Penetration(ConvexShape shape0, ref IndexedMatrix wtrs0, ConvexShape shape1, ref IndexedMatrix wtrs1, ref IndexedVector3 guess, ref GjkEpaSolver2Results results, bool usemargins)
{
using (GjkEpaSolver2MinkowskiDiff shape = BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.Get())
using (GJK gjk = BulletGlobals.GJKPool.Get())
{
Initialize(shape0, ref wtrs0, shape1, ref wtrs1, ref results, shape, usemargins);
gjk.Initialise();
IndexedVector3 minusGuess = -guess;
GJKStatus gjk_status = gjk.Evaluate(shape, ref minusGuess);
switch (gjk_status)
{
case GJKStatus.Inside:
{
//EPA epa = new EPA();
eStatus epa_status = epa.Evaluate(gjk, ref minusGuess);
if (epa_status != eStatus.Failed)
{
IndexedVector3 w0 = IndexedVector3.Zero;
for (uint i = 0; i < epa.m_result.rank; ++i)
{
// order of results here is 'different' , EPA.evaluate.
w0 += shape.Support(ref epa.m_result.c[i].d, 0) * epa.m_result.p[i];
}
results.status = GjkEpaSolver2Status.Penetrating;
results.witnesses0 = wtrs0 * w0;
results.witnesses1 = wtrs0 * (w0 - epa.m_normal * epa.m_depth);
results.normal = -epa.m_normal;
results.distance = -epa.m_depth;
return(true);
}
else
{
results.status = GjkEpaSolver2Status.EPA_Failed;
}
}
break;
case GJKStatus.Failed:
results.status = GjkEpaSolver2Status.GJK_Failed;
break;
}
}
return(false);
}
public void StepUp(CollisionWorld collisionWorld)
{
// phase 1: up
IndexedMatrix start = IndexedMatrix.Identity, end = IndexedMatrix.Identity;
m_targetPosition = m_currentPosition + upAxisDirection[m_upAxis] * (m_stepHeight + (m_verticalOffset > 0.0f ? m_verticalOffset : 0.0f));
/* FIXME: Handle penetration properly */
start._origin = (m_currentPosition + upAxisDirection[m_upAxis] * (m_convexShape.GetMargin() + m_addedMargin));
end._origin = (m_targetPosition);
KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(m_ghostObject, -upAxisDirection[m_upAxis], 0.7071f);
callback.m_collisionFilterGroup = GetGhostObject().GetBroadphaseHandle().m_collisionFilterGroup;
callback.m_collisionFilterMask = GetGhostObject().GetBroadphaseHandle().m_collisionFilterMask;
if (m_useGhostObjectSweepTest)
{
m_ghostObject.ConvexSweepTest(m_convexShape, ref start, ref end, callback, collisionWorld.GetDispatchInfo().GetAllowedCcdPenetration());
}
else
{
collisionWorld.ConvexSweepTest(m_convexShape, ref start, ref end, callback, 0f);
}
if (callback.HasHit())
{
// Only modify the position if the hit was a slope and not a wall or ceiling.
if (IndexedVector3.Dot(callback.m_hitNormalWorld, upAxisDirection[m_upAxis]) > 0.0)
{
// we moved up only a fraction of the step height
m_currentStepOffset = m_stepHeight * callback.m_closestHitFraction;
m_currentPosition = MathUtil.Interpolate3(ref m_currentPosition, ref m_targetPosition, callback.m_closestHitFraction);
}
m_verticalVelocity = 0.0f;
m_verticalOffset = 0.0f;
}
else
{
m_currentStepOffset = m_stepHeight;
m_currentPosition = m_targetPosition;
}
}
public override void ClientMoveAndDisplay(GameTime gameTime)
{
IndexedVector3 walkDirection = IndexedVector3.Zero;
float walkVelocity = 1.1f * 4.0f;
float walkSpeed = walkVelocity * gameTime.ElapsedGameTime.Milliseconds / 1000.0f;
IndexedMatrix xform = m_ghostObject.GetWorldTransform();
IndexedVector3 forwardDir = xform._basis[2];
IndexedVector3 upDir = xform._basis[1];
IndexedVector3 strafeDir = xform._basis[0];
forwardDir.Normalize();
upDir.Normalize();
strafeDir.Normalize();
KeyboardState keyboardState = Keyboard.GetState();
if (keyboardState.IsKeyDown(Keys.I))
{
walkDirection += forwardDir;
}
if (keyboardState.IsKeyDown(Keys.K))
{
walkDirection -= forwardDir;
}
if (keyboardState.IsKeyDown(Keys.J))
{
IndexedMatrix orn = m_ghostObject.GetWorldTransform();
orn._basis *= IndexedBasisMatrix.CreateFromAxisAngle(new IndexedVector3(0, 1, 0), 0.01f);
m_ghostObject.SetWorldTransform(orn);
}
if (keyboardState.IsKeyDown(Keys.L))
{
IndexedMatrix orn = m_ghostObject.GetWorldTransform();
orn._basis *= IndexedBasisMatrix.CreateFromAxisAngle(new IndexedVector3(0, 1, 0), -0.01f);
m_ghostObject.SetWorldTransform(orn);
}
IndexedVector3 result = walkDirection * walkSpeed;
m_character.SetWalkDirection(ref result);
base.ClientMoveAndDisplay(gameTime);
}
public override void GetAabb(ref IndexedMatrix trans, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
IndexedVector3 localHalfExtents = 0.5f * (m_localAabbMax - m_localAabbMin);
float margin = GetMargin();
localHalfExtents += new IndexedVector3(margin);
IndexedVector3 localCenter = 0.5f * (m_localAabbMax + m_localAabbMin);
IndexedBasisMatrix abs_b = trans._basis.Absolute();
IndexedVector3 center = trans * localCenter;
IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref localHalfExtents),
abs_b._el1.Dot(ref localHalfExtents),
abs_b._el2.Dot(ref localHalfExtents));
aabbMin = center - extent;
aabbMax = center + extent;
}
public void RenderOthers(GameTime gameTime, ref IndexedMatrix view, ref IndexedMatrix projection)
{
m_spriteBatch.Begin();
for (int i = 0; i < m_textPositionColours.Count; ++i)
{
m_spriteBatch.DrawString(m_spriteFont, m_textPositionColours[i].m_text, m_textPositionColours[i].m_position, m_textPositionColours[i].m_color);
}
// set mouse cursor to true so we can see where we're aiming/picking.
m_game.IsMouseVisible = true;
//if(m_game.Mou
m_spriteBatch.End();
m_textPositionColours.Clear();
// restore from sprite batch changes.
m_game.GraphicsDevice.BlendState = BlendState.Opaque;
m_game.GraphicsDevice.DepthStencilState = DepthStencilState.Default;
}
public RigidBodyConstructionInfo(float mass, IMotionState motionState, CollisionShape collisionShape, IndexedVector3 localInertia)
{
m_mass = mass;
m_motionState = motionState;
m_collisionShape = collisionShape;
m_localInertia = localInertia;
m_linearDamping = 0f;
m_angularDamping = 0f;
m_friction = 0.5f;
m_restitution = 0f;
m_linearSleepingThreshold = 0.8f;
m_angularSleepingThreshold = 1f;
m_additionalDamping = false;
m_additionalDampingFactor = 0.005f;
m_additionalLinearDampingThresholdSqr = 0.01f;
m_additionalAngularDampingThresholdSqr = 0.01f;
m_additionalAngularDampingFactor = 0.01f;
m_startWorldTransform = IndexedMatrix.Identity;
}
public override void GetAabb(ref IndexedMatrix t, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
{
IndexedVector3 halfExtents = (m_localAabbMax - m_localAabbMin) * m_localScaling * 0.5f;
IndexedVector3 localOrigin = IndexedVector3.Zero;
localOrigin[m_upAxis] = (m_minHeight + m_maxHeight) * 0.5f;
localOrigin *= m_localScaling;
IndexedBasisMatrix abs_b = t._basis.Absolute();
IndexedVector3 center = t._origin;
IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents),
abs_b._el1.Dot(ref halfExtents),
abs_b._el2.Dot(ref halfExtents));
extent += new IndexedVector3(GetMargin());
aabbMin = center - extent;
aabbMax = center + extent;
}
/** Re-calculate the local Aabb. Is called at the end of removeChildShapes.
* Use this yourself if you modify the children or their transforms. */
public virtual void RecalculateLocalAabb()
{
// Recalculate the local aabb
// Brute force, it iterates over all the shapes left.
m_localAabbMin = new IndexedVector3(float.MaxValue);
m_localAabbMax = new IndexedVector3(float.MinValue);
//extend the local aabbMin/aabbMax
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
for (int j = 0; j < m_children.Count; j++)
{
IndexedMatrix foo = m_children[j].m_transform;
m_children[j].m_childShape.GetAabb(ref foo, out localAabbMin, out localAabbMax);
MathUtil.VectorMin(ref localAabbMin, ref m_localAabbMin);
MathUtil.VectorMax(ref localAabbMax, ref m_localAabbMax);
}
}
public void UpdateChildTransform(int childIndex, ref IndexedMatrix newChildTransform, bool shouldRecalculateLocalAabb)
{
m_children[childIndex].m_transform = newChildTransform;
if (m_dynamicAabbTree != null)
{
///update the dynamic aabb tree
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
m_children[childIndex].m_childShape.GetAabb(ref newChildTransform, out localAabbMin, out localAabbMax);
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
//int index = m_children.Count - 1;
m_dynamicAabbTree.Update(m_children[childIndex].m_treeNode, ref bounds);
}
if (shouldRecalculateLocalAabb)
{
RecalculateLocalAabb();
}
}
public void Project(ref IndexedMatrix trans, ref IndexedVector3 dir, ref float min, ref float max)
{
#if true
min = float.MaxValue;
max = float.MinValue;
IndexedVector3 witnesPtMin;
IndexedVector3 witnesPtMax;
int numVerts = m_unscaledPoints.Count;
for (int i = 0; i < numVerts; i++)
{
IndexedVector3 vtx = m_unscaledPoints[i] * m_localScaling;
IndexedVector3 pt = trans * vtx;
float dp = pt.Dot(dir);
if (dp < min)
{
min = dp;
witnesPtMin = pt;
}
if (dp > max)
{
max = dp;
witnesPtMax = pt;
}
}
#else
btVector3 localAxis = dir * trans.getBasis();
btVector3 vtx1 = trans(localGetSupportingVertex(localAxis));
btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis));
min = vtx1.dot(dir);
max = vtx2.dot(dir);
#endif
if (min > max)
{
float tmp = min;
min = max;
max = tmp;
}
}
