Ejemplo n.º 1
0
    private void Solve(ref Weight weight, ref double dt)
    {
        Vector3G vectorg;

        vectorg.x = (dt * this.groundLocalVelocity.x) * this.cfg.forceSpeedMultiplier.x;
        vectorg.y = (dt * this.groundLocalVelocity.y) * this.cfg.forceSpeedMultiplier.y;
        vectorg.z = (dt * this.groundLocalVelocity.z) * this.cfg.forceSpeedMultiplier.z;
        Vector3G fE       = weight.position.fE;
        Vector3G vectorg3 = weight.rotation.fE;

        weight.position.fE = new Vector3G();
        weight.rotation.fE = new Vector3G();
        if (this.anyAdditionalCurves)
        {
            for (int i = 0; i < this.additionalCurveCount; i++)
            {
                double        groundLocalVelocityMag;
                BobForceCurve curve = this.cfg.additionalCurves[i];
                switch (curve.source)
                {
                case BobForceCurveSource.LocalMovementMagnitude:
                    groundLocalVelocityMag = this.groundLocalVelocityMag;
                    break;

                case BobForceCurveSource.LocalMovementX:
                    groundLocalVelocityMag = this.groundLocalVelocity.x;
                    break;

                case BobForceCurveSource.LocalMovementY:
                    groundLocalVelocityMag = this.groundLocalVelocity.y;
                    break;

                case BobForceCurveSource.LocalMovementZ:
                    groundLocalVelocityMag = this.groundLocalVelocity.z;
                    break;

                case BobForceCurveSource.WorldMovementMagnitude:
                    groundLocalVelocityMag = this.groundWorldVelocityMag;
                    break;

                case BobForceCurveSource.WorldMovementX:
                    groundLocalVelocityMag = this.groundWorldVelocity.x;
                    break;

                case BobForceCurveSource.WorldMovementY:
                    groundLocalVelocityMag = this.groundWorldVelocity.y;
                    break;

                case BobForceCurveSource.WorldMovementZ:
                    groundLocalVelocityMag = this.groundWorldVelocity.z;
                    break;

                case BobForceCurveSource.LocalVelocityMagnitude:
                    groundLocalVelocityMag = this.localVelocityMag;
                    break;

                case BobForceCurveSource.LocalVelocityX:
                    groundLocalVelocityMag = this.localVelocity.x;
                    break;

                case BobForceCurveSource.LocalVelocityY:
                    groundLocalVelocityMag = this.localVelocity.y;
                    break;

                case BobForceCurveSource.WorldVelocityMagnitude:
                    groundLocalVelocityMag = this.worldVelocityMag;
                    break;

                case BobForceCurveSource.WorldVelocityX:
                    groundLocalVelocityMag = this.worldVelocity.x;
                    break;

                case BobForceCurveSource.WorldVelocityY:
                    groundLocalVelocityMag = this.worldVelocity.y;
                    break;

                case BobForceCurveSource.WorldVelocityZ:
                    groundLocalVelocityMag = this.worldVelocity.z;
                    break;

                case BobForceCurveSource.RotationMagnitude:
                    groundLocalVelocityMag = this.localAngularVelocityMag;
                    break;

                case BobForceCurveSource.RotationPitch:
                    groundLocalVelocityMag = this.localAngularVelocity.x;
                    break;

                case BobForceCurveSource.RotationYaw:
                    groundLocalVelocityMag = this.localAngularVelocity.y;
                    break;

                case BobForceCurveSource.RotationRoll:
                    groundLocalVelocityMag = this.localAngularVelocity.z;
                    break;

                case BobForceCurveSource.TurnMagnitude:
                    groundLocalVelocityMag = this.groundLocalAngularVelocityMag;
                    break;

                case BobForceCurveSource.TurnPitch:
                    groundLocalVelocityMag = this.groundLocalAngularVelocity.x;
                    break;

                case BobForceCurveSource.TurnYaw:
                    groundLocalVelocityMag = this.groundLocalAngularVelocity.y;
                    break;

                case BobForceCurveSource.TurnRoll:
                    groundLocalVelocityMag = this.groundLocalAngularVelocity.z;
                    break;

                default:
                    groundLocalVelocityMag = this.localVelocity.z;
                    break;
                }
                BobForceCurveTarget target = curve.target;
                if ((target == BobForceCurveTarget.Position) || (target != BobForceCurveTarget.Rotation))
                {
                    curve.Calculate(ref weight.additionalPositions[i], ref groundLocalVelocityMag, ref dt, ref weight.position.fE);
                }
                else
                {
                    curve.Calculate(ref weight.additionalPositions[i], ref groundLocalVelocityMag, ref dt, ref weight.rotation.fE);
                }
            }
        }
        if (this.cfg.impulseForceSmooth > 0f)
        {
            Vector3G.SmoothDamp(ref weight.position.fI, ref this.impulseForce.accel, ref weight.position.fIV, this.cfg.impulseForceSmooth, this.cfg.impulseForceMaxChangeAcceleration, ref dt);
        }
        else
        {
            weight.position.fI = this.impulseForce.accel;
        }
        if (this.cfg.angleImpulseForceSmooth > 0f)
        {
            Vector3G.SmoothDamp(ref weight.rotation.fI, ref this.impulseTorque.accel, ref weight.rotation.fIV, this.cfg.angleImpulseForceSmooth, this.cfg.angleImpulseForceMaxChangeAcceleration, ref dt);
        }
        else
        {
            weight.rotation.fI = this.impulseTorque.accel;
        }
        weight.position.fE.x += this.inputForce.x + (weight.position.fI.x * this.cfg.impulseForceScale.x);
        weight.position.fE.y += this.inputForce.y + (weight.position.fI.y * this.cfg.impulseForceScale.y);
        weight.position.fE.z += this.inputForce.z + (weight.position.fI.z * this.cfg.impulseForceScale.z);
        weight.rotation.fE.x += weight.rotation.fI.x * this.cfg.angularImpulseForceScale.x;
        weight.rotation.fE.y += weight.rotation.fI.y * this.cfg.angularImpulseForceScale.y;
        weight.rotation.fE.z += weight.rotation.fI.z * this.cfg.angularImpulseForceScale.z;
        Vector3G vectorg4 = weight.position.value;

        vectorg4.x /= (double)this.cfg.elipsoidRadii.x;
        vectorg4.y /= (double)this.cfg.elipsoidRadii.y;
        vectorg4.z /= (double)this.cfg.elipsoidRadii.z;
        double d = ((vectorg4.x * vectorg4.x) + (vectorg4.y * vectorg4.y)) + (vectorg4.z * vectorg4.z);

        if (d > 1.0)
        {
            d           = 1.0 / Math.Sqrt(d);
            vectorg4.x *= d;
            vectorg4.y *= d;
            vectorg4.z *= d;
        }
        vectorg4.x *= this.cfg.elipsoidRadii.x;
        vectorg4.y *= this.cfg.elipsoidRadii.y;
        vectorg4.z *= this.cfg.elipsoidRadii.z;
        weight.stack.Simulate(ref dt, ref weight.position.fE, ref weight.rotation.fE);
        weight.position.acceleration.x = (weight.position.fE.x - fE.x) + (((vectorg4.x * -this.cfg.springConstant.x) - (weight.position.velocity.x * this.cfg.springDampen.x)) * this.cfg.weightMass);
        weight.position.acceleration.y = (weight.position.fE.y - fE.y) + (((vectorg4.y * -this.cfg.springConstant.y) - (weight.position.velocity.y * this.cfg.springDampen.y)) * this.cfg.weightMass);
        weight.position.acceleration.z = (weight.position.fE.z - fE.z) + (((vectorg4.z * -this.cfg.springConstant.z) - (weight.position.velocity.z * this.cfg.springDampen.z)) * this.cfg.weightMass);
        weight.position.velocity.x    += weight.position.acceleration.x * dt;
        weight.position.velocity.y    += weight.position.acceleration.y * dt;
        weight.position.velocity.z    += weight.position.acceleration.z * dt;
        if (!float.IsInfinity(this.cfg.maxVelocity.x))
        {
            if (weight.position.velocity.x < -this.cfg.maxVelocity.x)
            {
                weight.position.value.x -= this.cfg.maxVelocity.x * dt;
            }
            else if (weight.position.velocity.x > this.cfg.maxVelocity.x)
            {
                weight.position.value.x += this.cfg.maxVelocity.x * dt;
            }
            else
            {
                weight.position.value.x += weight.position.velocity.x * dt;
            }
        }
        else
        {
            weight.position.value.x += weight.position.velocity.x * dt;
        }
        if (!float.IsInfinity(this.cfg.maxVelocity.y))
        {
            if (weight.position.velocity.y < -this.cfg.maxVelocity.y)
            {
                weight.position.value.y -= this.cfg.maxVelocity.y * dt;
            }
            else if (weight.position.velocity.y > this.cfg.maxVelocity.y)
            {
                weight.position.value.y += this.cfg.maxVelocity.y * dt;
            }
            else
            {
                weight.position.value.y += weight.position.velocity.y * dt;
            }
        }
        else
        {
            weight.position.value.y += weight.position.velocity.y * dt;
        }
        if (!float.IsInfinity(this.cfg.maxVelocity.z))
        {
            if (weight.position.velocity.z < -this.cfg.maxVelocity.z)
            {
                weight.position.value.z -= this.cfg.maxVelocity.z * dt;
            }
            else if (weight.position.velocity.z > this.cfg.maxVelocity.z)
            {
                weight.position.value.z += this.cfg.maxVelocity.z * dt;
            }
            else
            {
                weight.position.value.z += weight.position.velocity.z * dt;
            }
        }
        else
        {
            weight.position.value.z += weight.position.velocity.z * dt;
        }
        weight.rotation.acceleration.x = (weight.rotation.fE.x - vectorg3.x) + (((weight.rotation.value.x * -this.cfg.angularSpringConstant.x) - (weight.rotation.velocity.x * this.cfg.angularSpringDampen.x)) * this.cfg.angularWeightMass);
        weight.rotation.acceleration.y = (weight.rotation.fE.y - vectorg3.y) + (((weight.rotation.value.y * -this.cfg.angularSpringConstant.y) - (weight.rotation.velocity.y * this.cfg.angularSpringDampen.y)) * this.cfg.angularWeightMass);
        weight.rotation.acceleration.z = (weight.rotation.fE.z - vectorg3.z) + (((weight.rotation.value.z * -this.cfg.angularSpringConstant.z) - (weight.rotation.velocity.z * this.cfg.angularSpringDampen.z)) * this.cfg.angularWeightMass);
        weight.rotation.velocity.x    += weight.rotation.acceleration.x * dt;
        weight.rotation.velocity.y    += weight.rotation.acceleration.y * dt;
        weight.rotation.velocity.z    += weight.rotation.acceleration.z * dt;
        weight.rotation.value.x       += weight.rotation.velocity.x * dt;
        weight.rotation.value.y       += weight.rotation.velocity.y * dt;
        weight.rotation.value.z       += weight.rotation.velocity.z * dt;
    }
