GEEKING

App/humanoid/Swimming.cpp

@@ -0,0 +1,146 @@
+/* Copyright 2003-2005 ROBLOX Corporation, All Rights Reserved */
+#include "stdafx.h"
+
+#include "Humanoid/Swimming.h"
+#include "Humanoid/Humanoid.h"
+#include "V8Kernel/Body.h"
+#include "V8Kernel/Constants.h"
+#include "V8DataModel/GameBasicSettings.h"
+
+namespace RBX {
+
+	namespace HUMAN {
+
+		float Swimming::velocityDecay() {return 0.05f;}
+
+		const char* const sSwimming = "Swimming";
+
+		Swimming::Swimming(Humanoid* humanoid, StateType priorState)
+			:Named<HumanoidState, sSwimming>(humanoid, priorState)
+		{
+			humanoid->swimmingSignal( 0.0f );
+		}
+
+		void Swimming::onComputeForceImpl()
+		{
+			float pPitch = 7500.0f;
+			float pRoll = 1000.0f;
+			float kD = 50.0f;
+
+			// Now move
+			Body* torsoBody = getHumanoid()->getTorsoBodyFast();
+			if (!torsoBody)
+				return;
+
+			Body* root = getHumanoid()->getRootBodyFast();
+			if (!root)
+				return;
+
+			Vector3 desiredVelocityInWorld = desiredVelocity.linear;
+			float desiredVelocityMagnitute = desiredVelocityInWorld.magnitude();
+			const Vector3 horizontalVelocity(desiredVelocityInWorld.x, 0.0f, desiredVelocityInWorld.z);
+			if ( desiredVelocityInWorld.y * 2.0f < horizontalVelocity.magnitude() &&
+				 desiredVelocityInWorld.y * 2.0f > -horizontalVelocity.magnitude() )	// Within +/- 26.5 degree
+			{
+				desiredVelocityInWorld = horizontalVelocity.direction() * desiredVelocityMagnitute;
+			}
+
+			const Vector3& currentVelocityInWorld = root->getBranchVelocity().linear;	// velocity at COFM of the assembly
+
+			// Move forward-backward (and up)
+			{
+				if( desiredVelocityMagnitute > 10.0f )
+				{
+					Vector3 desiredAccel = 35.0f * root->getBranchMass() * (desiredVelocityInWorld - currentVelocityInWorld);				
+					Vector3 deltaForce = G3D::clamp( desiredAccel, minSwimmingMoveForce(), maxSwimmingMoveForce()	);
+					root->accumulateForceAtBranchCofm(deltaForce);
+				}
+			}
+
+			// control torque for facing direction
+			{
+				// axis to balance for character pitching
+				Vector3 pitchBalanceAxis = torsoBody->getCoordinateFrame().rotation.column(1);
+				//Vector3 rollBalanceAxis = torsoBody->getCoordinateFrame().rotation.column(0);
+				Vector3 pitchTilt;
+				//Vector3 rollTilt = Vector3::unitX().cross( rollBalanceAxis );
+				Vector3 angVelWorld = torsoBody->getVelocity().rotational;
+				Vector3 externalTorqueWorld = root->getBranchTorque();
+
+				float speedSquared = desiredVelocityInWorld.squaredMagnitude();
+
+				if( speedSquared < 5.0f )
+				{
+					// if desired velocity is small, balance towards the y-axis
+					pPitch = 500.0f;
+					pitchTilt = Vector3::unitY().cross( pitchBalanceAxis );	
+					pRoll = 0.0f;
+				}
+				else
+				{
+					pPitch = 2000.0f;
+					// if there's a desired velocity, balance towards the direction of the velocity
+					pitchTilt = desiredVelocityInWorld.unit().cross( pitchBalanceAxis );					
+				}
+				const CoordinateFrame& rootCoord = root->getCoordinateFrame();
+
+				Vector3 angVelRoot = rootCoord.vectorToObjectSpace(angVelWorld);
+				RBXASSERT(!Math::isNanInfVector3(angVelRoot));
+
+				// P control component
+				Vector3 controlTorqueRoot = -pPitch * (root->getBranchIBody() *  rootCoord.vectorToObjectSpace( pitchTilt ) );  // apply scalar to Vector3, not Matrix3				
+				
+				// D control component
+				controlTorqueRoot -= kD * ( root->getBranchIBodyV3() * angVelRoot );
+
+				// existing torque component
+				controlTorqueRoot += rootCoord.vectorToObjectSpace( externalTorqueWorld );
+								
+				Vector3 torqueWorld = rootCoord.vectorToWorldSpace( controlTorqueRoot );
+				root->accumulateTorque( torqueWorld - externalTorqueWorld );
+
+				//RBX::StandardOut::singleton()->printf( RBX::MESSAGE_WARNING, "Swimming facing vector (%f, %f, %f)\n", controlTorqueRoot.x, controlTorqueRoot.y, controlTorqueRoot.z );
+			}
+		}
+
+		void Swimming::onSimulatorStepImpl(float stepDt)
+		{
+			Velocity desiredWalkVelocity = 	getHumanoid()->calcDesiredWalkVelocity();
+
+			desiredVelocity = desiredWalkVelocity;
+			desiredVelocity.linear += initialLinearVelocity;
+
+			if ((desiredVelocity.linear.magnitude() < 0.1)) {
+				desiredVelocity = Velocity::zero();		
+			}
+#if 0		// Swimming direction is now decoupled from torso facing direction
+			else {
+				if (desiredWalkVelocity.linear.magnitude() > 0.1) {
+					float yawAngle = static_cast<float>(Math::radWrap(Math::getHeading(desiredVelocity.linear) - getHumanoid()->getTorsoHeading()));
+					float pitchAngle = static_cast<float>(Math::radWrap(Math::getElevation(desiredVelocity.linear.direction()) - getHumanoid()->getTorsoElevation()));
+					if(!RBX::GameBasicSettings::singleton().mouseLockedInMouseLockMode())
+					{
+						if (fabs(yawAngle) > 0.2f)
+							desiredVelocity.rotational.y = kTurnSpeed() * Math::polarity(yawAngle);
+						else
+							desiredVelocity.rotational.y = 0.25f * kTurnSpeed() * Math::polarity(yawAngle) * fabs(yawAngle);
+						if (fabs(pitchAngle) > 0.2f)
+							desiredVelocity.rotational.x = kTurnSpeed() * Math::polarity(pitchAngle);
+						else
+							desiredVelocity.rotational.x = 0.25f * kTurnSpeed() * Math::polarity(pitchAngle) * fabs(pitchAngle);
+					}
+				}
+			}
+#endif
+			initialLinearVelocity *= ( 1 - (velocityDecay() * stepDt / (1.0f/30.0f) ) );
+		}
+
+		void Swimming::fireEvents()
+		{
+			Super::fireEvents();
+			
+			fireMovementSignal( getHumanoid()->swimmingSignal, getRelativeMovementVelocity().length() );
+		}
+
+	} // HUMAN
+} // namespace