public EntityProperties FromTransform(uint id, IndexedMatrix startTransform)
{
EntityProperties ret = new EntityProperties();
ID = id;
Position = startTransform._origin;
Rotation = startTransform.GetRotation();
return ret;
}
public void SetWorldTransform(ref IndexedMatrix worldTrans, bool force)
{
m_xform = worldTrans;
// Put the new transform into m_properties
IndexedQuaternion OrientationQuaternion = m_xform.GetRotation();
IndexedVector3 LinearVelocityVector = Rigidbody.GetLinearVelocity();
IndexedVector3 AngularVelocityVector = Rigidbody.GetAngularVelocity();
m_properties.Position = new Vector3(m_xform._origin.X, m_xform._origin.Y, m_xform._origin.Z);
m_properties.Rotation = new Quaternion(OrientationQuaternion.X, OrientationQuaternion.Y,
OrientationQuaternion.Z, OrientationQuaternion.W);
// 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 = new Vector3(LinearVelocityVector.X, LinearVelocityVector.Y, LinearVelocityVector.Z);
m_properties.RotationalVelocity = new Vector3(AngularVelocityVector.X, AngularVelocityVector.Y, AngularVelocityVector.Z);
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.RotationalVelocity == ZeroVect)
&&
(m_properties.Velocity != m_lastProperties.Velocity ||
m_properties.RotationalVelocity != m_lastProperties.RotationalVelocity))
// 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.RotationalVelocity, ref m_lastProperties.RotationalVelocity,
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 SimMotionState(BSAPIXNA pWorld, uint id, IndexedMatrix starTransform, object frameUpdates)
{
IndexedQuaternion OrientationQuaterion = starTransform.GetRotation();
m_properties = new EntityProperties()
{
ID = id,
Position = new Vector3(starTransform._origin.X, starTransform._origin.Y,starTransform._origin.Z),
Rotation = new Quaternion(OrientationQuaterion.X,OrientationQuaterion.Y,OrientationQuaterion.Z,OrientationQuaterion.W)
};
m_lastProperties = new EntityProperties()
{
ID = id,
Position = new Vector3(starTransform._origin.X, starTransform._origin.Y, starTransform._origin.Z),
Rotation = new Quaternion(OrientationQuaterion.X, OrientationQuaterion.Y, OrientationQuaterion.Z, OrientationQuaterion.W)
};
m_world = pWorld;
m_xform = starTransform;
}
public virtual void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
{
BulletGlobals.StartProfile("convexSweepTest");
/// use the broadphase to accelerate the search for objects, based on their aabb
/// and for each object with ray-aabb overlap, perform an exact ray test
/// unfortunately the implementation for rayTest and convexSweepTest duplicated, albeit practically identical
IndexedMatrix convexFromTrans;
IndexedMatrix convexToTrans;
convexFromTrans = convexFromWorld;
convexToTrans = convexToWorld;
IndexedVector3 castShapeAabbMin;
IndexedVector3 castShapeAabbMax;
/* Compute AABB that encompasses angular movement */
{
IndexedVector3 linVel;
IndexedVector3 angVel;
TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel);
IndexedVector3 zeroLinVel = new IndexedVector3();
IndexedMatrix R = IndexedMatrix.Identity;
R.SetRotation(convexFromTrans.GetRotation());
castShape.CalculateTemporalAabb(ref R, ref zeroLinVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax);
}
#if !USE_BRUTEFORCE_RAYBROADPHASE
SingleSweepCallback convexCB = BulletGlobals.SingleSweepCallbackPool.Get();
convexCB.Initialize(castShape, ref convexFromWorld, ref convexToWorld, this, resultCallback, allowedCcdPenetration);
IndexedVector3 tempFrom = convexFromTrans._origin;
IndexedVector3 tempTo = convexToTrans._origin;
m_broadphasePairCache.RayTest(ref tempFrom, ref tempTo, convexCB, ref castShapeAabbMin, ref castShapeAabbMax);
convexCB.Cleanup();
BulletGlobals.SingleSweepCallbackPool.Free(convexCB);
#else
/// go over all objects, and if the ray intersects their aabb + cast shape aabb,
// do a ray-shape query using convexCaster (CCD)
int i;
for (i=0;i<m_collisionObjects.Count;i++)
{
CollisionObject collisionObject= m_collisionObjects[i];
//only perform raycast if filterMask matches
if(resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
{
//RigidcollisionObject* collisionObject = ctrl.GetRigidcollisionObject();
IndexedVector3 collisionObjectAabbMin = new IndexedVector3();
IndexedVector3 collisionObjectAabbMax = new IndexedVector3();
collisionObject.GetCollisionShape().GetAabb(collisionObject.GetWorldTransform(),ref collisionObjectAabbMin,ref collisionObjectAabbMax);
AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
IndexedVector3 hitNormal = new IndexedVector3();
IndexedVector3 fromOrigin = convexFromWorld._origin;
IndexedVector3 toOrigin = convexToWorld._origin;
if (AabbUtil2.RayAabb(ref fromOrigin, ref toOrigin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, ref hitNormal))
{
IndexedMatrix trans = collisionObject.GetWorldTransform();
ObjectQuerySingle(castShape, ref convexFromTrans,ref convexToTrans,
collisionObject,
collisionObject.GetCollisionShape(),
ref trans,
resultCallback,
allowedCcdPenetration);
}
}
}
#endif //USE_BRUTEFORCE_RAYBROADPHASE
BulletGlobals.StopProfile();
}
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;
}
void InitSeparatingDistance(ref IndexedVector3 separatingVector, float separatingDistance, ref IndexedMatrix transA, ref IndexedMatrix transB)
{
m_separatingNormal = separatingVector;
m_separatingDistance = separatingDistance;
IndexedVector3 toPosA = transA._origin;
IndexedVector3 toPosB = transB._origin;
IndexedQuaternion toOrnA = transA.GetRotation();
IndexedQuaternion toOrnB = transB.GetRotation();
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
public void UpdateSeparatingDistance(ref IndexedMatrix transA, ref IndexedMatrix transB)
{
IndexedVector3 toPosA = transA._origin;
IndexedVector3 toPosB = transB._origin;
IndexedQuaternion toOrnA = transA.GetRotation();
IndexedQuaternion toOrnB = transB.GetRotation();
if (m_separatingDistance > 0.0f)
{
IndexedVector3 linVelA;
IndexedVector3 angVelA;
IndexedVector3 linVelB;
IndexedVector3 angVelB;
TransformUtil.CalculateVelocityQuaternion(ref m_posA, ref toPosA, ref m_ornA, ref toOrnA, 1f, out linVelA, out angVelA);
TransformUtil.CalculateVelocityQuaternion(ref m_posB, ref toPosB, ref m_ornB, ref toOrnB, 1f, out linVelB, out angVelB);
float maxAngularProjectedVelocity = angVelA.Length() * m_boundingRadiusA + angVelB.Length() * m_boundingRadiusB;
IndexedVector3 relLinVel = (linVelB - linVelA);
float relLinVelocLength = IndexedVector3.Dot((linVelB - linVelA), m_separatingNormal);
if (relLinVelocLength < 0f)
{
relLinVelocLength = 0f;
}
float projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
m_separatingDistance -= projectedMotion;
}
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
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;
}
public void CalcAngleInfo2(ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedBasisMatrix invInertiaWorldA, ref IndexedBasisMatrix invInertiaWorldB)
{
m_swingCorrection = 0;
m_twistLimitSign = 0;
m_solveTwistLimit = false;
m_solveSwingLimit = false;
// compute rotation of A wrt B (in constraint space)
if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
{ // it is assumed that setMotorTarget() was alredy called
// and motor target m_qTarget is within constraint limits
// TODO : split rotation to pure swing and pure twist
// compute desired transforms in world
IndexedMatrix trPose = IndexedMatrix.CreateFromQuaternion(m_qTarget);
IndexedMatrix trA = transA * m_rbAFrame;
IndexedMatrix trB = transB * m_rbBFrame;
IndexedMatrix trDeltaAB = trB * trPose * trA.Inverse();
IndexedQuaternion qDeltaAB = trDeltaAB.GetRotation();
IndexedVector3 swingAxis = new IndexedVector3(qDeltaAB.X, qDeltaAB.Y, qDeltaAB.Z);
float swingAxisLen2 = swingAxis.LengthSquared();
if (MathUtil.FuzzyZero(swingAxisLen2))
{
return;
}
m_swingAxis = swingAxis;
m_swingAxis.Normalize();
m_swingCorrection = MathUtil.QuatAngle(ref qDeltaAB);
if (!MathUtil.FuzzyZero(m_swingCorrection))
{
m_solveSwingLimit = true;
}
return;
}
{
// compute rotation of A wrt B (in constraint space)
// Not sure if these need order swapping as well?
IndexedQuaternion qA = transA.GetRotation() * m_rbAFrame.GetRotation();
IndexedQuaternion qB = transB.GetRotation() * m_rbBFrame.GetRotation();
IndexedQuaternion qAB = MathUtil.QuaternionInverse(qB) * qA;
// split rotation into cone and twist
// (all this is done from B's perspective. Maybe I should be averaging axes...)
IndexedVector3 vConeNoTwist = MathUtil.QuatRotate(ref qAB, ref vTwist);
vConeNoTwist.Normalize();
IndexedQuaternion qABCone = MathUtil.ShortestArcQuat(ref vTwist, ref vConeNoTwist);
qABCone.Normalize();
IndexedQuaternion qABTwist = MathUtil.QuaternionInverse(qABCone) * qAB;
qABTwist.Normalize();
if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
{
float swingAngle = 0f, swingLimit = 0f;
IndexedVector3 swingAxis = IndexedVector3.Zero;
ComputeConeLimitInfo(ref qABCone, ref swingAngle, ref swingAxis, ref swingLimit);
if (swingAngle > swingLimit * m_limitSoftness)
{
m_solveSwingLimit = true;
// compute limit ratio: 0->1, where
// 0 == beginning of soft limit
// 1 == hard/real limit
m_swingLimitRatio = 1f;
if (swingAngle < swingLimit && m_limitSoftness < 1f - MathUtil.SIMD_EPSILON)
{
m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness) /
(swingLimit - (swingLimit * m_limitSoftness));
}
// swing correction tries to get back to soft limit
m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
// adjustment of swing axis (based on ellipse normal)
AdjustSwingAxisToUseEllipseNormal(ref swingAxis);
// Calculate necessary axis & factors
m_swingAxis = MathUtil.QuatRotate(qB, -swingAxis);
m_twistAxisA = IndexedVector3.Zero;
m_kSwing = 1f /
(ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldA) +
ComputeAngularImpulseDenominator(ref m_swingAxis, ref invInertiaWorldB));
}
}
else
{
// you haven't set any limits;
// or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
// anyway, we have either hinge or fixed joint
IndexedVector3 ivA = transA._basis * m_rbAFrame._basis.GetColumn(0);
IndexedVector3 jvA = transA._basis * m_rbAFrame._basis.GetColumn(1);
IndexedVector3 kvA = transA._basis * m_rbAFrame._basis.GetColumn(2);
IndexedVector3 ivB = transB._basis * m_rbBFrame._basis.GetColumn(0);
IndexedVector3 target = IndexedVector3.Zero;
float x = ivB.Dot(ref ivA);
float y = ivB.Dot(ref jvA);
float z = ivB.Dot(ref kvA);
if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
{ // fixed. We'll need to add one more row to constraint
if ((!MathUtil.FuzzyZero(y)) || (!(MathUtil.FuzzyZero(z))))
{
m_solveSwingLimit = true;
m_swingAxis = -ivB.Cross(ref ivA);
}
}
else
{
if (m_swingSpan1 < m_fixThresh)
{ // hinge around Y axis
if (!(MathUtil.FuzzyZero(y)))
{
m_solveSwingLimit = true;
if (m_swingSpan2 >= m_fixThresh)
{
y = 0;
float span2 = (float)Math.Atan2(z, x);
if (span2 > m_swingSpan2)
{
x = (float)Math.Cos(m_swingSpan2);
z = (float)Math.Sin(m_swingSpan2);
}
else if (span2 < -m_swingSpan2)
{
x = (float)Math.Cos(m_swingSpan2);
z = -(float)Math.Sin(m_swingSpan2);
}
}
}
}
else
{ // hinge around Z axis
if (!MathUtil.FuzzyZero(z))
{
m_solveSwingLimit = true;
if (m_swingSpan1 >= m_fixThresh)
{
z = 0f;
float span1 = (float)Math.Atan2(y, x);
if (span1 > m_swingSpan1)
{
x = (float)Math.Cos(m_swingSpan1);
y = (float)Math.Sin(m_swingSpan1);
}
else if (span1 < -m_swingSpan1)
{
x = (float)Math.Cos(m_swingSpan1);
y = -(float)Math.Sin(m_swingSpan1);
}
}
}
}
target.X = x * ivA.X + y * jvA.X + z * kvA.X;
target.Y = x * ivA.Y + y * jvA.Y + z * kvA.Y;
target.Z = x * ivA.Z + y * jvA.Z + z * kvA.Z;
target.Normalize();
m_swingAxis = -(ivB.Cross(ref target));
m_swingCorrection = m_swingAxis.Length();
m_swingAxis.Normalize();
}
}
if (m_twistSpan >= 0f)
{
IndexedVector3 twistAxis;
ComputeTwistLimitInfo(ref qABTwist, out m_twistAngle, out twistAxis);
if (m_twistAngle > m_twistSpan * m_limitSoftness)
{
m_solveTwistLimit = true;
m_twistLimitRatio = 1f;
if (m_twistAngle < m_twistSpan && m_limitSoftness < 1f - MathUtil.SIMD_EPSILON)
{
m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness) /
(m_twistSpan - m_twistSpan * m_limitSoftness);
}
// twist correction tries to get back to soft limit
m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
m_twistAxis = MathUtil.QuatRotate(qB, -twistAxis);
m_kTwist = 1f /
(ComputeAngularImpulseDenominator(ref m_twistAxis, ref invInertiaWorldA) +
ComputeAngularImpulseDenominator(ref m_twistAxis, ref invInertiaWorldB));
}
if (m_solveSwingLimit)
{
m_twistAxisA = MathUtil.QuatRotate(qA, -twistAxis);
}
}
else
{
m_twistAngle = 0f;
}
}
}