Ejemplo n.º 2
0
    private void Solve(ref HeadBob.Weight weight, ref double dt)
    {
        Vector3G vector3G = new Vector3G();
        double   num;

        vector3G.x = dt * this.groundLocalVelocity.x * (double)this.cfg.forceSpeedMultiplier.x;
        vector3G.y = dt * this.groundLocalVelocity.y * (double)this.cfg.forceSpeedMultiplier.y;
        vector3G.z = dt * this.groundLocalVelocity.z * (double)this.cfg.forceSpeedMultiplier.z;
        Vector3G vector3G1 = weight.position.fE;
        Vector3G vector3G2 = weight.rotation.fE;
        Vector3G vector3G3 = new Vector3G();

        weight.position.fE = vector3G3;
        Vector3G vector3G4 = new Vector3G();

        weight.rotation.fE = vector3G4;
        if (this.anyAdditionalCurves)
        {
            for (int i = 0; i < this.additionalCurveCount; i++)
            {
                BobForceCurve bobForceCurve = this.cfg.additionalCurves[i];
                switch (bobForceCurve.source)
                {
                case BobForceCurveSource.LocalMovementMagnitude:
                {
                    num = this.groundLocalVelocityMag;
                    break;
                }

                case BobForceCurveSource.LocalMovementX:
                {
                    num = this.groundLocalVelocity.x;
                    break;
                }

                case BobForceCurveSource.LocalMovementY:
                {
                    num = this.groundLocalVelocity.y;
                    break;
                }

                case BobForceCurveSource.LocalMovementZ:
                {
                    num = this.groundLocalVelocity.z;
                    break;
                }

                case BobForceCurveSource.WorldMovementMagnitude:
                {
                    num = this.groundWorldVelocityMag;
                    break;
                }

                case BobForceCurveSource.WorldMovementX:
                {
                    num = this.groundWorldVelocity.x;
                    break;
                }

                case BobForceCurveSource.WorldMovementY:
                {
                    num = this.groundWorldVelocity.y;
                    break;
                }

                case BobForceCurveSource.WorldMovementZ:
                {
                    num = this.groundWorldVelocity.z;
                    break;
                }

                case BobForceCurveSource.LocalVelocityMagnitude:
                {
                    num = this.localVelocityMag;
                    break;
                }

                case BobForceCurveSource.LocalVelocityX:
                {
                    num = this.localVelocity.x;
                    break;
                }

                case BobForceCurveSource.LocalVelocityY:
                {
                    num = this.localVelocity.y;
                    break;
                }

                case BobForceCurveSource.LocalVelocityZ:
                {
                    num = this.localVelocity.z;
                    break;
                }

                case BobForceCurveSource.WorldVelocityMagnitude:
                {
                    num = this.worldVelocityMag;
                    break;
                }

                case BobForceCurveSource.WorldVelocityX:
                {
                    num = this.worldVelocity.x;
                    break;
                }

                case BobForceCurveSource.WorldVelocityY:
                {
                    num = this.worldVelocity.y;
                    break;
                }

                case BobForceCurveSource.WorldVelocityZ:
                {
                    num = this.worldVelocity.z;
                    break;
                }

                case BobForceCurveSource.RotationMagnitude:
                {
                    num = this.localAngularVelocityMag;
                    break;
                }

                case BobForceCurveSource.RotationPitch:
                {
                    num = this.localAngularVelocity.x;
                    break;
                }

                case BobForceCurveSource.RotationYaw:
                {
                    num = this.localAngularVelocity.y;
                    break;
                }

                case BobForceCurveSource.RotationRoll:
                {
                    num = this.localAngularVelocity.z;
                    break;
                }

                case BobForceCurveSource.TurnMagnitude:
                {
                    num = this.groundLocalAngularVelocityMag;
                    break;
                }

                case BobForceCurveSource.TurnPitch:
                {
                    num = this.groundLocalAngularVelocity.x;
                    break;
                }

                case BobForceCurveSource.TurnYaw:
                {
                    num = this.groundLocalAngularVelocity.y;
                    break;
                }

                case BobForceCurveSource.TurnRoll:
                {
                    num = this.groundLocalAngularVelocity.z;
                    break;
                }

                default:
                {
                    goto case BobForceCurveSource.LocalVelocityZ;
                }
                }
                BobForceCurveTarget bobForceCurveTarget = bobForceCurve.target;
                if (bobForceCurveTarget != BobForceCurveTarget.Position)
                {
                    if (bobForceCurveTarget != BobForceCurveTarget.Rotation)
                    {
                        goto Label3;
                    }
                    bobForceCurve.Calculate(ref weight.additionalPositions[i], ref num, ref dt, ref weight.rotation.fE);
                    goto Label1;
                }
Label3:
                bobForceCurve.Calculate(ref weight.additionalPositions[i], ref num, ref dt, ref weight.position.fE);
Label1:
            }
        }
        if (this.cfg.impulseForceSmooth <= 0f)
        {
            weight.position.fI = this.impulseForce.accel;
        }
        else
        {
            Vector3G.SmoothDamp(ref weight.position.fI, ref this.impulseForce.accel, ref weight.position.fIV, this.cfg.impulseForceSmooth, this.cfg.impulseForceMaxChangeAcceleration, ref dt);
        }
        if (this.cfg.angleImpulseForceSmooth <= 0f)
        {
            weight.rotation.fI = this.impulseTorque.accel;
        }
        else
        {
            Vector3G.SmoothDamp(ref weight.rotation.fI, ref this.impulseTorque.accel, ref weight.rotation.fIV, this.cfg.angleImpulseForceSmooth, this.cfg.angleImpulseForceMaxChangeAcceleration, ref dt);
        }
        weight.position.fE.x = weight.position.fE.x + (this.inputForce.x + weight.position.fI.x * (double)this.cfg.impulseForceScale.x);
        weight.position.fE.y = weight.position.fE.y + (this.inputForce.y + weight.position.fI.y * (double)this.cfg.impulseForceScale.y);
        weight.position.fE.z = weight.position.fE.z + (this.inputForce.z + weight.position.fI.z * (double)this.cfg.impulseForceScale.z);
        weight.rotation.fE.x = weight.rotation.fE.x + weight.rotation.fI.x * (double)this.cfg.angularImpulseForceScale.x;
        weight.rotation.fE.y = weight.rotation.fE.y + weight.rotation.fI.y * (double)this.cfg.angularImpulseForceScale.y;
        weight.rotation.fE.z = weight.rotation.fE.z + weight.rotation.fI.z * (double)this.cfg.angularImpulseForceScale.z;
        Vector3G vector3G5 = weight.position.@value;

        vector3G5.x = vector3G5.x / (double)this.cfg.elipsoidRadii.x;
        vector3G5.y = vector3G5.y / (double)this.cfg.elipsoidRadii.y;
        vector3G5.z = vector3G5.z / (double)this.cfg.elipsoidRadii.z;
        double num1 = vector3G5.x * vector3G5.x + vector3G5.y * vector3G5.y + vector3G5.z * vector3G5.z;

        if (num1 > 1)
        {
            num1        = 1 / Math.Sqrt(num1);
            vector3G5.x = vector3G5.x * num1;
            vector3G5.y = vector3G5.y * num1;
            vector3G5.z = vector3G5.z * num1;
        }
        vector3G5.x = vector3G5.x * (double)this.cfg.elipsoidRadii.x;
        vector3G5.y = vector3G5.y * (double)this.cfg.elipsoidRadii.y;
        vector3G5.z = vector3G5.z * (double)this.cfg.elipsoidRadii.z;
        weight.stack.Simulate(ref dt, ref weight.position.fE, ref weight.rotation.fE);
        weight.position.acceleration.x = weight.position.fE.x - vector3G1.x + (vector3G5.x * (double)(-this.cfg.springConstant.x) - weight.position.velocity.x * (double)this.cfg.springDampen.x) * (double)this.cfg.weightMass;
        weight.position.acceleration.y = weight.position.fE.y - vector3G1.y + (vector3G5.y * (double)(-this.cfg.springConstant.y) - weight.position.velocity.y * (double)this.cfg.springDampen.y) * (double)this.cfg.weightMass;
        weight.position.acceleration.z = weight.position.fE.z - vector3G1.z + (vector3G5.z * (double)(-this.cfg.springConstant.z) - weight.position.velocity.z * (double)this.cfg.springDampen.z) * (double)this.cfg.weightMass;
        weight.position.velocity.x     = weight.position.velocity.x + weight.position.acceleration.x * dt;
        weight.position.velocity.y     = weight.position.velocity.y + weight.position.acceleration.y * dt;
        weight.position.velocity.z     = weight.position.velocity.z + weight.position.acceleration.z * dt;
        if (float.IsInfinity(this.cfg.maxVelocity.x))
        {
            [email protected] = [email protected] + weight.position.velocity.x * dt;
        }
        else if (weight.position.velocity.x < (double)(-this.cfg.maxVelocity.x))
        {
            [email protected] = [email protected] - (double)this.cfg.maxVelocity.x * dt;
        }
        else if (weight.position.velocity.x <= (double)this.cfg.maxVelocity.x)
        {
            [email protected] = [email protected] + weight.position.velocity.x * dt;
        }
        else
        {
            [email protected] = [email protected] + (double)this.cfg.maxVelocity.x * dt;
        }
        if (float.IsInfinity(this.cfg.maxVelocity.y))
        {
            [email protected] = [email protected] + weight.position.velocity.y * dt;
        }
        else if (weight.position.velocity.y < (double)(-this.cfg.maxVelocity.y))
        {
            [email protected] = [email protected] - (double)this.cfg.maxVelocity.y * dt;
        }
        else if (weight.position.velocity.y <= (double)this.cfg.maxVelocity.y)
        {
            [email protected] = [email protected] + weight.position.velocity.y * dt;
        }
        else
        {
            [email protected] = [email protected] + (double)this.cfg.maxVelocity.y * dt;
        }
        if (float.IsInfinity(this.cfg.maxVelocity.z))
        {
            [email protected] = [email protected] + weight.position.velocity.z * dt;
        }
        else if (weight.position.velocity.z < (double)(-this.cfg.maxVelocity.z))
        {
            [email protected] = [email protected] - (double)this.cfg.maxVelocity.z * dt;
        }
        else if (weight.position.velocity.z <= (double)this.cfg.maxVelocity.z)
        {
            [email protected] = [email protected] + weight.position.velocity.z * dt;
        }
        else
        {
            [email protected] = [email protected] + (double)this.cfg.maxVelocity.z * dt;
        }
        weight.rotation.acceleration.x = weight.rotation.fE.x - vector3G2.x + ([email protected] * (double)(-this.cfg.angularSpringConstant.x) - weight.rotation.velocity.x * (double)this.cfg.angularSpringDampen.x) * (double)this.cfg.angularWeightMass;
        weight.rotation.acceleration.y = weight.rotation.fE.y - vector3G2.y + ([email protected] * (double)(-this.cfg.angularSpringConstant.y) - weight.rotation.velocity.y * (double)this.cfg.angularSpringDampen.y) * (double)this.cfg.angularWeightMass;
        weight.rotation.acceleration.z = weight.rotation.fE.z - vector3G2.z + ([email protected] * (double)(-this.cfg.angularSpringConstant.z) - weight.rotation.velocity.z * (double)this.cfg.angularSpringDampen.z) * (double)this.cfg.angularWeightMass;
        weight.rotation.velocity.x     = weight.rotation.velocity.x + weight.rotation.acceleration.x * dt;
        weight.rotation.velocity.y     = weight.rotation.velocity.y + weight.rotation.acceleration.y * dt;
        weight.rotation.velocity.z     = weight.rotation.velocity.z + weight.rotation.acceleration.z * dt;
        [email protected]       = [email protected] + weight.rotation.velocity.x * dt;
        [email protected]       = [email protected] + weight.rotation.velocity.y * dt;
        [email protected]       = [email protected] + weight.rotation.velocity.z * dt;
    }