public void SetRotation(ref IndexedVector3 axis, float angle) { float d = axis.Length(); Debug.Assert(d != 0.0f); float halfAngle = angle * 0.5f; float s = (float)Math.Sin(halfAngle) / d; X = axis.X * s; Y = axis.Y * s; Z = axis.Z * s; W = (float)Math.Cos(halfAngle); }
public void SetRotation(ref IndexedVector3 axis, float angle) { float d = axis.Length(); Debug.Assert(d != 0.0f); float halfAngle = angle * 0.5f; #if UNITY float s = (d > UnityEngine.Mathf.Epsilon) ? (UnityEngine.Mathf.Sin(halfAngle) / d) : 0.0f; #else float s = (float)Math.Sin(halfAngle) / d; #endif X = axis.X * s; Y = axis.Y * s; Z = axis.Z * s; #if UNITY W = UnityEngine.Mathf.Cos(halfAngle); #else W = (float)Math.Cos(halfAngle); #endif }
protected IndexedVector3 ConeLocalSupport(ref IndexedVector3 v) { float halfHeight = m_height * 0.5f; if (v[m_coneIndices[1]] > v.Length() * m_sinAngle) { IndexedVector3 tmp = new IndexedVector3(); tmp[m_coneIndices[0]] = 0.0f; tmp[m_coneIndices[1]] = halfHeight; tmp[m_coneIndices[2]] = 0.0f; return tmp; } else { float s = (float)Math.Sqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]); if (s > MathUtil.SIMD_EPSILON) { float d = m_radius / s; IndexedVector3 tmp = new IndexedVector3(); tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d; tmp[m_coneIndices[1]] = -halfHeight; tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d; return tmp; } else { IndexedVector3 tmp = new IndexedVector3(); tmp[m_coneIndices[0]] = 0.0f; tmp[m_coneIndices[1]] = -halfHeight; tmp[m_coneIndices[2]] = 0.0f; return tmp; } } }
///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep) ///result is conservative public void CalculateTemporalAabb(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep, out IndexedVector3 temporalAabbMin,out IndexedVector3 temporalAabbMax) { //start with static aabb GetAabb(ref curTrans,out temporalAabbMin,out temporalAabbMax); float temporalAabbMaxx = temporalAabbMax.X; float temporalAabbMaxy = temporalAabbMax.Y; float temporalAabbMaxz = temporalAabbMax.Z; float temporalAabbMinx = temporalAabbMin.X; float temporalAabbMiny = temporalAabbMin.Y; float temporalAabbMinz = temporalAabbMin.Z; // add linear motion IndexedVector3 linMotion = linvel*timeStep; ///@todo: simd would have a vector max/min operation, instead of per-element access if (linMotion.X > 0f) temporalAabbMaxx += linMotion.X; else temporalAabbMinx += linMotion.X; if (linMotion.Y > 0f) temporalAabbMaxy += linMotion.Y; else temporalAabbMiny += linMotion.Y; if (linMotion.Z > 0f) temporalAabbMaxz += linMotion.Z; else temporalAabbMinz += linMotion.Z; //add conservative angular motion float angularMotion = angvel.Length() * GetAngularMotionDisc() * timeStep; IndexedVector3 angularMotion3d = new IndexedVector3(angularMotion); temporalAabbMin = new IndexedVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz); temporalAabbMax = new IndexedVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz); temporalAabbMin -= angularMotion3d; temporalAabbMax += angularMotion3d; }
public bool GetSphereDistance(CollisionObject boxObj, ref IndexedVector3 pointOnBox, ref IndexedVector3 normal, ref float penetrationDepth, IndexedVector3 sphereCenter, float fRadius, float maxContactDistance) { BoxShape boxShape = boxObj.GetCollisionShape() as BoxShape; IndexedVector3 boxHalfExtent = boxShape.GetHalfExtentsWithoutMargin(); float boxMargin = boxShape.GetMargin(); penetrationDepth = 1.0f; // convert the sphere position to the box's local space IndexedMatrix m44T = boxObj.GetWorldTransform(); IndexedVector3 sphereRelPos = m44T.InvXform(sphereCenter); // Determine the closest point to the sphere center in the box IndexedVector3 closestPoint = sphereRelPos; closestPoint.X = (Math.Min(boxHalfExtent.X, closestPoint.X)); closestPoint.X = (Math.Max(-boxHalfExtent.X, closestPoint.X)); closestPoint.Y = (Math.Min(boxHalfExtent.Y, closestPoint.Y)); closestPoint.Y = (Math.Max(-boxHalfExtent.Y, closestPoint.Y)); closestPoint.Z = (Math.Min(boxHalfExtent.Z, closestPoint.Z)); closestPoint.Z = (Math.Max(-boxHalfExtent.Z, closestPoint.Z)); float intersectionDist = fRadius + boxMargin; float contactDist = intersectionDist + maxContactDistance; normal = sphereRelPos - closestPoint; //if there is no penetration, we are done float dist2 = normal.LengthSquared(); if (dist2 > contactDist * contactDist) { return false; } float distance; //special case if the sphere center is inside the box if (dist2 == 0.0f) { distance = -GetSpherePenetration(ref boxHalfExtent, ref sphereRelPos, ref closestPoint, ref normal); } else //compute the penetration details { distance = normal.Length(); normal /= distance; } pointOnBox = closestPoint + normal * boxMargin; // v3PointOnSphere = sphereRelPos - (normal * fRadius); penetrationDepth = distance - intersectionDist; // transform back in world space IndexedVector3 tmp = m44T * pointOnBox; pointOnBox = tmp; // tmp = m44T(v3PointOnSphere); // v3PointOnSphere = tmp; tmp = m44T._basis * normal; normal = tmp; return true; }
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 IntegrateVelocities(float step) { if (IsStaticOrKinematicObject()) return; #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugRigidBody) { BulletGlobals.g_streamWriter.WriteLine(String.Format("[{0}] RigidBody integrateVelocities", (String)m_userObjectPointer)); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "integrate LinVel pre", m_linearVelocity); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "integrate AngVel pre", m_angularVelocity); } #endif m_linearVelocity += m_totalForce * (m_inverseMass * step); MathUtil.SanityCheckVector(ref m_linearVelocity); m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step; MathUtil.SanityCheckVector(ref m_angularVelocity); /// clamp angular velocity. collision calculations will fail on higher angular velocities float angvel = m_angularVelocity.Length(); if (angvel*step > MAX_ANGVEL) { m_angularVelocity *= (MAX_ANGVEL/step) /angvel; } MathUtil.SanityCheckVector(ref m_angularVelocity); #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugRigidBody) { MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "integrate LinVel post", m_linearVelocity); MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "integrate AngVel post", m_angularVelocity); } #endif }
public void GetSupport(ref IndexedVector3 d,ref sSV sv) { sv.d = d/d.Length(); sv.w = m_shape.Support(ref sv.d); }
public GJKStatus Evaluate(GjkEpaSolver2MinkowskiDiff shapearg, ref IndexedVector3 guess) { uint iterations=0; float sqdist=0f; float alpha=0f; uint clastw=0; /* Initialize solver */ m_free[0] = m_store[0]; m_free[1] = m_store[1]; m_free[2] = m_store[2]; m_free[3] = m_store[3]; m_nfree = 4; m_current = 0; m_status = GJKStatus.Valid; m_shape = shapearg; m_distance = 0f; /* Initialize simplex */ m_simplices[0].rank = 0; m_ray = guess; float sqrl= m_ray.LengthSquared(); IndexedVector3 temp = sqrl>0?-m_ray:new IndexedVector3(1,0,0); AppendVertice(m_simplices[0],ref temp); m_simplices[0].p[0] = 1; m_ray = m_simplices[0].c[0].w; sqdist = sqrl; lastw[0]=lastw[1]=lastw[2]=lastw[3] = m_ray; /* Loop */ do { uint next = 1-m_current; sSimplex cs = m_simplices[m_current]; sSimplex ns = m_simplices[next]; /* Check zero */ float rl=m_ray.Length(); if (rl < GjkEpaSolver2.GJK_MIN_DISTANCE) {/* Touching or inside */ m_status=GJKStatus.Inside; break; } /* Append new vertice in -'v' direction */ IndexedVector3 temp2 = -m_ray; AppendVertice(cs,ref temp2); IndexedVector3 w = cs.c[cs.rank-1].w; bool found = false; for(int i=0;i<4;++i) { if ((w - lastw[i]).LengthSquared() < GjkEpaSolver2.GJK_DUPLICATED_EPS) { found=true; break; } } if(found) {/* Return old simplex */ RemoveVertice(m_simplices[m_current]); break; } else {/* Update lastw */ lastw[clastw=(clastw+1)&3]=w; } /* Check for termination */ float omega=IndexedVector3.Dot(ref m_ray,ref w)/rl; alpha=Math.Max(omega,alpha); if (((rl - alpha) - (GjkEpaSolver2.GJK_ACCURARY * rl)) <= 0) {/* Return old simplex */ RemoveVertice(m_simplices[m_current]); break; } /* Reduce simplex */ IndexedVector4 weights = new IndexedVector4(); uint mask=0; switch(cs.rank) { case 2 : { sqdist=GJK.ProjectOrigin(ref cs.c[0].w,ref cs.c[1].w,ref weights,ref mask); break; } case 3: { sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref weights, ref mask); break; } case 4: { sqdist = GJK.ProjectOrigin(ref cs.c[0].w, ref cs.c[1].w, ref cs.c[2].w, ref cs.c[3].w, ref weights, ref mask); break; } } if(sqdist>=0) {/* Valid */ ns.rank = 0; m_ray = IndexedVector3.Zero; m_current = next; for(uint i=0,ni=cs.rank;i<ni;++i) { if((mask&(1<<(int)i)) != 0) { ns.c[ns.rank] = cs.c[i]; float weight = weights[(int)i]; ns.p[ns.rank++] = weight; m_ray += cs.c[i].w * weight; } else { m_free[m_nfree++] = cs.c[i]; } } if(mask==15) { m_status=GJKStatus.Inside; } } else {/* Return old simplex */ RemoveVertice(m_simplices[m_current]); break; } m_status = ((++iterations) < GjkEpaSolver2.GJK_MAX_ITERATIONS) ? m_status : GJKStatus.Failed; } while(m_status==GJKStatus.Valid); m_simplex = m_simplices[m_current]; switch(m_status) { case GJKStatus.Valid: { m_distance=m_ray.Length(); break; } case GJKStatus.Inside: { m_distance=0; break; } } #if DEBUG if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK) { BulletGlobals.g_streamWriter.WriteLine(String.Format("gjk eval dist[{0}]", m_distance)); } #endif return(m_status); }
// // solve unilateral raint (equality, direct method) // public void ResolveUnilateralPairConstraint(RigidBody body0, RigidBody body1, ref IndexedBasisMatrix world2A, ref IndexedBasisMatrix world2B, ref IndexedVector3 invInertiaADiag, float invMassA, ref IndexedVector3 linvelA, ref IndexedVector3 angvelA, ref IndexedVector3 rel_posA1, ref IndexedVector3 invInertiaBDiag, float invMassB, ref IndexedVector3 linvelB, ref IndexedVector3 angvelB, ref IndexedVector3 rel_posA2, float depthA, ref IndexedVector3 normalA, ref IndexedVector3 rel_posB1, ref IndexedVector3 rel_posB2, float depthB, ref IndexedVector3 normalB, out float imp0, out float imp1) { //(void)linvelA; //(void)linvelB; //(void)angvelB; //(void)angvelA; imp0 = 0f; imp1 = 0f; float len = Math.Abs(normalA.Length()) - 1f; if (Math.Abs(len) >= MathUtil.SIMD_EPSILON) return; Debug.Assert(len < MathUtil.SIMD_EPSILON); //this jacobian entry could be re-used for all iterations JacobianEntry jacA = new JacobianEntry(ref world2A,ref world2B,ref rel_posA1,ref rel_posA2,ref normalA,ref invInertiaADiag,invMassA, ref invInertiaBDiag,invMassB); JacobianEntry jacB = new JacobianEntry(ref world2A,ref world2B,ref rel_posB1,ref rel_posB2,ref normalB,ref invInertiaADiag,invMassA, ref invInertiaBDiag,invMassB); // float vel0 = jacA.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); // float vel1 = jacB.getRelativeVelocity(linvelA,angvelA,linvelB,angvelB); float vel0 = IndexedVector3.Dot(normalA,(body0.GetVelocityInLocalPoint(ref rel_posA1)-body1.GetVelocityInLocalPoint(ref rel_posA1))); float vel1 = IndexedVector3.Dot(normalB,(body0.GetVelocityInLocalPoint(ref rel_posB1)-body1.GetVelocityInLocalPoint(ref rel_posB1))); // float penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv float massTerm = 1f / (invMassA + invMassB); // calculate rhs (or error) terms float dv0 = depthA * m_tau * massTerm - vel0 * m_damping; float dv1 = depthB * m_tau * massTerm - vel1 * m_damping; // dC/dv * dv = -C // jacobian * impulse = -error // //impulse = jacobianInverse * -error // inverting 2x2 symmetric system (offdiagonal are equal!) // float nonDiag = jacA.GetNonDiagonal(jacB,invMassA,invMassB); float invDet = 1f / (jacA.GetDiagonal() * jacB.GetDiagonal() - nonDiag * nonDiag ); //imp0 = dv0 * jacA.getDiagonal() * invDet + dv1 * -nonDiag * invDet; //imp1 = dv1 * jacB.getDiagonal() * invDet + dv0 * - nonDiag * invDet; imp0 = dv0 * jacA.GetDiagonal() * invDet + dv1 * -nonDiag * invDet; imp1 = dv1 * jacB.GetDiagonal() * invDet + dv0 * - nonDiag * invDet; //[a b] [d -c] //[c d] inverse = (1 / determinant) * [-b a] where determinant is (ad - bc) //[jA nD] * [imp0] = [dv0] //[nD jB] [imp1] [dv1] }
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; } } }