示例#1
0
    public void OnDrawGizmos()
    {
        if (LocalDirection.Equals(Vector3.zero))
        {
            return;
        }

        var originalColor  = Gizmos.color;
        var originalMatrix = Gizmos.matrix;

        Gizmos.color = Color.red;

        // Calculate the final Physics Body runtime coordinate system which bakes out skew from non-uniform scaling in parent
        var worldFromLocalRigidTransform = Math.DecomposeRigidBodyTransform(transform.localToWorldMatrix);
        var worldFromLocal = Matrix4x4.TRS(worldFromLocalRigidTransform.pos, worldFromLocalRigidTransform.rot, Vector3.one);

        Vector3 directionWorld = worldFromLocal.MultiplyVector(LocalDirection.normalized);
        Vector3 offsetWorld    = worldFromLocal.MultiplyPoint(LocalOffset);

        // Calculate the final world Thrust coordinate system from the world Body transform and local offset and direction
        Math.CalculatePerpendicularNormalized(directionWorld, out _, out var directionPerpendicular);
        var worldFromThrust = Matrix4x4.TRS(offsetWorld, Quaternion.LookRotation(directionWorld, directionPerpendicular), Vector3.one);

        Gizmos.matrix = worldFromThrust;

        float Shift = Magnitude * 0.1f;

        Gizmos.DrawFrustum(new Vector3(0, 0, -Shift), UnityEngine.Random.Range(1.0f, 2.5f), Magnitude, Shift, 1.0f);

        Gizmos.matrix = originalMatrix;
        Gizmos.color  = originalColor;
    }
