C# Vector3.AddScaled方法代码示例

        /// <summary>
        /// Computes the volume distribution and center of the shape.
        /// </summary>
        /// <param name="entries">Mass-weighted entries of the compound.</param>
        /// <param name="volume">Summed volume of the constituent shapes. Intersecting volumes get double counted.</param>
        /// <param name="volumeDistribution">Volume distribution of the shape.</param>
        /// <param name="center">Center of the compound.</param>
        public static void ComputeVolumeDistribution(IList<CompoundShapeEntry> entries, out float volume, out Matrix3x3 volumeDistribution, out Vector3 center)
        {
            center = new Vector3();
            float totalWeight = 0;
            volume = 0;
            for (int i = 0; i < entries.Count; i++)
            {
				RigidTransform r = entries[i].LocalTransform;
                center.AddScaled( ref r.Position, entries[i].Weight, out center );
                volume += entries[i].Shape.Volume;
                totalWeight += entries[i].Weight;
            }
            if (totalWeight <= 0)
                throw new NotFiniteNumberException("Cannot compute distribution; the total weight of a compound shape must be positive.");
            float totalWeightInverse = 1 / totalWeight;
            totalWeightInverse.Validate();
            center.Mult( totalWeightInverse, out center );

            volumeDistribution = new Matrix3x3();
            for (int i = 0; i < entries.Count; i++)
            {
                RigidTransform transform = entries[i].LocalTransform;
                Matrix3x3 contribution;
                TransformContribution(ref transform, ref center, ref entries[i].Shape.volumeDistribution, entries[i].Weight, out contribution);
                Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution);
            }
            Matrix3x3.Multiply(ref volumeDistribution, totalWeightInverse, out volumeDistribution);
            volumeDistribution.Validate();
        }

        CharacterContactPositionState TryUpStepPosition(ref Vector3 sideNormal, ref Vector3 position, ref Vector3 down,
            ref QuickList<CharacterContact> tractionContacts, ref QuickList<CharacterContact> supportContacts, ref QuickList<CharacterContact> sideContacts, ref QuickList<CharacterContact> headContacts,
            out float hintOffset)
        {
            hintOffset = 0;
            PrepareQueryObject(ref position);
            QueryManager.QueryContacts(currentQueryObject, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts);
            if (headContacts.Count > 0)
            {
                //The head is obstructed.  This will define a maximum bound.
                //Find the deepest contact on the head and use it to provide a hint.
                float dot;
                Vector3.Dot(ref down, ref headContacts.Elements[0].Contact.Normal, out dot);
                hintOffset = -dot * headContacts.Elements[0].Contact.PenetrationDepth;
                for (int i = 1; i < headContacts.Count; i++)
                {
                    Vector3.Dot(ref down, ref headContacts.Elements[i].Contact.Normal, out dot);
                    dot *= -headContacts.Elements[i].Contact.PenetrationDepth;
                    if (dot > hintOffset)
                    {
                        hintOffset = dot;
                    }
                }
                return CharacterContactPositionState.HeadObstructed;
            }
            bool obstructed = IsUpStepObstructedBySideContacts(ref sideNormal, ref sideContacts);
            if (!obstructed && supportContacts.Count > 0)
            {
                CharacterContactPositionState supportState;
                CharacterContact supportContact;
                QueryManager.AnalyzeSupportState(ref tractionContacts, ref supportContacts, out supportState, out supportContact);
                if (supportState == CharacterContactPositionState.Accepted)
                {
                    if (tractionContacts.Count > 0)
                    {
                        //We're done! The guess found a good spot to stand on.
                        //Unlike down stepping, upstepping DOES need good contacts in the final state.
                        //Push it up if necessary, but don't push it too far.
                        //Putting it into the middle of the allowed penetration makes it very likely that it will properly generate contacts.
                        //Choosing something smaller than allowed penetration ensures that the search makes meaningful progress forward when the sizes get really tiny;
                        //we wouldn't want it edging every closer to AllowedPenetration and then exit because too many queries were made.
                        hintOffset = Math.Min(0, Vector3.Dot(ref supportContact.Contact.Normal, ref down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
                        return CharacterContactPositionState.Accepted;
                    }
                    else
                    {
                        //No traction... Before we give up and reject the step altogether, let's try one last thing.  It's possible that the character is trying to step up onto the side of a ramp or something.
                        //In this scenario, the top-down ray cast detects a perfectly walkable slope.  However, the contact queries will find a contact with a normal necessarily
                        //steeper than the one found by the ray cast because it is an edge contact.  Not being able to step up in this scenario doesn't feel natural to the player
                        //even if it is technically consistent.

                        //So, let's try to ray cast down to the a point just barely beyond the contact (to ensure we don't land right on the edge, which would invite numerical issues).
                        //Note that this is NOT equivalent to the ray cast we performed earlier to test for an initial step height and surface normal.
                        //This one is based on the QUERY state and the QUERY's contact position.

                        //Find the down test ray's position.
                        Ray downRay;
                        supportContact.Contact.Position.AddScaled( ref sideNormal, .001f, out downRay.Position );
						Vector3 tmp;
						downRay.Position.Sub( ref position, out tmp );
                        float verticalOffset = Vector3.Dot( ref tmp, ref down);
                        verticalOffset = characterBody.Height * .5f + verticalOffset;
                        downRay.Position.AddScaled( ref down,  -verticalOffset, out downRay.Position );
                        downRay.Direction = down;

                        //First, we must ensure that the ray cast test origin is not obstructed.  Starting very close to the very top of the character is safe because the process has already validated
                        //this location as accepted, just without traction.
                        Ray obstructionTestRay;
                        position.AddScaled( ref down, -(characterBody.Height * .5f), out obstructionTestRay.Position );
                        downRay.Position.Sub( ref obstructionTestRay.Position, out obstructionTestRay.Direction );

                        if (!QueryManager.RayCastHitAnything(ref obstructionTestRay, 1))
                        {
                            //Okay! it's safe to cast down, then.
                            RayHit hit;
                            if (QueryManager.RayCast(downRay, characterBody.Height, out hit))
                            {
                                //Got a hit!
                                if (characterBody.Height - maximumStepHeight < hit.T)
                                {
                                    //It's in range!                   
                                    float dot;
                                    hit.Normal.Normalize();
                                    Vector3.Dot(ref hit.Normal, ref down, out dot);
                                    if (Math.Abs(dot) > ContactCategorizer.TractionThreshold)
                                    {
                                        //Slope is shallow enough to stand on!
                                        hintOffset = Math.Min(0, Vector3.Dot(ref supportContact.Contact.Normal, ref down) * (CollisionDetectionSettings.AllowedPenetration * .5f - supportContact.Contact.PenetrationDepth));
                                        //ONE MORE thing to check.  The new position of the center ray must be able to touch the ground!
                                        downRay.Position = position;
                                        if (QueryManager.RayCast(downRay, characterBody.Height * .5f + maximumStepHeight, out hit))
                                        {
                                            //It hit.. almost there!
                                            hit.Normal.Normalize();
                                            Vector3.Dot(ref hit.Normal, ref down, out dot);
                                            if (Math.Abs(dot) > ContactCategorizer.TractionThreshold)
                                            {
                                                //It has traction! We can step!
                                                return CharacterContactPositionState.Accepted;
                                            }
//.........这里部分代码省略.........

        /// <summary>
        /// Updates the movement basis of the horizontal motion constraint.
        /// Should be updated automatically by the character on each time step; other code should not need to call this.
        /// </summary>
        /// <param name="forward">Forward facing direction of the character.</param>
        public void UpdateMovementBasis(ref Vector3 forward)
        {
            Vector3 down;// = characterBody.orientationMatrix.Down;
            characterBody.orientationMatrix.M2.Invert(out down);
            forward.AddScaled(ref down, -Vector3.Dot(ref down, ref forward), out horizontalForwardDirection);
            float forwardLengthSquared = horizontalForwardDirection.LengthSquared();

            if (forwardLengthSquared < Toolbox.Epsilon)
            {
                //Use an arbitrary direction to complete the basis.
                horizontalForwardDirection = characterBody.orientationMatrix.Forward;
                strafeDirection = characterBody.orientationMatrix.Right;
            }
            else
            {
                Vector3.Divide(ref horizontalForwardDirection, (float)Math.Sqrt(forwardLengthSquared), out horizontalForwardDirection);
                Vector3.Cross(ref down, ref horizontalForwardDirection, out strafeDirection);
                //Don't need to normalize the strafe direction; it's the cross product of two normalized perpendicular vectors.
            }


            Vector3.Multiply(ref horizontalForwardDirection, movementDirection.Y, out movementDirection3d);
            Vector3 strafeComponent;
            Vector3.Multiply(ref strafeDirection, movementDirection.X, out strafeComponent);
            Vector3.Add(ref strafeComponent, ref movementDirection3d, out movementDirection3d);

        }