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;
}
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;
}