示例#2
0
    protected override JobHandle OnUpdate(JobHandle inputDeps)
    {
        if (m_ContactModifierGroup.CalculateLength() == 0)
        {
            return(inputDeps);
        }

        if (m_StepPhysicsWorld.Simulation.Type == SimulationType.NoPhysics)
        {
            return(inputDeps);
        }

        SimulationCallbacks.Callback preparationCallback = (ref ISimulation simulation, JobHandle inDeps) =>
        {
            inDeps.Complete();  // shouldn't be needed (jobify the below)

            SimulationData.Contacts.Iterator iterator = simulation.Contacts.GetIterator();
            while (iterator.HasItemsLeft())
            {
                ContactHeader manifold = iterator.GetNextContactHeader();

                // JacobianModifierFlags format for this example
                // UserData 0 - soft contact
                // UserData 1 - surface velocity
                // UserData 2 - infinite inertia
                // UserData 3 - no torque
                // UserData 4 - clip impulse
                // UserData 5 - disabled contact

                if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 0)) ||
                    0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 0)))
                {
                    manifold.JacobianFlags |= JacobianFlags.UserFlag0; // Soft Contact
                }
                if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 1)) ||
                    0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 1)))
                {
                    manifold.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity;
                }
                if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 2)) ||
                    0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 2)))
                {
                    manifold.JacobianFlags |= JacobianFlags.EnableMassFactors;
                }
                if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 3)) ||
                    0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 3)))
                {
                    manifold.JacobianFlags |= JacobianFlags.UserFlag1; // No Torque
                }
                if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 4)) ||
                    0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 4)))
                {
                    manifold.JacobianFlags |= JacobianFlags.EnableMaxImpulse; // No Torque
                }
                if (0 != (manifold.BodyCustomDatas.CustomDataA & (byte)(1 << 5)) ||
                    0 != (manifold.BodyCustomDatas.CustomDataB & (byte)(1 << 5)))
                {
                    manifold.JacobianFlags |= JacobianFlags.Disabled;
                }

                iterator.UpdatePreviousContactHeader(manifold);

                // Just read contacts
                for (int i = 0; i < manifold.NumContacts; i++)
                {
                    iterator.GetNextContact();
                }
            }

            return(inDeps);
        };

        SimulationCallbacks.Callback jacobianModificationCallback = (ref ISimulation simulation, JobHandle inDeps) =>
        {
            inDeps.Complete();  // shouldn't be needed (jobify the below)

            JacobianIterator iterator = simulation.Jacobians.Iterator;
            while (iterator.HasJacobiansLeft())
            {
                // JacobianModifierFlags format for this example
                // UserFlag0 - soft contact
                // UserFlag1 - no torque

                // Jacobian header
                ref JacobianHeader jacHeader = ref iterator.ReadJacobianHeader();

                // Triggers can only be disabled, other modifiers have no effect
                if (jacHeader.Type == JacobianType.Contact)
                {
                    // Contact jacobian modification
                    ref ContactJacobian contactJacobian = ref jacHeader.AccessBaseJacobian <ContactJacobian>();
                    {
                        // Check if NoTorque modifier
                        if ((jacHeader.Flags & JacobianFlags.UserFlag1) != 0)
                        {
                            // Disable all friction angular effects
                            contactJacobian.Friction0.AngularA       = 0.0f;
                            contactJacobian.Friction1.AngularA       = 0.0f;
                            contactJacobian.AngularFriction.AngularA = 0.0f;
                            contactJacobian.Friction0.AngularB       = 0.0f;
                            contactJacobian.Friction1.AngularB       = 0.0f;
                            contactJacobian.AngularFriction.AngularB = 0.0f;
                        }

                        // Check if SurfaceVelocity present
                        if (jacHeader.HasSurfaceVelocity)
                        {
                            // Since surface normal can change, make sure angular velocity is always relative to it, not independent
                            float3 angVel = contactJacobian.BaseJacobian.Normal * (new float3(0.0f, 1.0f, 0.0f));
                            float3 linVel = float3.zero;

                            Math.CalculatePerpendicularNormalized(contactJacobian.BaseJacobian.Normal, out float3 dir0, out float3 dir1);
                            float linVel0 = math.dot(linVel, dir0);
                            float linVel1 = math.dot(linVel, dir1);

                            float angVelProj = math.dot(angVel, contactJacobian.BaseJacobian.Normal);

                            jacHeader.SurfaceVelocity = new SurfaceVelocity {
                                ExtraFrictionDv = new float3(linVel0, linVel1, angVelProj)
                            };
                        }
示例#3
0
        public void Execute(ref ModifiableJacobianHeader jacHeader, ref ModifiableContactJacobian contactJacobian)
        {
            Entity entityA = jacHeader.Entities.EntityA;
            Entity entityB = jacHeader.Entities.EntityB;

            ModifyContactJacobians.ModificationType typeA = ModifyContactJacobians.ModificationType.None;
            if (modificationData.Exists(entityA))
            {
                typeA = modificationData[entityA].type;
            }

            ModifyContactJacobians.ModificationType typeB = ModifyContactJacobians.ModificationType.None;
            if (modificationData.Exists(entityB))
            {
                typeB = modificationData[entityB].type;
            }

            {
                // Check for jacobians we want to ignore:
                if (typeA == ModifyContactJacobians.ModificationType.DisabledContact || typeB == ModifyContactJacobians.ModificationType.DisabledContact)
                {
                    jacHeader.Flags = jacHeader.Flags | JacobianFlags.Disabled;
                }

                // Check if NoTorque modifier
                if (typeA == ModifyContactJacobians.ModificationType.NoAngularEffects || typeB == ModifyContactJacobians.ModificationType.NoAngularEffects)
                {
                    // Disable all friction angular effects
                    var friction0 = contactJacobian.Friction0;
                    friction0.AngularA        = 0.0f;
                    friction0.AngularB        = 0.0f;
                    contactJacobian.Friction0 = friction0;

                    var friction1 = contactJacobian.Friction1;
                    friction1.AngularA        = 0.0f;
                    friction1.AngularB        = 0.0f;
                    contactJacobian.Friction1 = friction1;

                    var angularFriction = contactJacobian.AngularFriction;
                    angularFriction.AngularA        = 0.0f;
                    angularFriction.AngularB        = 0.0f;
                    contactJacobian.AngularFriction = angularFriction;
                }

                // Check if SurfaceVelocity present
                if (jacHeader.HasSurfaceVelocity &&
                    (typeA == ModifyContactJacobians.ModificationType.SurfaceVelocity || typeB == ModifyContactJacobians.ModificationType.SurfaceVelocity))
                {
                    // Since surface normal can change, make sure angular velocity is always relative to it, not independent
                    float3 angVel = contactJacobian.Normal * (new float3(0.0f, 1.0f, 0.0f));
                    float3 linVel = float3.zero;

                    Math.CalculatePerpendicularNormalized(contactJacobian.Normal, out float3 dir0, out float3 dir1);
                    float linVel0 = math.dot(linVel, dir0);
                    float linVel1 = math.dot(linVel, dir1);

                    float angVelProj = math.dot(angVel, contactJacobian.Normal);

                    //jacHeader.SurfaceVelocity = new SurfaceVelocity { ExtraFrictionDv = new float3(linVel0, linVel1, angVelProj) };
                }

                /*
                 * // Check if MaxImpulse present
                 * if (jacHeader.HasMaxImpulse &&
                 *  (typeA == ModifyContactJacobians.ModificationType.ClippedImpulse || typeB == ModifyContactJacobians.ModificationType.ClippedImpulse))
                 * {
                 *  // Max impulse in Ns (Newton-second)
                 *  jacHeader.MaxImpulse = 20.0f;
                 * }
                 */

                // Check if MassFactors present
                if (jacHeader.HasMassFactors &&
                    (typeA == ModifyContactJacobians.ModificationType.InfiniteInertia || typeB == ModifyContactJacobians.ModificationType.InfiniteInertia))
                {
                    // Give both bodies infinite inertia
                    jacHeader.MassFactors = new MassFactors
                    {
                        InvInertiaAndMassFactorA = new float4(0.0f, 0.0f, 0.0f, 1.0f),
                        InvInertiaAndMassFactorB = new float4(0.0f, 0.0f, 0.0f, 1.0f)
                    };
                }
            }

            // Angular jacobian modifications
            for (int i = 0; i < contactJacobian.NumContacts; i++)
            {
                ContactJacAngAndVelToReachCp jacobianAngular = jacHeader.GetAngularJacobian(i);

                // Check if NoTorque modifier
                if (typeA == ModifyContactJacobians.ModificationType.NoAngularEffects || typeB == ModifyContactJacobians.ModificationType.NoAngularEffects)
                {
                    // Disable all angular effects
                    jacobianAngular.Jac.AngularA = 0.0f;
                    jacobianAngular.Jac.AngularB = 0.0f;
                }

                // Check if SoftContact modifier
                if (typeA == ModifyContactJacobians.ModificationType.SoftContact || typeB == ModifyContactJacobians.ModificationType.SoftContact)
                {
                    jacobianAngular.Jac.EffectiveMass *= 0.1f;
                    if (jacobianAngular.VelToReachCp > 0.0f)
                    {
                        jacobianAngular.VelToReachCp *= 0.5f;
                    }
                }

                jacHeader.SetAngularJacobian(i, jacobianAngular);
            }
        }