protected override void Dispose(bool isdisposing) { if (isInWorld && isdisposing && m_linkCollider != null) { BPhysicsWorld pw = BPhysicsWorld.Get(); if (pw != null && pw.world != null) { //constraints must be removed before rigid body is removed /* * for (int i = m_linkCollider.NumConstraintRefs; i > 0; i--) * { * BTypedConstraint tc = (BTypedConstraint)m_rigidBody.GetConstraintRef(i - 1).Userobject; * ((DiscreteDynamicsWorld)pw.world).RemoveConstraint(tc.GetConstraint()); * } */ ((DiscreteDynamicsWorld)pw.world).RemoveCollisionObject(m_linkCollider); } } if (m_linkCollider != null) { m_linkCollider.Dispose(); m_linkCollider = null; } }
protected override void Dispose(bool isdisposing) { if (isInWorld && isdisposing && m_multibody != null) { BPhysicsWorld pw = BPhysicsWorld.Get(); if (pw != null && pw.world != null) { if (m_baseCollider != null) { pw.world.RemoveCollisionObject(m_baseCollider); } if (m_multibody != null) { ((MultiBodyDynamicsWorld)pw.world).RemoveMultiBody(m_multibody); } } } if (m_multibody != null) { m_multibody.Dispose(); m_multibody = null; } if (m_baseCollider != null) { m_baseCollider.Dispose(); m_baseCollider = null; } if (m_links != null) { m_links.Clear(); m_links = null; } }
/** * Creates or configures a RigidBody based on the current settings. Does not alter the internal state of this component in any way. * Can be used to create copies of this BRigidBody for use in other physics simulations. */ public virtual bool CreateMultiBody(ref MultiBody mb, ref BulletSharp.Math.Vector3 localInertia, CollisionShape cs) { localInertia = BulletSharp.Math.Vector3.Zero; if (_mass > 0) { cs.CalculateLocalInertia(_mass, out localInertia); } if (mb == null) { int nbLinks = Links.Count; foreach (BMultiBodyLink link in Links) { nbLinks += link.NbLinks; } Debug.Log("Adding multibody with " + nbLinks + " links"); mb = new MultiBody(nbLinks, _mass, localInertia, false, false); mb.BaseWorldTransform = transform.localToWorldMatrix.ToBullet(); mb.HasSelfCollision = SelfCollision; var collider = new MultiBodyLinkCollider(mb, -1); collider.CollisionShape = cs; collider.WorldTransform = transform.localToWorldMatrix.ToBullet(); collider.CollisionFlags = collisionFlags; collider.UserObject = UserObject ?? this; BPhysicsWorld.Get().world.AddCollisionObject(collider, groupsIBelongTo, collisionMask); mb.BaseCollider = collider; m_collisionObject = collider; BulletMultiBodyLinkColliderProxy baseProxy = gameObject.GetComponent <BulletMultiBodyLinkColliderProxy>(); if (baseProxy == null) { baseProxy = gameObject.AddComponent <BulletMultiBodyLinkColliderProxy>(); } baseProxy.target = collider; try { int currentLinkIndex = 0; for (int i = 0; i < Links.Count; ++i) { currentLinkIndex += Links[i].AddLinkToMultiBody(this, currentLinkIndex, -1, transform); } mb.FinalizeMultiDof(); } catch (Exception e) // if an error occurs, don't add the object, otherwise unity will crash { Debug.LogErrorFormat("Error occured while setting MultiBody : {0}\n{1}", e.Message, e.StackTrace); BPhysicsWorld.Get().world.RemoveCollisionObject(collider); return(false); } mb.CanSleep = false; mb.UserObject = UserObject ?? this; } return(true); }
private void Update() { if (m_baseCollider != null && isInWorld) { Matrix4x4 m = m_baseCollider.WorldTransform.ToUnity(); transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m); transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m); for (int i = 0; i < m_links.Count; i++) { MultiBodyLinkCollider linkCollider = m_links[i].GetLinkCollider(); m = linkCollider.WorldTransform.ToUnity(); m_links[i].transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m); m_links[i].transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m); } } }
void DoEverything() { OnInitializePhysics(); for (int i = 0; i < 100; i++) { OnUpdate(); for (int j = 0; j < numLinks; j++) { MultiBodyLinkCollider linkCollider = links[j]; UnityEngine.Matrix4x4 m = linkCollider.WorldTransform.ToUnity(); UnityEngine.Vector3 p = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m); linkPositions[j] = p; UnityEngine.Debug.Log("pos " + p.ToString("f3") + " " + j); } } ExitPhysics(); }
void AddColliders(MultiBody multiBody, Vector3 baseHalfExtents, Vector3 linkHalfExtents) { // Add a collider for the base Quaternion[] worldToLocal = new Quaternion[multiBody.NumLinks + 1]; Vector3[] localOrigin = new Vector3[multiBody.NumLinks + 1]; worldToLocal[0] = multiBody.WorldToBaseRot; localOrigin[0] = multiBody.BasePosition; //if (true) { var collider = new MultiBodyLinkCollider(multiBody, -1); collider.CollisionShape = new BoxShape(baseHalfExtents); Matrix tr = Matrix.RotationQuaternion(worldToLocal[0].Inverse); tr.Origin = localOrigin[0]; collider.WorldTransform = tr; World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter, CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter); BulletExampleRunner.Get().CreateUnityMultiBodyLinkColliderProxy(collider); collider.Friction = Friction; multiBody.BaseCollider = collider; } for (int i = 0; i < multiBody.NumLinks; i++) { int parent = multiBody.GetParent(i); worldToLocal[i + 1] = multiBody.GetParentToLocalRot(i) * worldToLocal[parent + 1]; localOrigin[i + 1] = localOrigin[parent + 1] + (worldToLocal[i + 1].Inverse.Rotate(multiBody.GetRVector(i))); } for (int i = 0; i < multiBody.NumLinks; i++) { var collider = new MultiBodyLinkCollider(multiBody, i); collider.CollisionShape = new BoxShape(linkHalfExtents); Matrix tr = Matrix.RotationQuaternion(worldToLocal[i + 1].Inverse) * Matrix.Translation(localOrigin[i + 1]); collider.WorldTransform = tr; World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter, CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter); collider.Friction = Friction; BulletExampleRunner.Get().CreateUnityMultiBodyLinkColliderProxy(collider); multiBody.GetLink(i).Collider = collider; } }
private void PickMultiBody(MultiBodyLinkCollider collider, ref Vector3 pickPosition) { MultiBody multiBody = collider.MultiBody; if (multiBody == null) { return; } _prevCanSleep = multiBody.CanSleep; multiBody.CanSleep = false; Vector3 pivotInA = multiBody.WorldPosToLocal(collider.Link, pickPosition); var p2p = new MultiBodyPoint2Point(multiBody, collider.Link, null, pivotInA, pickPosition) { MaxAppliedImpulse = 2 }; var world = _demo.Simulation.World as MultiBodyDynamicsWorld; world.AddMultiBodyConstraint(p2p); _multiBodyPickConstraint = p2p; }
/// <summary> /// A MultiBody object needs to be created and added a controled sequence /// 1) The multibody needs to be added to the PhysicsWorld. /// 2) Then the base colliders and other colliders can be created and added. They /// depend on the multibody reference. /// 3) Then collision objects can be added. /// </summary> internal void CreateColliders() { if (m_multibody == null) { Debug.LogError("Multibody must exist before creating colliders"); return; } m_baseCollider = new MultiBodyLinkCollider(m_multibody, -1); m_baseCollider.UserObject = this; //todo validate that shape exists on base and all BCollisionShape shape = GetComponent <BCollisionShape>(); m_baseCollider.CollisionShape = shape.GetCollisionShape(); Matrix worldTrans = Matrix.RotationQuaternion(transform.rotation.ToBullet()); worldTrans.Origin = transform.position.ToBullet(); m_baseCollider.WorldTransform = worldTrans; bool isDynamic = (baseMass > 0 && !fixedBase); m_groupsIBelongTo = isDynamic ? (m_groupsIBelongTo) : (m_groupsIBelongTo | BulletSharp.CollisionFilterGroups.StaticFilter); m_collisionMask = isDynamic ? (m_collisionMask) : (m_collisionMask | BulletSharp.CollisionFilterGroups.StaticFilter); m_multibody.BaseCollider = m_baseCollider; for (int i = 0; i < m_links.Count; i++) { //create colliders MultiBodyLinkCollider col = new MultiBodyLinkCollider(m_multibody, m_links[i].index); m_links[i].AssignMultiBodyLinkColliderOnCreation(this, col); col.UserObject = m_links[i]; shape = m_links[i].GetComponent <BCollisionShape>(); col.CollisionShape = shape.GetCollisionShape(); worldTrans = Matrix.RotationQuaternion(m_links[i].transform.rotation.ToBullet()); worldTrans.Origin = m_links[i].transform.position.ToBullet(); col.WorldTransform = worldTrans; m_multibody.GetLink(i).Collider = col; } }
IEnumerator myCoroutine() { Debug.Log("Waiting to start"); while (!Input.GetKeyDown(KeyCode.Space)) { yield return(null); } OnInitializePhysics(); Debug.Log("Initialized"); int frame = 0; while (frame < 10) { if (Input.GetKeyDown(KeyCode.Space)) { frame++; Debug.Log("Do Update " + frame); for (int j = 0; j < numLinks; j++) { MultiBodyLinkCollider linkCollider = links[j]; for (int k = 0; k < 5; k++) { OnUpdate(); } UnityEngine.Matrix4x4 m = linkCollider.WorldTransform.ToUnity(); UnityEngine.Vector3 p = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m); linkPositions[j] = p; UnityEngine.Debug.Log("pos " + p.ToString("f3") + " " + j); } } yield return(null); } Debug.Log("Finalize"); ExitPhysics(); }
public void CreateUnityMultiBodyLinkColliderProxy(MultiBodyLinkCollider body) { GameObject cube = Instantiate <GameObject>(cubePrefab); CollisionShape cs = body.CollisionShape; if (cs is BoxShape) { BoxShape bxcs = cs as BoxShape; BulletSharp.Math.Vector3 s = bxcs.HalfExtentsWithMargin; MeshRenderer mr = cube.GetComponentInChildren <MeshRenderer>(); mr.transform.localScale = s.ToUnity() * 2f; Matrix4x4 m = body.WorldTransform.ToUnity(); cube.transform.position = BSExtensionMethods2.ExtractTranslationFromMatrix(ref m); cube.transform.rotation = BSExtensionMethods2.ExtractRotationFromMatrix(ref m); cube.transform.localScale = BSExtensionMethods2.ExtractScaleFromMatrix(ref m); Destroy(cube.GetComponent <BulletRigidBodyProxy>()); BulletMultiBodyLinkColliderProxy cp = cube.AddComponent <BulletMultiBodyLinkColliderProxy>(); cp.target = body; } else { Debug.LogError("Not implemented"); } }
protected override void OnInitializePhysics() { // collision configuration contains default setup for memory, collision setup CollisionConf = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConf); Broadphase = new DbvtBroadphase(); Solver = new MultiBodyConstraintSolver(); World = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, Solver as MultiBodyConstraintSolver, CollisionConf); World.Gravity = new Vector3(0, -9.81f, 0); const bool floating = false; const bool gyro = false; const int numLinks = 1; const bool canSleep = false; const bool selfCollide = false; Vector3 linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f); //Vector3 baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f); Vector3 baseInertiaDiag = Vector3.Zero; const float baseMass = 0; multiBody = new MultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); //multiBody.UseRK4Integration = true; //multiBody.BaseWorldTransform = Matrix.Identity; //init the links Vector3 hingeJointAxis = new Vector3(1, 0, 0); //y-axis assumed up Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0); Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0); Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; for (int i = 0; i < numLinks; i++) { const float linkMass = 10; Vector3 linkInertiaDiag = Vector3.Zero; using (var shape = new SphereShape(radius)) { shape.CalculateLocalInertia(linkMass, out linkInertiaDiag); } multiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, Quaternion.Identity, hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); } multiBody.FinalizeMultiDof(); (World as MultiBodyDynamicsWorld).AddMultiBody(multiBody); multiBody.CanSleep = canSleep; multiBody.HasSelfCollision = selfCollide; multiBody.UseGyroTerm = gyro; #if PENDULUM_DAMPING multiBody.LinearDamping = 0.1f; multiBody.AngularDamping = 0.9f; #else multiBody.LinearDamping = 0; multiBody.AngularDamping = 0; #endif for (int i = 0; i < numLinks; i++) { var shape = new SphereShape(radius); CollisionShapes.Add(shape); var col = new MultiBodyLinkCollider(multiBody, i); col.CollisionShape = shape; //const bool isDynamic = true; CollisionFilterGroups collisionFilterGroup = CollisionFilterGroups.DefaultFilter; // : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = CollisionFilterGroups.AllFilter; // : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter; World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask); multiBody.GetLink(i).Collider = col; } }
public int AddLinkToMultiBody(BMultiBody mb, int currentLinkIndex, int parentIndex, Transform parent) { if (isLinked) { Debug.LogErrorFormat("Cannot add link {0} to multibody {1} bacause it is already linked", name, mb.name); return(0); } BCollisionShape collisionShape = GetComponent <BCollisionShape>(); if (collisionShape == null) { throw new MissingComponentException("Could not find " + typeof(BCollisionShape).Name + " component on BodyLink " + name); } multiBody = mb; linkId = currentLinkIndex; parentLinkId = parentIndex; parentTransform = parent; CollisionShape shape = collisionShape.GetCollisionShape(); if (shape == null) { throw new MissingComponentException("Could not get collision shape from " + collisionShape.GetType().Name + " shape component on BodyLink " + name); } BulletSharp.Math.Vector3 linkInertia; shape.CalculateLocalInertia(Mass, out linkInertia); if (BPhysicsWorld.Get().debugType >= BulletUnity.Debugging.BDebug.DebugType.Debug) { Debug.LogFormat(this, "Adding link {0} : {1} to parent {2} of multibody {3}", currentLinkIndex, name, parentIndex, mb.name); } SetupLink(linkInertia); linkCollider = new MultiBodyLinkCollider(mb.MultiBody, currentLinkIndex); linkCollider.CollisionShape = shape; linkCollider.WorldTransform = transform.localToWorldMatrix.ToBullet(); linkCollider.CollisionFlags = collisionFlags; linkCollider.Friction = Friction; linkCollider.RollingFriction = RollingFriction; linkCollider.Restitution = Restitution; linkCollider.UserObject = this; BPhysicsWorld.Get().world.AddCollisionObject(linkCollider, groupsIBelongTo, collisionMask); m_collisionObject = linkCollider; BulletMultiBodyLinkColliderProxy proxy = gameObject.GetComponent <BulletMultiBodyLinkColliderProxy>(); if (proxy == null) { proxy = gameObject.AddComponent <BulletMultiBodyLinkColliderProxy>(); } mb.MultiBody.GetLink(currentLinkIndex).Collider = linkCollider; proxy.target = linkCollider; isLinked = true; foreach (BMultiBodyConstraint mbc in Constraints) { mbc.AddConstraintToMultiBody(MultiBody, LinkId); } int addedLinks = 1; for (int i = 0; i < Links.Count; ++i) { addedLinks += Links[i].AddLinkToMultiBody(mb, i + currentLinkIndex + 1, currentLinkIndex, transform); } return(addedLinks); }
public virtual void OnHandleInput() { if (Input.KeysPressed.Count != 0) { switch (Input.KeysPressed[0]) { case Keys.Escape: case Keys.Q: Graphics.Form.Close(); return; case Keys.F1: MessageBox.Show( "Move using WASD + shift\n" + "Left click - point camera\n" + "Right click - pick up an object using a Point2PointConstraint\n" + "Right click + shift - pick up an object using a fixed Generic6DofConstraint\n" + "Space - shoot box\n" + "Q - quit\n" + Graphics.InfoText, "Help"); // Key release won't be captured Input.KeysDown.Remove(Keys.F1); break; case Keys.F3: IsDebugDrawEnabled = !IsDebugDrawEnabled; break; case Keys.F8: Input.ClearKeyCache(); GraphicsLibraryManager.ExitWithReload = true; Graphics.Form.Close(); break; case Keys.F11: Graphics.IsFullScreen = !Graphics.IsFullScreen; break; case (Keys.Control | Keys.F): const int maxSerializeBufferSize = 1024 * 1024 * 5; using (var serializer = new DefaultSerializer(maxSerializeBufferSize)) { World.Serialize(serializer); byte[] dataBytes = new byte[serializer.CurrentBufferSize]; System.Runtime.InteropServices.Marshal.Copy(serializer.BufferPointer, dataBytes, 0, dataBytes.Length); using (var file = new System.IO.FileStream("world.bullet", System.IO.FileMode.Create)) { file.Write(dataBytes, 0, dataBytes.Length); } } break; case Keys.G: //shadowsEnabled = !shadowsEnabled; break; case Keys.Space: ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView)); break; case Keys.Return: ClientResetScene(); break; } } if (Input.MousePressed != MouseButtons.None) { Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView); if (Input.MousePressed == MouseButtons.Right) { if (World != null) { Vector3 rayFrom = Freelook.Eye; ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo); World.RayTest(ref rayFrom, ref rayTo, rayCallback); if (rayCallback.HasHit) { Vector3 pickPos = rayCallback.HitPointWorld; RigidBody body = rayCallback.CollisionObject as RigidBody; if (body != null) { if (!(body.IsStaticObject || body.IsKinematicObject)) { pickedBody = body; pickedBody.ActivationState = ActivationState.DisableDeactivation; Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform)); if (Input.KeysDown.Contains(Keys.ShiftKey)) { Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false) { LinearLowerLimit = Vector3.Zero, LinearUpperLimit = Vector3.Zero, AngularLowerLimit = Vector3.Zero, AngularUpperLimit = Vector3.Zero }; World.AddConstraint(dof6); pickConstraint = dof6; dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5); } else { Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot); World.AddConstraint(p2p); pickConstraint = p2p; p2p.Setting.ImpulseClamp = 30; //very weak constraint for picking p2p.Setting.Tau = 0.001f; /* * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 0); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 1); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 2); */ } } } else { MultiBodyLinkCollider multiCol = rayCallback.CollisionObject as MultiBodyLinkCollider; if (multiCol != null && multiCol.MultiBody != null) { MultiBody mb = multiCol.MultiBody; prevCanSleep = mb.CanSleep; mb.CanSleep = false; Vector3 pivotInA = mb.WorldPosToLocal(multiCol.Link, pickPos); MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(mb, multiCol.Link, null, pivotInA, pickPos); p2p.MaxAppliedImpulse = 2; (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p); pickingMultiBodyPoint2Point = p2p; } } oldPickingDist = (pickPos - rayFrom).Length; } rayCallback.Dispose(); } } } else if (Input.MouseReleased == MouseButtons.Right) { RemovePickingConstraint(); } // Mouse movement if (Input.MouseDown == MouseButtons.Right) { MovePickedBody(); } }
MultiBody CreateFeatherstoneMultiBody(MultiBodyDynamicsWorld world, MultiBodySettings settings) { int nLinks = settings.NumLinks; float mass = 13.5f * Scaling; Vector3 inertia = new Vector3(91, 344, 253) * Scaling * Scaling; var body = new MultiBody(nLinks, mass, inertia, settings.IsFixedBase, settings.CanSleep); //body.HasSelfCollision = false; //Quaternion orn = new Quaternion(0, 0, 1, -0.125f * Math.PI); Quaternion orn = new Quaternion(0, 0, 0, 1); body.BasePosition = settings.BasePosition; body.WorldToBaseRot = orn; body.BaseVelocity = Vector3.Zero; Vector3 joint_axis_hinge = new Vector3(1, 0, 0); Vector3 joint_axis_prismatic = new Vector3(0, 0, 1); Quaternion parent_to_child = orn.Inverse(); Vector3 joint_axis_child_prismatic = parent_to_child.Rotate(joint_axis_prismatic); Vector3 joint_axis_child_hinge = parent_to_child.Rotate(joint_axis_hinge); int this_link_num = -1; int link_num_counter = 0; Vector3 pos = new Vector3(0, 0, 9.0500002f) * Scaling; Vector3 joint_axis_position = new Vector3(0, 0, 4.5250001f) * Scaling; for (int i = 0; i < nLinks; i++) { float initial_joint_angle = 0.3f; if (i > 0) { initial_joint_angle = -0.06f; } int child_link_num = link_num_counter++; if (settings.UsePrismatic) // i == (nLinks - 1)) { body.SetupPrismatic(child_link_num, mass, inertia, this_link_num, parent_to_child, joint_axis_child_prismatic, parent_to_child.Rotate(pos), Vector3.Zero, settings.DisableParentCollision); } else { body.SetupRevolute(child_link_num, mass, inertia, this_link_num, parent_to_child, joint_axis_child_hinge, joint_axis_position, parent_to_child.Rotate(pos - joint_axis_position), settings.DisableParentCollision); } body.SetJointPos(child_link_num, initial_joint_angle); this_link_num = i; /*if (false) //!useGroundShape && i == 4) * { * Vector3 pivotInAworld = new Vector3(0, 20, 46); * Vector3 pivotInAlocal = body.WorldPosToLocal(i, pivotInAworld); * Vector3 pivotInBworld = pivotInAworld; * MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(body, i, TypedConstraint.FixedBody, pivotInAlocal, pivotInBworld); * (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p); * }*/ if (settings.UsePrismatic) { //MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, nLinks - 1, 2, 3); if (settings.CreateConstraints) { MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, i, -1, 1); (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con); } } else { //if (true) { var con = new MultiBodyJointMotor(body, i, 0, 50000); (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con); } var con2 = new MultiBodyJointLimitConstraint(body, i, -1, 1); (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con2); } } // Add a collider for the base Quaternion[] worldToLocal = new Quaternion[nLinks + 1]; Vector3[] localOrigin = new Vector3[nLinks + 1]; worldToLocal[0] = body.WorldToBaseRot; localOrigin[0] = body.BasePosition; //Vector3 halfExtents = new Vector3(7.5f, 0.05f, 4.5f); Vector3 halfExtents = new Vector3(7.5f, 0.45f, 4.5f); float[] posB = new float[] { localOrigin[0].X, localOrigin[0].Y, localOrigin[0].Z, 1 }; //float[] quatB = new float[] { worldToLocal[0].X, worldToLocal[0].Y, worldToLocal[0].Z, worldToLocal[0].W }; //if (true) { CollisionShape box = new BoxShape(halfExtents * Scaling); var bodyInfo = new RigidBodyConstructionInfo(mass, null, box, inertia); RigidBody bodyB = new RigidBody(bodyInfo); var collider = new MultiBodyLinkCollider(body, -1); collider.CollisionShape = box; Matrix tr = Matrix.RotationQuaternion(worldToLocal[0].Inverse()) * Matrix.Translation(localOrigin[0]); collider.WorldTransform = tr; bodyB.WorldTransform = tr; World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter, CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter); collider.Friction = Friction; body.BaseCollider = collider; } for (int i = 0; i < body.NumLinks; i++) { int parent = body.GetParent(i); worldToLocal[i + 1] = body.GetParentToLocalRot(i) * worldToLocal[parent + 1]; localOrigin[i + 1] = localOrigin[parent + 1] + (worldToLocal[i + 1].Inverse().Rotate(body.GetRVector(i))); } for (int i = 0; i < body.NumLinks; i++) { CollisionShape box = new BoxShape(halfExtents * Scaling); var collider = new MultiBodyLinkCollider(body, i); collider.CollisionShape = box; Matrix tr = Matrix.RotationQuaternion(worldToLocal[i + 1].Inverse()) * Matrix.Translation(localOrigin[i + 1]); collider.WorldTransform = tr; World.AddCollisionObject(collider, CollisionFilterGroups.StaticFilter, CollisionFilterGroups.DefaultFilter | CollisionFilterGroups.StaticFilter); collider.Friction = Friction; body.GetLink(i).Collider = collider; //World.DebugDrawer.DrawBox(halfExtents, pos, quat); } (World as MultiBodyDynamicsWorld).AddMultiBody(body); return(body); }
public PendulumDemoSimulation() { CollisionConfiguration = new DefaultCollisionConfiguration(); Dispatcher = new CollisionDispatcher(CollisionConfiguration); Broadphase = new DbvtBroadphase(); _solver = new MultiBodyConstraintSolver(); MultiBodyWorld = new MultiBodyDynamicsWorld(Dispatcher, Broadphase, _solver, CollisionConfiguration); World.SetInternalTickCallback(TickCallback, null, true); const bool floating = false; const bool gyro = false; const int numLinks = 1; const bool canSleep = false; const bool selfCollide = false; var linkHalfExtents = new Vector3(0.05f, 0.5f, 0.1f); var baseHalfExtents = new Vector3(0.05f, 0.5f, 0.1f); var baseInertiaDiag = Vector3.Zero; const float baseMass = 0; MultiBody = new MultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep); //MultiBody.UseRK4Integration = true; //MultiBody.BaseWorldTransform = Matrix.Identity; //init the links var hingeJointAxis = new Vector3(1, 0, 0); //y-axis assumed up Vector3 parentComToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0); Vector3 currentPivotToCurrentCom = new Vector3(0, -linkHalfExtents[1], 0); Vector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; for (int i = 0; i < numLinks; i++) { const float linkMass = 10; Vector3 linkInertiaDiag = Vector3.Zero; using (var shape = new SphereShape(radius)) { shape.CalculateLocalInertia(linkMass, out linkInertiaDiag); } MultiBody.SetupRevolute(i, linkMass, linkInertiaDiag, i - 1, Quaternion.Identity, hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); } MultiBody.FinalizeMultiDof(); MultiBodyWorld.AddMultiBody(MultiBody); MultiBody.CanSleep = canSleep; MultiBody.HasSelfCollision = selfCollide; MultiBody.UseGyroTerm = gyro; #if PENDULUM_DAMPING MultiBody.LinearDamping = 0.1f; MultiBody.AngularDamping = 0.9f; #else MultiBody.LinearDamping = 0; MultiBody.AngularDamping = 0; #endif for (int i = 0; i < numLinks; i++) { var shape = new SphereShape(radius); var col = new MultiBodyLinkCollider(MultiBody, i); col.CollisionShape = shape; const bool isDynamic = true; CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter; CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.StaticFilter; World.AddCollisionObject(col, collisionFilterGroup, collisionFilterMask); MultiBody.GetLink(i).Collider = col; } }
/// <summary> /// BMultiBodyLinks do not create their own native parts. That happens as part of the MultiBody creation. /// After the native MultiBody has been created. The references to the native MultiBodyLinkColliders can be /// obtatined and are assigned using this function. /// </summary> internal void AssignMultiBodyLinkColliderOnCreation(BMultiBody mb, MultiBodyLinkCollider linkCollider) { m_linkCollider = linkCollider; }
//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); }
public virtual void OnHandleInput() { if (Input.KeysPressed.Count != 0) { switch (Input.KeysPressed[0]) { case Keys.Escape: case Keys.Q: Graphics.Form.Close(); return; case Keys.F3: IsDebugDrawEnabled = !IsDebugDrawEnabled; break; case Keys.F8: Input.ClearKeyCache(); GraphicsLibraryManager.ExitWithReload = true; Graphics.Form.Close(); break; case Keys.F11: Graphics.IsFullScreen = !Graphics.IsFullScreen; break; case Keys.G: //shadowsEnabled = !shadowsEnabled; break; case Keys.Space: ShootBox(Freelook.Eye, GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView)); break; case Keys.Return: ClientResetScene(); break; } } if (Input.MousePressed != MouseButtons.None) { Vector3 rayTo = GetRayTo(Input.MousePoint, Freelook.Eye, Freelook.Target, Graphics.FieldOfView); if (Input.MousePressed == MouseButtons.Right) { if (_world != null) { Vector3 rayFrom = Freelook.Eye; ClosestRayResultCallback rayCallback = new ClosestRayResultCallback(ref rayFrom, ref rayTo); _world.RayTest(ref rayFrom, ref rayTo, rayCallback); if (rayCallback.HasHit) { Vector3 pickPos = rayCallback.HitPointWorld; RigidBody body = rayCallback.CollisionObject as RigidBody; if (body != null) { if (!(body.IsStaticObject || body.IsKinematicObject)) { pickedBody = body; pickedBody.ActivationState = ActivationState.DisableDeactivation; Vector3 localPivot = Vector3.TransformCoordinate(pickPos, Matrix.Invert(body.CenterOfMassTransform)); if (Input.KeysDown.Contains(Keys.ShiftKey)) { Generic6DofConstraint dof6 = new Generic6DofConstraint(body, Matrix.Translation(localPivot), false) { LinearLowerLimit = Vector3.Zero, LinearUpperLimit = Vector3.Zero, AngularLowerLimit = Vector3.Zero, AngularUpperLimit = Vector3.Zero }; _world.AddConstraint(dof6); pickConstraint = dof6; dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 0); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 1); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 2); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 3); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 4); dof6.SetParam(ConstraintParam.StopCfm, 0.8f, 5); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 0); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 1); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 2); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 3); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 4); dof6.SetParam(ConstraintParam.StopErp, 0.1f, 5); } else { Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot); _world.AddConstraint(p2p); pickConstraint = p2p; p2p.Setting.ImpulseClamp = 30; //very weak constraint for picking p2p.Setting.Tau = 0.001f; /* * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 0); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 1); * p2p.SetParam(ConstraintParams.Cfm, 0.8f, 2); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 0); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 1); * p2p.SetParam(ConstraintParams.Erp, 0.1f, 2); */ } } } else { MultiBodyLinkCollider multiCol = rayCallback.CollisionObject as MultiBodyLinkCollider; if (multiCol != null && multiCol.MultiBody != null) { MultiBody mb = multiCol.MultiBody; prevCanSleep = mb.CanSleep; mb.CanSleep = false; Vector3 pivotInA = mb.WorldPosToLocal(multiCol.Link, pickPos); MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(mb, multiCol.Link, null, pivotInA, pickPos); p2p.MaxAppliedImpulse = 2; (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p); pickingMultiBodyPoint2Point = p2p; } } oldPickingDist = (pickPos - rayFrom).Length; } rayCallback.Dispose(); } } } else if (Input.MouseReleased == MouseButtons.Right) { RemovePickingConstraint(); } // Mouse movement if (Input.MouseDown == MouseButtons.Right) { MovePickedBody(); } }