//Think this is supposed doing the move and slide Riley reffered to. protected void UpdateTargetPositionBasedOnCollision(ref Vector3 hitNormal, float tangentMag, float normalMag) { Vector3 movementDirection = targetPosition - currentPosition; float movementLength = movementDirection.Length; if (movementLength > MathUtil.SIMD_EPSILON) { movementDirection.Normalize(); Vector3 reflectDir = ComputeReflectionDirection(ref movementDirection, ref hitNormal); reflectDir.Normalize(); Vector3 perpindicularDir; perpindicularDir = PerpindicularComponent(ref reflectDir, ref hitNormal); targetPosition = currentPosition; if (normalMag != 0.0f) { Vector3 perpComponent = perpindicularDir * (normalMag * movementLength); targetPosition += perpComponent; } } }
public override void SetGravityDirection(Vector3 direction) { var d = Vector3.Normalize(direction); _gravityDirection = d; var gravity = _gravityDirection * _gravityStrength * 10.0f; _world.SetGravity(ref gravity); }
public static void Update(float timeElapsed) { colWorld.StepSimulation(timeElapsed); foreach (var ball in balls) { BulletSharp.Math.Vector3 linearVelocity = ball.Ball.LinearVelocity; linearVelocity.Normalize(); ball.Ball.LinearVelocity = linearVelocity * 180; } }
public void Jump(Vector3 dir) { if (!OnGround) { return; } _isJumping = true; dir.Y = 0; _jumpDir = dir; if (dir.LengthSquared < 0.001f) { _jumpDir = new Vector3(0, 0, 1); } _jumpDir += Vector3.UnitY; _jumpDir.Normalize(); }
void UpdateTargetPositionBasedOnCollision(ref Vector3 hitNormal) { Vector3 movement = targetPosition - currentPosition; Vector3 movementDirection = movement.normalized(); float movementLength = movement.Length; if (!Mathf.Approximately(movementLength, 0)) { Vector3 reflectDir = ComputeReflectionDirection(ref movementDirection, ref hitNormal); reflectDir.Normalize(); Vector3 perpindicularDir = PerpindicularComponent(ref reflectDir, ref hitNormal); targetPosition = currentPosition; Vector3 perpComponent = perpindicularDir * movementLength; targetPosition += perpComponent; } }
public void StepUp(float dt) { Matrix transform = RigidBody.MotionState.WorldTransform; if (!_stepping) { _stepping = true; _steppingTo.Y += CapsuleHalfHeight + CapsuleRadius; } /* Bullet reacts with velocity also with kinematic bodies, althogh this * should not change anything, until we do not go back to dynamic body. */ Vector3 origin = transform.Origin; float hor = Vector3.Dot(origin - _steppingTo, _steppingInvNormal); Vector3 stepDir = _steppingTo - origin; stepDir.Y = 0; stepDir.Normalize(); float speed = Vector3.Dot(stepDir, Vector3.TransformNormal(_targetSpeed, transform.Basis)) * SteppingSpeed; origin.Y += speed * dt * 2; float dv = _steppingTo.Y - origin.Y; if (dv <= -1 * MaxStepHeight || _targetSpeed.LengthSquared < 0.0001f || speed <= 0) { CancelStep(); } else if (dv < CapsuleRadius) { float dh = (float)System.Math.Sqrt(dv * (2 * CapsuleRadius - dv)); if (dh < System.Math.Abs(hor)) { float advance = System.Math.Min(dh * System.Math.Sign(hor) - hor, speed * dt); var addVec = _steppingInvNormal * advance; transform.Origin = transform.Origin + addVec; } } RigidBody.MotionState.WorldTransform = transform; }
protected void SetupSim() { // randomly choose scaling before resetting rigid body float randomMass = 0; BulletSharp.Math.Vector3 randomInertia; if (randomScale) { if (varyScale) { m_pclScale.x = Random.Range(this.scaleMin, this.scaleMax); m_pclScale.y = Random.Range(this.scaleMin, this.scaleMax); m_pclScale.z = Random.Range(this.scaleMin, this.scaleMax); } else { float uniformScale = Random.Range(this.scaleMin, this.scaleMax); m_pclScale.x = uniformScale; m_pclScale.y = uniformScale; m_pclScale.z = uniformScale; } //// z can't be more than thrice or less than half of x scale //float zmin = Mathf.Max(this.scaleMin, 0.5f * m_pclScale.x); //float zmax = Mathf.Min(this.scaleMax, 3.0f * m_pclScale.x); //randomScale = Random.Range(zmin, zmax); //m_pclScale.z = randomScale; //// y can't be greater than 2 times the smallest of x and z //float ymax = 2.0f * Mathf.Min(m_pclScale.x, m_pclScale.z); //randomScale = Random.Range(this.scaleMin, Mathf.Min(ymax, this.scaleMax)); //m_pclScale.y = randomScale; if (DEBUG) { Debug.Log("Scaling by " + m_pclScale.ToString()); } // randomMass = m_masses[m_curObjIdx] * m_pclScale.x * m_pclScale.y * m_pclScale.z; float randomDensity; if (useConstantDensity) { // density is constant so mass must scale with volume randomDensity = densityMin; randomMass = randomDensity * m_masses[m_curObjIdx] * m_pclScale.x * m_pclScale.y * m_pclScale.z; } else { randomDensity = Random.Range(densityMin, densityMax); randomMass = randomDensity * m_masses[m_curObjIdx]; } // inertia must scale with volume no matter if the density is constant or not BulletSharp.Math.Vector3 objInertiaInfo = m_inertias[m_curObjIdx]; float scalexyz = m_pclScale.x * m_pclScale.y * m_pclScale.z; float scalex2 = m_pclScale.x * m_pclScale.x; float scaley2 = m_pclScale.y * m_pclScale.y; float scalez2 = m_pclScale.z * m_pclScale.z; float inertiax = randomDensity * scalexyz * (scaley2 * objInertiaInfo[1] + scalez2 * objInertiaInfo[2]); float inertiay = randomDensity * scalexyz * (scalex2 * objInertiaInfo[0] + scalez2 * objInertiaInfo[2]); float inertiaz = randomDensity * scalexyz * (scalex2 * objInertiaInfo[0] + scaley2 * objInertiaInfo[1]); randomInertia = new BulletSharp.Math.Vector3(inertiax, inertiay, inertiaz); // need to completely destory rigid body because need new mass/moment of inertia DestroySimObj(); DataGenUtils.BulletOBJMesh scaledMesh = m_btmesh.Scale(m_pclScale.x, m_pclScale.y, m_pclScale.z); var triVtxarray = new TriangleIndexVertexArray(scaledMesh.indices, scaledMesh.vertices); m_cs = new GImpactMeshShape(triVtxarray); m_cs.LocalScaling = new BulletSharp.Math.Vector3(1); m_cs.Margin = bodyMargin; m_cs.UpdateBound(); AddCollisionShape(m_cs); // move it up so resting on the ground plane float miny = float.MaxValue; float maxz = float.MinValue; float cury; float curz; for (int i = 0; i < scaledMesh.vertices.Length / 3; i++) { cury = scaledMesh.vertices[i * 3 + 1]; if (cury < miny) { miny = cury; } curz = scaledMesh.vertices[i * 3 + 2]; if (curz > maxz) { maxz = curz; } } miny = -miny; m_rbInitTransVec = new BulletSharp.Math.Vector3(0, miny + bodyMargin + m_groundMargin, 0); m_rbInitTrans = Matrix.Translation(m_rbInitTransVec); //* Matrix.RotationY(Random.Range(0.0f, 360.0f)); //float gtInertiaX = (1.0f / 12.0f) * randomMass * (3.0f * maxz * maxz + (2.0f * miny) * (2.0f * miny)); //float gtInertiaZ = gtInertiaX; //float gtInertiaY = 0.5f * randomMass * maxz * maxz; //BulletSharp.Math.Vector3 gtInertia = new BulletSharp.Math.Vector3(gtInertiaX, gtInertiaY, gtInertiaZ); //Debug.Log("GT INERTIA: " + gtInertia.ToString()); //randomInertia = gtInertia; m_rb = CreateRigidBody(randomMass, randomInertia, m_rbInitTrans, m_cs, bodyMat, bodyFriction, viz: RENDER_MODE); //m_rb = CreateRigidBody(randomMass, m_rbInitTrans, m_cs, bodyMat, bodyFriction); m_rb.AngularFactor = angularFactor; m_rb.SetSleepingThresholds(linearSleepThresh, angularSleepThresh); m_mass = randomMass; m_inertia = randomInertia; m_density = randomDensity; } else { // using the same mesh just need to choose a new density // steps for determinism // https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=3143 float randomDensity; if (useConstantDensity) { randomDensity = densityMin; } else { randomDensity = Random.Range(densityMin, densityMax); } randomMass = randomDensity * m_masses[m_curObjIdx]; BulletSharp.Math.Vector3 objInertiaInfo = m_inertias[m_curObjIdx]; float inertiax = randomDensity * (objInertiaInfo[1] + objInertiaInfo[2]); float inertiay = randomDensity * (objInertiaInfo[0] + objInertiaInfo[2]); float inertiaz = randomDensity * (objInertiaInfo[0] + objInertiaInfo[1]); randomInertia = new BulletSharp.Math.Vector3(inertiax, inertiay, inertiaz); m_rb.SetMassProps(randomMass, randomInertia); m_rbInitTrans = Matrix.Translation(m_rbInitTransVec); // * Matrix.RotationY(Random.Range(0.0f, 360.0f)); m_rb = ResetRigidBody(m_rb, randomMass, randomInertia, m_rbInitTrans, m_cs, bodyFriction); m_rb.AngularFactor = angularFactor; m_rb.SetSleepingThresholds(linearSleepThresh, angularSleepThresh); // HingeConstraint hingeConstraint = new HingeConstraint(m_rb, new BulletSharp.Math.Vector3(0.0f), new BulletSharp.Math.Vector3(0.0f, 1.0f, 0.0f), false); // m_world.AddConstraint(hingeConstraint); // DestroySimObj(); // have to do this to set mass properties but can reuse previously calculated everything else // if (m_cs == null) Debug.Log("NOT NULL"); // m_rb = CreateRigidBody(randomMass, randomInertia, m_rbInitTrans, m_cs, bodyMat, bodyFriction); // m_rb.AngularFactor = new BulletSharp.Math.Vector3(angularFactor); // m_rb.SetSleepingThresholds(linearSleepThresh, angularSleepThresh); m_mass = randomMass; m_inertia = randomInertia; m_density = randomDensity; } m_stepCount = 0; m_broadphase.ResetPool(m_colDispatcher); m_solver.Reset(); float curMass = 1.0f / m_rb.InvMass; if (DEBUG) { Debug.Log("Mass: " + curMass.ToString()); } if (DEBUG) { Debug.Log("LOCAL MOMENT: " + m_rb.LocalInertia.ToString()); } if (DEBUG) { Debug.Log("COM " + m_rb.CenterOfMassPosition.ToString()); } if (DEBUG) { Debug.Log("Density " + m_density.ToString()); } // determine impulse position ClosestRayResultCallback cb; BulletSharp.Math.Vector3 vertexNormal = new BulletSharp.Math.Vector3(); int missCount = 0; do { // choose random vertex to apply force to // pick a random point around in the plane around y position float offsetx = UnityEngine.Random.Range(-100.0f, 100.0f); float offsetz = UnityEngine.Random.Range(-100.0f, 100.0f); Vector2 offsetvec = new Vector2(offsetx, offsetz); // offsetvec.Normalize(); //float relForceHeight = 0.75f; UnityEngine.Vector3 offsetPt = new UnityEngine.Vector3(offsetvec[0], m_rb.CenterOfMassPosition.Y, offsetvec[1]); BulletSharp.Math.Vector3 btOffsetPt = BSExtensionMethods2.ToBullet(offsetPt); BulletSharp.Math.Vector3 btInnerPt = m_rb.CenterOfMassPosition; cb = new ClosestRayResultCallback(ref btOffsetPt, ref btInnerPt); // Debug.DrawLine(BSExtensionMethods2.ToUnity(btInnerPt), offsetPt, Color.red, 2.0f); m_world.RayTest(btOffsetPt, btInnerPt, cb); if (cb.HasHit) { m_forcePoint = cb.HitPointWorld; vertexNormal = cb.HitNormalWorld; } else { missCount++; //Debug.Log("ERROR - couldn't find point to apply force to. Retrying..."); //return; } } while (!cb.HasHit); if (DEBUG) { Debug.Log("Missed impulse " + missCount.ToString() + " times."); } if (DEBUG) { Debug.LogFormat("ForcePoint: " + m_forcePoint.ToString()); } // get force vector // loop until force is applied to outside of object UnityEngine.Vector3 uForceVec = new UnityEngine.Vector3(); // initialize force vector to coincide with center of mass BulletSharp.Math.Vector3 btForceVec = m_rb.CenterOfMassPosition - m_forcePoint; // then randomly vary it within the x/z plane to be within the specified distance BulletSharp.Math.Vector3 btVariationVec = new BulletSharp.Math.Vector3(-btForceVec[2], 0.0f, btForceVec[0]); btVariationVec.Normalize(); float varyForce; BulletSharp.Math.Vector3 proposedForceVec; do { varyForce = UnityEngine.Random.Range(-forceDistMax, forceDistMax); proposedForceVec = btVariationVec * varyForce + btForceVec; } while (proposedForceVec.Dot(vertexNormal) >= 0); // must also be on the outside of the object btForceVec = proposedForceVec; btForceVec.Normalize(); uForceVec = BSExtensionMethods2.ToUnity(btForceVec); if (DEBUG) { Debug.Log("FORCE DIST: " + varyForce.ToString()); } //UnityEngine.Vector3 uVtxNormal = BSExtensionMethods2.ToUnity(vertexNormal); //uVtxNormal.Normalize(); //do //{ // float forcex = UnityEngine.Random.Range(-1.0f, 1.0f); // float forcez = UnityEngine.Random.Range(-1.0f, 1.0f); // uForceVec.Set(forcex, 0.0f, forcez); // uForceVec.Normalize(); //} while (UnityEngine.Vector3.Dot(uForceVec, uVtxNormal) >= 0); // random constrained magnitude float mag = UnityEngine.Random.Range(impulseMin, impulseMax); //Debug.Log("Vol: " + objectVolume.ToString()); // if (varyScale) { // mag *= randomMass; // scale impulse t unity //according to object scale // } else { // mag *= curMass; // } mag *= m_mass; // scale impulse according to object mass uForceVec *= mag; // set directly for debugging //uForceVec.Set(2.5f, 0.0f, 0.0f); //m_forcePoint = new BulletSharp.Math.Vector3(0.0f, m_rb.CenterOfMassPosition.Y, -0.15f); m_forceVec = BSExtensionMethods2.ToBullet(uForceVec); if (DEBUG) { Debug.LogFormat("ForceVec: " + m_forceVec.ToString()); } if (DEBUG) { UnityEngine.Vector3 debugVec = -uForceVec; debugVec.Scale(new UnityEngine.Vector3(0.5f, 0.5f, 0.5f)); Debug.DrawRay(BSExtensionMethods2.ToUnity(m_forcePoint), debugVec, Color.green, 1.0f); Debug.DrawLine(BSExtensionMethods2.ToUnity(m_rb.CenterOfMassPosition), BSExtensionMethods2.ToUnity(m_forcePoint), Color.cyan, 1.0f); Debug.DrawLine(BSExtensionMethods2.ToUnity(m_rb.CenterOfMassPosition), BSExtensionMethods2.ToUnity(m_rb.CenterOfMassPosition) + BSExtensionMethods2.ToUnity(btVariationVec) * varyForce, Color.blue, 1.0f); } // apply the random impulse BulletSharp.Math.Vector3 radius = m_forcePoint - m_rb.CenterOfMassPosition; m_rb.ApplyImpulse(m_forceVec, radius); // m_rb.ApplyTorqueImpulse(new BulletSharp.Math.Vector3(0.0f, 1.0f, 0.0f)); // m_rb.ApplyCentralImpulse(new BulletSharp.Math.Vector3(4.0f, 0.0f, 2.0f)); // BulletSharp.Math.Vector3 newAngVel = m_rb.AngularVelocity; // newAngVel.X = 0.0f; // newAngVel.Z = 0.0f; // m_rb.AngularVelocity = newAngVel; // calculate ground truth for debugging //BulletSharp.Math.Vector3 gtAngVel = radius.Cross(m_forceVec) / m_inertia; //BulletSharp.Math.Vector3 gtLinVel = m_forceVec / m_mass; //Debug.Log("GT LIN VEL: " + gtLinVel.ToString()); //Debug.Log("GT ANG VEL: " + gtAngVel.ToString()); }
//todo(erwincoumans) Quick hack, reference to InvertedPendulumPDControl implementation. Will create a separate header/source file for this. public static MultiBody createInvertedPendulumMultiBody(float radius, MultiBodyDynamicsWorld world, Matrix baseWorldTrans, bool fixedBase) { BulletSharp.Math.Vector4[] colors = new BulletSharp.Math.Vector4[] { new BulletSharp.Math.Vector4(1, 0, 0, 1), new BulletSharp.Math.Vector4(0, 1, 0, 1), new BulletSharp.Math.Vector4(0, 1, 1, 1), new BulletSharp.Math.Vector4(1, 1, 0, 1), }; int curColor = 0; bool damping = false; bool gyro = false; int numLinks = 2; bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool canSleep = false; bool selfCollide = false; BulletSharp.Math.Vector3 linkHalfExtents = new BulletSharp.Math.Vector3(0.05f, 0.37f, 0.1f); BulletSharp.Math.Vector3 baseHalfExtents = new BulletSharp.Math.Vector3(0.04f, 0.35f, 0.08f); //mbC.forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm //init the base BulletSharp.Math.Vector3 baseInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f); float baseMass = fixedBase ? 0.0f : 10.0f; if (baseMass != 0) { //CollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); shape.CalculateLocalInertia(baseMass, out baseInertiaDiag); shape.Dispose(); } MultiBody pMultiBody = new MultiBody(numLinks, 0, baseInertiaDiag, fixedBase, canSleep); pMultiBody.BaseWorldTransform = baseWorldTrans; BulletSharp.Math.Vector3 vel = new BulletSharp.Math.Vector3(0, 0, 0); // pMultiBody.setBaseVel(vel); //init the links BulletSharp.Math.Vector3 hingeJointAxis = new BulletSharp.Math.Vector3(1, 0, 0); //y-axis assumed up BulletSharp.Math.Vector3 parentComToCurrentCom = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1] * 2.0f, 0); //par body's COM to cur body's COM offset BulletSharp.Math.Vector3 currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset BulletSharp.Math.Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset ////// float q0 = 1.0f * Mathf.PI / 180.0f; BulletSharp.Math.Quaternion quat0 = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(1, 0, 0), q0); quat0.Normalize(); ///// for (int i = 0; i < numLinks; ++i) { float linkMass = 1.0f; //if (i==3 || i==2) // linkMass= 1000; BulletSharp.Math.Vector3 linkInertiaDiag = new BulletSharp.Math.Vector3(0.0f, 0.0f, 0.0f); CollisionShape shape = null; if (i == 0) { shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));// } else { shape = new SphereShape(radius); } shape.CalculateLocalInertia(linkMass, out linkInertiaDiag); shape.Dispose(); if (!spherical) { //pMultiBody.setupRevolute(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); if (i == 0) { pMultiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); } else { parentComToCurrentCom = new BulletSharp.Math.Vector3(0, -radius * 2.0f, 0); //par body's COM to cur body's COM offset currentPivotToCurrentCom = new BulletSharp.Math.Vector3(0, -radius, 0); //cur body's COM to cur body's PIV offset parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset pMultiBody.SetupFixed(i, linkMass, linkInertiaDiag, i - 1, new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f), parentComToCurrentPivot, currentPivotToCurrentCom); } } else { //pMultiBody.setupPlanar(i, linkMass, linkInertiaDiag, i - 1, BulletSharp.Math.Quaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, BulletSharp.Math.Vector3(1, 0, 0), parentComToCurrentPivot*2, false); pMultiBody.SetupSpherical(i, linkMass, linkInertiaDiag, i - 1, new BulletSharp.Math.Quaternion(0.0f, 0.0f, 0.0f, 1.0f), parentComToCurrentPivot, currentPivotToCurrentCom, false); } } pMultiBody.FinalizeMultiDof(); world.AddMultiBody(pMultiBody); MultiBody mbC = pMultiBody; mbC.CanSleep = (canSleep); mbC.HasSelfCollision = (selfCollide); mbC.UseGyroTerm = (gyro); // if (!damping) { mbC.LinearDamping = (0.0f); mbC.AngularDamping = (0.0f); } else { mbC.LinearDamping = (0.1f); mbC.AngularDamping = (0.9f); } if (numLinks > 0) { q0 = 180.0f * Mathf.PI / 180.0f; if (!spherical) { mbC.SetJointPosMultiDof(0, new float[] { q0 }); } else { BulletSharp.Math.Vector3 vv = new BulletSharp.Math.Vector3(1, 1, 0); vv.Normalize(); quat0 = new BulletSharp.Math.Quaternion(vv, q0); quat0.Normalize(); float[] quat0fs = new float[] { quat0.X, quat0.Y, quat0.Z, quat0.W }; mbC.SetJointPosMultiDof(0, quat0fs); } } /// BulletSharp.Math.Quaternion[] world_to_local; //btAlignedObjectArray<BulletSharp.Math.Quaternion> world_to_local = new BulletSharp.Math.Quaternion[pMultiBody.NumLinks + 1]; BulletSharp.Math.Vector3[] local_origin; //btAlignedObjectArray<BulletSharp.Math.Vector3> local_origin = new BulletSharp.Math.Vector3[pMultiBody.NumLinks + 1]; world_to_local[0] = pMultiBody.WorldToBaseRot; local_origin[0] = pMultiBody.BasePosition; // double friction = 1; { if (true) { CollisionShape shape = new BoxShape(new BulletSharp.Math.Vector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); //new btSphereShape(baseHalfExtents[0]); // guiHelper.createCollisionShapeGraphicsObject(shape); MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, -1); col.CollisionShape = shape; Matrix tr = new Matrix(); tr.ScaleVector = BulletSharp.Math.Vector3.One; //if we don't set the initial pose of the btCollisionObject, the simulator will do this //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider tr.Origin = local_origin[0]; BulletSharp.Math.Quaternion orn = new BulletSharp.Math.Quaternion(new BulletSharp.Math.Vector3(0, 0, 1), 0.25f * 3.1415926538f); tr.Rotation = (orn); col.WorldTransform = (tr); bool isDynamic = (baseMass > 0 && !fixedBase); CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter; world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//, 2,1+2); BulletSharp.Math.Vector4 color = new BulletSharp.Math.Vector4(0.0f, 0.0f, 0.5f, 1f); //guiHelper.createCollisionObjectGraphicsObject(col, color); // col.setFriction(friction); pMultiBody.BaseCollider = (col); } } for (int i = 0; i < pMultiBody.NumLinks; ++i) { int parent = pMultiBody.GetParent(i); world_to_local[i + 1] = pMultiBody.GetParentToLocalRot(i) * world_to_local[parent + 1]; BulletSharp.Math.Vector3 vv = world_to_local[i + 1].Inverse.Rotate(pMultiBody.GetRVector(i)); local_origin[i + 1] = local_origin[parent + 1] + vv; } for (int i = 0; i < pMultiBody.NumLinks; ++i) { BulletSharp.Math.Vector3 posr = local_origin[i + 1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; float[] quat = new float[] { -world_to_local[i + 1].X, -world_to_local[i + 1].Y, -world_to_local[i + 1].Z, world_to_local[i + 1].W }; CollisionShape shape = null; if (i == 0) { shape = new BoxShape(new BulletSharp.Math.Vector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]); } else { shape = new SphereShape(radius); } //guiHelper.createCollisionShapeGraphicsObject(shape); MultiBodyLinkCollider col = new MultiBodyLinkCollider(pMultiBody, i); col.CollisionShape = (shape); Matrix tr = new Matrix(); tr.ScaleVector = new BulletSharp.Math.Vector3(); tr.Origin = (posr); tr.Rotation = (new BulletSharp.Math.Quaternion(quat[0], quat[1], quat[2], quat[3])); col.WorldTransform = (tr); // col.setFriction(friction); bool isDynamic = true;//(linkMass > 0); CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter; //if (i==0||i>numLinks-2) { world.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask);//,2,1+2); BulletSharp.Math.Vector4 color = colors[curColor]; curColor++; curColor &= 3; //guiHelper.createCollisionObjectGraphicsObject(col, color); pMultiBody.GetLink(i).Collider = col; } } return(pMultiBody); }
protected void StepForwardAndStrafe(CollisionWorld collisionWorld, ref Vector3 walkMove) { Matrix start = Matrix.Identity, end = Matrix.Identity; targetPosition = currentPosition + walkMove; float fraction = 1.0f; float distance2 = (currentPosition - targetPosition).LengthSquared; // printf("distance2=%f\n",distance2); if (touchingContact) { float dot; Vector3.Dot(ref normalizedDirection, ref m_touchingNormal, out dot); if (dot > 0.0f) { //interferes with step movement //UpdateTargetPositionBasedOnCollision(ref m_touchingNormal, 0.0f, 1.0f); } } int maxIter = 10; while (fraction > 0.01f && maxIter-- > 0) { start.Origin = (currentPosition); end.Origin = (targetPosition); Vector3 sweepDirNegative = currentPosition - targetPosition; KinematicClosestNotMeConvexResultCallback callback = new KinematicClosestNotMeConvexResultCallback(ghostObject, sweepDirNegative, 0f); callback.CollisionFilterGroup = ghostObject.BroadphaseHandle.CollisionFilterGroup; callback.CollisionFilterMask = ghostObject.BroadphaseHandle.CollisionFilterMask; float margin = convexShape.Margin; convexShape.Margin = margin + .02f; ghostObject.ConvexSweepTestRef(convexShape, ref start, ref end, callback, collisionWorld.DispatchInfo.AllowedCcdPenetration); convexShape.Margin = margin; fraction -= callback.ClosestHitFraction; if (callback.HasHit) { // We moved only a fraction float hitDistance = (callback.HitPointWorld - currentPosition).Length; Vector3 hitNormalWorld = callback.HitNormalWorld; UpdateTargetPositionBasedOnCollision(ref hitNormalWorld, 0f, 1f); Vector3 currentDir = targetPosition - currentPosition; distance2 = currentDir.LengthSquared; if (distance2 > MathUtil.SIMD_EPSILON) { currentDir.Normalize(); /* See Quake2: "If velocity is against original velocity, stop ead to avoid tiny oscilations in sloping corners." */ float dot; Vector3.Dot(ref currentDir, ref normalizedDirection, out dot); if (dot <= 0.0f) { break; } } else { break; } } else { // we moved whole way currentPosition = targetPosition; } } }