private HkConstraintData CreateFixedConstraintData(ref Matrix otherLocalMatrix, float headDistance) { m_fixedConstraintData = new HkFixedConstraintData(); Matrix headPivotLocalMatrix; GetHeadPivotLocalMatrix(headDistance, out headPivotLocalMatrix); m_fixedConstraintData.SetInBodySpace(otherLocalMatrix, headPivotLocalMatrix, m_otherPhysicsBody, OwnerVirtualPhysics); if (MyFakes.MANIPULATION_TOOL_VELOCITY_LIMIT) { m_fixedConstraintData.MaximumLinearImpulse = 0.5f; m_fixedConstraintData.MaximumAngularImpulse = 0.5f; HkMalleableConstraintData mcData = new HkMalleableConstraintData(); mcData.SetData(m_fixedConstraintData); mcData.Strength = 0.0001f; m_fixedConstraintData.Dispose(); return(mcData); } else { m_fixedConstraintData.MaximumLinearImpulse = 0.005f; //7500 * MyDestructionHelper.MASS_REDUCTION_COEF * (MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS * MyFakes.SIMULATION_SPEED); m_fixedConstraintData.MaximumAngularImpulse = 0.005f; //7500 * MyDestructionHelper.MASS_REDUCTION_COEF * (MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS * MyFakes.SIMULATION_SPEED); } return(m_fixedConstraintData); //HkBreakableConstraintData bcData = new HkBreakableConstraintData(m_fixedConstraintData); //bcData.ReapplyVelocityOnBreak = false; //bcData.RemoveFromWorldOnBrake = true; //bcData.Threshold = /*100000;*/ 2500 * MyDestructionHelper.MASS_REDUCTION_COEF; //return bcData; }
private void CreateConstraint(MyCubeGrid other, MyShipMergeBlock block) { var data = new HkPrismaticConstraintData(); data.MaximumLinearLimit = 0; data.MinimumLinearLimit = 0; var posA = ConstraintPositionInGridSpace(); var posB = block.ConstraintPositionInGridSpace(); var axisA = PositionComp.LocalMatrix.GetDirectionVector(m_forward); var axisAPerp = PositionComp.LocalMatrix.GetDirectionVector(m_right); var axisB = -block.PositionComp.LocalMatrix.GetDirectionVector(m_forward); Base6Directions.Direction thisRightForOther = block.WorldMatrix.GetClosestDirection(WorldMatrix.GetDirectionVector(m_right)); Base6Directions.Direction otherRight = WorldMatrix.GetClosestDirection(block.WorldMatrix.GetDirectionVector(block.m_right)); var axisBPerp = block.PositionComp.LocalMatrix.GetDirectionVector(thisRightForOther); data.SetInBodySpace(posA, posB, axisA, axisB, axisAPerp, axisBPerp, CubeGrid.Physics, other.Physics); var data2 = new HkMalleableConstraintData(); data2.SetData(data); data.ClearHandle(); data = null; data2.Strength = 0.00001f; var constraint = new HkConstraint(CubeGrid.Physics.RigidBody, other.Physics.RigidBody, data2); AddConstraint(constraint); SetConstraint(block, constraint, otherRight); m_other.SetConstraint(this, constraint, thisRightForOther); }
private HkConstraintData CreateBallAndSocketConstraintData(ref Matrix otherLocalMatrix, ref Matrix headPivotLocalMatrix) { HkBallAndSocketConstraintData data = new HkBallAndSocketConstraintData(); Vector3 otherPivot = otherLocalMatrix.Translation; Vector3 headPivot = headPivotLocalMatrix.Translation; data.SetInBodySpace(otherPivot, headPivot, m_otherPhysicsBody, OwnerVirtualPhysics); HkMalleableConstraintData mcData = new HkMalleableConstraintData(); mcData.SetData(data); mcData.Strength = 0.00006f; data.Dispose(); return(mcData); }
private static HkConstraintData CreateBallAndSocketConstraintData(ref Matrix otherLocalMatrix, ref Matrix headPivotLocalMatrix) { HkBallAndSocketConstraintData data = new HkBallAndSocketConstraintData(); Vector3 otherPivot = otherLocalMatrix.Translation; Vector3 headPivot = headPivotLocalMatrix.Translation; data.SetInBodySpace(ref otherPivot, ref headPivot); HkMalleableConstraintData mcData = new HkMalleableConstraintData(); mcData.SetData(data); mcData.Strength = 0.0003f; data.Dispose(); return(mcData); }
private void CreateConstraint(MyCubeGrid other, MyShipMergeBlock block) { var data = new HkPrismaticConstraintData(); data.MaximumLinearLimit = 0; data.MinimumLinearLimit = 0; var posA = ConstraintPositionInGridSpace(); var posB = block.ConstraintPositionInGridSpace(); var axisA = PositionComp.LocalMatrix.GetDirectionVector(m_forward); var axisAPerp = PositionComp.LocalMatrix.GetDirectionVector(m_right); var axisB = -block.PositionComp.LocalMatrix.GetDirectionVector(m_forward); Base6Directions.Direction thisRightForOther = block.WorldMatrix.GetClosestDirection(WorldMatrix.GetDirectionVector(m_right)); Base6Directions.Direction otherRight = WorldMatrix.GetClosestDirection(block.WorldMatrix.GetDirectionVector(block.m_right)); var axisBPerp = block.PositionComp.LocalMatrix.GetDirectionVector(thisRightForOther); data.SetInBodySpace( posA, posB, axisA, axisB, axisAPerp, axisBPerp, CubeGrid.Physics, other.Physics); var data2 = new HkMalleableConstraintData(); data2.SetData(data); data.ClearHandle(); data = null; data2.Strength = 0.00001f; var constraint = new HkConstraint(CubeGrid.Physics.RigidBody, other.Physics.RigidBody, data2); AddConstraint(constraint); SetConstraint(block, constraint, otherRight); m_other.SetConstraint(this, constraint, thisRightForOther); }
public void StartManipulation(MyState state, MyEntity otherEntity, Vector3D hitPosition, ref MatrixD ownerWorldHeadMatrix, bool fromServer = false) { Debug.Assert(m_constraintInitialized == false); // Commenting this out to allow picking up dead bodies and characters if (otherEntity is MyCharacter) { return; } if (Owner == null) { Debug.Assert(!fromServer, "Desync!"); return; } if (otherEntity.Physics == null) { return; } m_otherRigidBody = otherEntity.Physics.RigidBody; m_otherPhysicsBody = otherEntity.Physics; if (otherEntity is MyCharacter) { if (!(otherEntity as MyCharacter).IsDead || !((otherEntity as MyCharacter).Components.Has <MyCharacterRagdollComponent>() && (otherEntity as MyCharacter).Components.Get <MyCharacterRagdollComponent>().IsRagdollActivated)) { Debug.Assert(!fromServer, "Desync!"); return; } m_otherRigidBody = (otherEntity as MyCharacter).Physics.Ragdoll.GetRootRigidBody(); } // else (!otherEntity.Physics.RigidBody.InWorld) // Do we need to check if the body is in the world when this was returned byt RayCast ? Commenting this out for now.. var characterMovementState = Owner.GetCurrentMovementState(); if (!CanManipulate(characterMovementState)) { Debug.Assert(!fromServer, "Desync!"); return; } if (!CanManipulateEntity(otherEntity)) { Debug.Assert(!fromServer, "Desync!"); return; } m_otherRigidBody.Activate(); Vector3D hitUp = Vector3D.Up; Vector3D hitRight; double dot = Vector3D.Dot(ownerWorldHeadMatrix.Forward, hitUp); if (dot == 1 || dot == -1) { hitRight = ownerWorldHeadMatrix.Right; } else { hitRight = Vector3D.Cross(ownerWorldHeadMatrix.Forward, hitUp); hitRight.Normalize(); } Vector3D hitForward = Vector3D.Cross(hitUp, hitRight); hitForward.Normalize(); // Matrix of constraint for other body in world MatrixD otherWorldMatrix = MatrixD.Identity; otherWorldMatrix.Forward = hitForward; otherWorldMatrix.Right = hitRight; otherWorldMatrix.Up = hitUp; otherWorldMatrix.Translation = hitPosition; const float headPivotOffset = 1.5f; MatrixD headPivotWorldMatrix = ownerWorldHeadMatrix; headPivotWorldMatrix.Translation = ownerWorldHeadMatrix.Translation + headPivotOffset * ownerWorldHeadMatrix.Forward; //float distanceToHead = (float)(headPivotWorldMatrix.Translation - otherWorldMatrix.Translation).Length(); //distanceToHead = MathHelper.Clamp(distanceToHead, 0.6f, 2.5f); // Matrix of constraint for other body in local m_otherLocalPivotMatrix = (Matrix)(otherWorldMatrix * otherEntity.PositionComp.WorldMatrixNormalizedInv); m_otherWorldPivotOrigin = otherWorldMatrix.Translation; MatrixD ownerWorldMatrixInverse = MatrixD.Normalize(MatrixD.Invert(ownerWorldHeadMatrix)); m_headLocalPivotMatrix = Matrix.Identity; m_headLocalPivotMatrix.Translation = Vector3D.Transform(headPivotWorldMatrix.Translation, ownerWorldMatrixInverse); HkConstraintData data; if (state == MyState.HOLD) { if (!fromServer) { float mass = 0; if (otherEntity is MyCubeGrid) { var group = MyCubeGridGroups.Static.Physical.GetGroup((otherEntity as MyCubeGrid)); foreach (var node in group.Nodes) { if (node.NodeData.IsStatic) //fixed constraint on part connected to static grid isnt good idea { return; } mass += node.NodeData.Physics.Mass; } mass = GetRealMass(mass); } else if (otherEntity is MyCharacter) { mass = (otherEntity as MyCharacter).Definition.Mass; } else { mass = GetRealMass(m_otherRigidBody.Mass); } // Player can hold large projectile (~222kg) if ((mass > 210) || ((otherEntity is MyCharacter) && (otherEntity.Physics.Mass > 210))) { return; } } if (!CreateOwnerVirtualPhysics()) { return; } if (IsOwnerLocalPlayer()) { var controllingPlayer = Sync.Players.GetControllingPlayer(otherEntity); if (controllingPlayer != null) { RemoveOwnerVirtualPhysics(); return; } if (!(otherEntity is MyCharacter)) // this would cause to start controlling the dead body.. { Sync.Players.TrySetControlledEntity(Owner.ControllerInfo.Controller.Player.Id, otherEntity); } } SetMotionOnClient(otherEntity, HkMotionType.Dynamic); data = CreateFixedConstraintData(ref m_otherLocalPivotMatrix, headPivotOffset); if (otherEntity is MyCharacter) { if (MyFakes.MANIPULATION_TOOL_VELOCITY_LIMIT) { HkMalleableConstraintData mcData = data as HkMalleableConstraintData; mcData.Strength = 0.005f; } else { HkFixedConstraintData fcData = data as HkFixedConstraintData; fcData.MaximumAngularImpulse = 0.0005f; fcData.MaximumLinearImpulse = 0.0005f; fcData.BreachImpulse = 0.0004f; } } } else { if (!CreateOwnerVirtualPhysics()) { return; } data = CreateBallAndSocketConstraintData(ref m_otherLocalPivotMatrix, ref m_headLocalPivotMatrix); if (otherEntity is MyCharacter) { HkMalleableConstraintData mcData = data as HkMalleableConstraintData; mcData.Strength = 0.005f; } } m_otherEntity = otherEntity; m_otherEntity.OnClosing += OtherEntity_OnClosing; //m_otherAngularDamping = m_otherRigidBody.AngularDamping; //m_otherLinearDamping = m_otherRigidBody.LinearDamping; m_otherRestitution = m_otherRigidBody.Restitution; m_otherMaxLinearVelocity = m_otherRigidBody.MaxLinearVelocity; m_otherMaxAngularVelocity = m_otherRigidBody.MaxAngularVelocity; SetManipulated(m_otherEntity, true); if (state == MyState.HOLD) { if (m_otherEntity is MyCharacter) { foreach (var body in m_otherEntity.Physics.Ragdoll.RigidBodies) { //body.AngularDamping = TARGET_ANGULAR_DAMPING; //body.LinearDamping = TARGET_LINEAR_DAMPING; //body.Mass = 5; body.Restitution = TARGET_RESTITUTION; body.MaxLinearVelocity = m_limitingLinearVelocity; body.MaxAngularVelocity = (float)Math.PI * 0.1f; } } else { m_massChange = HkMassChangerUtil.Create(m_otherRigidBody, int.MaxValue, 1, 0.001f); //m_otherRigidBody.AngularDamping = TARGET_ANGULAR_DAMPING; //m_otherRigidBody.LinearDamping = TARGET_LINEAR_DAMPING; m_otherRigidBody.Restitution = TARGET_RESTITUTION; m_otherRigidBody.MaxLinearVelocity = m_limitingLinearVelocity; m_otherRigidBody.MaxAngularVelocity = (float)Math.PI; } const float holdingTransparency = 0.4f; SetTransparent(otherEntity, holdingTransparency); } m_constraint = new HkConstraint(m_otherRigidBody, OwnerVirtualPhysics.RigidBody, data); OwnerVirtualPhysics.AddConstraint(m_constraint); m_constraintCreationTime = MySandboxGame.Static.UpdateTime; m_state = state; m_previousCharacterMovementState = Owner.GetCurrentMovementState(); if (m_state == MyState.HOLD) { Owner.PlayCharacterAnimation("PickLumber", MyBlendOption.Immediate, MyFrameOption.PlayOnce, 0.2f, 1f); } else { Owner.PlayCharacterAnimation("PullLumber", MyBlendOption.Immediate, MyFrameOption.PlayOnce, 0.2f, 1f); } m_manipulatedEntitites.Add(m_otherEntity); Owner.ManipulatedEntity = m_otherEntity; var handler = ManipulationStarted; if (handler != null) { handler(m_otherEntity); } }
private void CreateConstraintNosync(MyShipConnector otherConnector, ref Vector3 posA, ref Vector3 posB, ref Vector3 axisA, ref Vector3 axisB) { var data = new HkHingeConstraintData(); data.SetInBodySpace(ref posA, ref posB, ref axisA, ref axisB); var data2 = new HkMalleableConstraintData(); data2.SetData(data); data.ClearHandle(); data = null; data2.Strength = 0.0003f; var newConstraint = new HkConstraint(CubeGrid.Physics.RigidBody, otherConnector.CubeGrid.Physics.RigidBody, data2); this.Master = true; otherConnector.Master = false; SetConstraint(otherConnector, newConstraint); otherConnector.SetConstraint(this, newConstraint); AddConstraint(newConstraint); }
private void CreateConstraintNosync(MyShipConnector otherConnector) { Debug.Assert(IsMaster, "Constraints should be created only master (entity with higher EntityId)"); var posA = ConstraintPositionInGridSpace(); var posB = otherConnector.ConstraintPositionInGridSpace(); var axisA = ConstraintAxisGridSpace(); var axisB = -otherConnector.ConstraintAxisGridSpace(); var data = new HkHingeConstraintData(); data.SetInBodySpace(posA, posB, axisA, axisB, CubeGrid.Physics, otherConnector.CubeGrid.Physics); var data2 = new HkMalleableConstraintData(); data2.SetData(data); data.ClearHandle(); data = null; data2.Strength = GetEffectiveStrength(otherConnector); var newConstraint = new HkConstraint(CubeGrid.Physics.RigidBody, otherConnector.CubeGrid.Physics.RigidBody, data2); SetConstraint(otherConnector, newConstraint); otherConnector.SetConstraint(this, newConstraint); AddConstraint(newConstraint); }
void CubeGrid_OnPhysicsChanged(MyEntity obj) { if (Sync.IsServer && m_welding == false && InConstraint && m_welded == false) { if (m_hasConstraint) { if (MyPhysicsBody.IsConstraintValid(m_constraint) == false && m_constraint.IsDisposed == false) { RemoveConstraint(m_other, m_constraint); this.m_constraint = null; m_other.m_constraint = null; m_hasConstraint = false; m_other.m_hasConstraint = false; if (m_connectionState.Value.MasterToSlave.HasValue == false && CubeGrid.Physics != null && m_other.CubeGrid.Physics != null) { var posA = ConstraintPositionInGridSpace(); var posB = m_other.ConstraintPositionInGridSpace(); var axisA = ConstraintAxisGridSpace(); var axisB = -m_other.ConstraintAxisGridSpace(); var data = new HkHingeConstraintData(); data.SetInBodySpace(posA, posB, axisA, axisB, CubeGrid.Physics, m_other.CubeGrid.Physics); var data2 = new HkMalleableConstraintData(); data2.SetData(data); data.ClearHandle(); data = null; data2.Strength = GetEffectiveStrength(m_other); var newConstraint = new HkConstraint(CubeGrid.Physics.RigidBody, m_other.CubeGrid.Physics.RigidBody, data2); this.SetConstraint(m_other, newConstraint); m_other.SetConstraint(this, newConstraint); AddConstraint(newConstraint); } } } } }
public void StartManipulation(MyState state, MyEntity otherEntity, Vector3D hitPosition, ref MatrixD ownerWorldHeadMatrix) { Debug.Assert(m_constraintInitialized == false); // Commenting this out to allow picking up dead bodies and characters //if (otherEntity is MyCharacter) // return; if (Owner == null || Owner.VirtualPhysics == null || !Owner.VirtualPhysics.RigidBody.InWorld) { return; } var ownerRigidBody = Owner.VirtualPhysics.RigidBody; if (otherEntity.Physics == null) { return; } m_otherRigidBody = otherEntity.Physics.RigidBody; if (otherEntity is MyCharacter) { if (!(otherEntity as MyCharacter).IsDead || !(otherEntity as MyCharacter).IsRagdollActivated) { return; } m_otherRigidBody = (otherEntity as MyCharacter).Physics.Ragdoll.GetRootRigidBody(); } // else (!otherEntity.Physics.RigidBody.InWorld) // Do we need to check if the body is in the world when this was returned byt RayCast ? Commenting this out for now.. var characterMovementState = Owner.GetCurrentMovementState(); if (!CanManipulate(characterMovementState)) { return; } if (!CanManipulateEntity(otherEntity)) { return; } // PetrM:TODO: Make all client grids dynamic. if (!(otherEntity is MyCharacter && (otherEntity as MyCharacter).IsRagdollActivated) && state == MyState.HOLD) // In case of picking up a ragdoll don't turn off and on the physics { otherEntity.Physics.Enabled = false; otherEntity.SyncFlag = false; m_otherRigidBody.UpdateMotionType(HkMotionType.Dynamic); otherEntity.Physics.Enabled = true; // HACK: This is here only because disabling and enabling physics puts constraints into inconsistent state. otherEntity.RaisePhysicsChanged(); } Owner.VirtualPhysics.RigidBody.Activate(); m_otherRigidBody.Activate(); Vector3D hitUp = Vector3D.Up; Vector3D hitRight; double dot = Vector3D.Dot(ownerWorldHeadMatrix.Forward, hitUp); if (dot == 1 || dot == -1) { hitRight = ownerWorldHeadMatrix.Right; } else { hitRight = Vector3D.Cross(ownerWorldHeadMatrix.Forward, hitUp); hitRight.Normalize(); } Vector3D hitForward = Vector3D.Cross(hitUp, hitRight); hitForward.Normalize(); // Matrix of constraint for other body in world MatrixD otherWorldMatrix = MatrixD.Identity; otherWorldMatrix.Forward = hitForward; otherWorldMatrix.Right = hitRight; otherWorldMatrix.Up = hitUp; otherWorldMatrix.Translation = hitPosition; const float headPivotOffset = 1.5f; MatrixD headPivotWorldMatrix = ownerWorldHeadMatrix; headPivotWorldMatrix.Translation = ownerWorldHeadMatrix.Translation + headPivotOffset * ownerWorldHeadMatrix.Forward; //float distanceToHead = (float)(headPivotWorldMatrix.Translation - otherWorldMatrix.Translation).Length(); //distanceToHead = MathHelper.Clamp(distanceToHead, 0.6f, 2.5f); // Matrix of constraint for other body in local m_otherLocalPivotMatrix = (Matrix)(otherWorldMatrix * otherEntity.PositionComp.WorldMatrixNormalizedInv); m_otherWorldPivotOrigin = otherWorldMatrix.Translation; MatrixD ownerWorldMatrixInverse = MatrixD.Normalize(MatrixD.Invert(ownerWorldHeadMatrix)); m_headLocalPivotMatrix = Matrix.Identity; m_headLocalPivotMatrix.Translation = Vector3D.Transform(headPivotWorldMatrix.Translation, ownerWorldMatrixInverse); HkConstraintData data; if (state == MyState.HOLD) { float mass = 0; if (otherEntity is MyCubeGrid) { var group = MyCubeGridGroups.Static.Physical.GetGroup((otherEntity as MyCubeGrid)); foreach (var node in group.Nodes) { mass += node.NodeData.Physics.Mass; } mass = GetRealMass(mass); } else { mass = GetRealMass(otherEntity.Physics.Mass); } // Player can hold large projectile (~222kg) if ((mass <= 210) || ((otherEntity is MyCharacter) && (otherEntity.Physics.Mass < 210))) { data = CreateFixedConstraintData(ref m_otherLocalPivotMatrix, headPivotOffset); } else { return; } if (otherEntity is MyCharacter) { if (MyFakes.MANIPULATION_TOOL_VELOCITY_LIMIT) { HkMalleableConstraintData mcData = data as HkMalleableConstraintData; mcData.Strength = 0.005f; } else { HkFixedConstraintData fcData = data as HkFixedConstraintData; fcData.MaximumAngularImpulse = 2.0f; fcData.MaximumLinearImpulse = 2.0f; } } } else { data = CreateBallAndSocketConstraintData(ref m_otherLocalPivotMatrix, ref m_headLocalPivotMatrix); if (otherEntity is MyCharacter) { HkMalleableConstraintData mcData = data as HkMalleableConstraintData; mcData.Strength = 0.005f; } } m_otherEntity = otherEntity; m_otherEntity.OnClosing += OtherEntity_OnClosing; //m_otherAngularDamping = m_otherRigidBody.AngularDamping; //m_otherLinearDamping = m_otherRigidBody.LinearDamping; m_otherRestitution = m_otherRigidBody.Restitution; m_otherMaxLinearVelocity = m_otherRigidBody.MaxLinearVelocity; m_otherMaxAngularVelocity = m_otherRigidBody.MaxAngularVelocity; SetManipulated(m_otherEntity, true); if (state == MyState.HOLD) { m_massChange = HkMassChangerUtil.Create(m_otherRigidBody, int.MaxValue, 1, 0.001f); //m_otherRigidBody.AngularDamping = TARGET_ANGULAR_DAMPING; //m_otherRigidBody.LinearDamping = TARGET_LINEAR_DAMPING; m_otherRigidBody.Restitution = TARGET_RESTITUTION; m_otherRigidBody.MaxLinearVelocity = m_limitingLinearVelocity; m_otherRigidBody.MaxAngularVelocity = (float)Math.PI; if (m_otherEntity is MyCharacter) { foreach (var body in m_otherEntity.Physics.Ragdoll.RigidBodies) { //body.AngularDamping = TARGET_ANGULAR_DAMPING; //body.LinearDamping = TARGET_LINEAR_DAMPING; body.Restitution = TARGET_RESTITUTION; body.MaxLinearVelocity = m_limitingLinearVelocity; body.MaxAngularVelocity = (float)Math.PI; } } const float holdingTransparency = 0.4f; SetTransparent(otherEntity, holdingTransparency); //TODO jt: why it must be called twice? SetTransparent(otherEntity, holdingTransparency); } m_constraint = new HkConstraint(m_otherRigidBody, Owner.VirtualPhysics.RigidBody, data); Owner.VirtualPhysics.AddConstraint(m_constraint); m_constraintCreationTime = MySandboxGame.Static.UpdateTime; m_state = state; m_previousCharacterMovementState = Owner.GetCurrentMovementState(); if (m_state == MyState.HOLD) { Owner.PlayCharacterAnimation("PickLumber", false, MyPlayAnimationMode.Immediate | MyPlayAnimationMode.Play, 0.2f, 1f); } else { Owner.PlayCharacterAnimation("PullLumber", false, MyPlayAnimationMode.Immediate | MyPlayAnimationMode.Play, 0.2f, 1f); } m_manipulatedEntitites.Add(m_otherEntity); Owner.ManipulatedEntity = m_otherEntity; }
void CubeGrid_OnPhysicsChanged(MyEntity obj) { if (m_hasConstraint) { if (MyPhysicsBody.IsConstraintValid(m_constraint) == false && m_constraint.IsDisposed == false) { RemoveConstraint(m_other, m_constraint); if (m_connectionState.Value.MasterToSlave.HasValue) { Matrix localSpaceA = Matrix.CreateTranslation(m_connectionPosition); Matrix localSpaceB = localSpaceA * m_connectionState.Value.MasterToSlave.Value; localSpaceA = localSpaceA * this.PositionComp.LocalMatrix; localSpaceB = localSpaceB * m_other.PositionComp.LocalMatrix; var data = new HkFixedConstraintData(); data.SetInBodySpace(localSpaceA, localSpaceB, CubeGrid.Physics, m_other.CubeGrid.Physics); var newConstraint = new HkConstraint(CubeGrid.Physics.RigidBody, m_other.CubeGrid.Physics.RigidBody, data); this.SetConstraint(m_other, newConstraint); m_other.SetConstraint(this, newConstraint); AddConstraint(newConstraint); } else { var posA = ConstraintPositionInGridSpace(); var posB = m_other.ConstraintPositionInGridSpace(); var axisA = ConstraintAxisGridSpace(); var axisB = -m_other.ConstraintAxisGridSpace(); var data = new HkHingeConstraintData(); data.SetInBodySpace(posA, posB, axisA, axisB, CubeGrid.Physics, m_other.CubeGrid.Physics); var data2 = new HkMalleableConstraintData(); data2.SetData(data); data.ClearHandle(); data = null; data2.Strength = GetEffectiveStrength(m_other); var newConstraint = new HkConstraint(CubeGrid.Physics.RigidBody, m_other.CubeGrid.Physics.RigidBody, data2); this.SetConstraint(m_other, newConstraint); m_other.SetConstraint(this, newConstraint); AddConstraint(newConstraint); } } } }