void CStatueProp::VPhysicsUpdate( IPhysicsObject *pPhysics )
{
	BaseClass::VPhysicsUpdate( pPhysics );

	if ( s_vcollide_wireframe->GetBool() )
	{
		const CPhysCollide *pCollide = pPhysics->GetCollide();

		Vector vecOrigin;
		QAngle angAngles;

		pPhysics->GetPosition( &vecOrigin, &angAngles );

		if ( pCollide )
		{
			Vector *outVerts;
			int vertCount = physcollision->CreateDebugMesh( pCollide, &outVerts );
			int triCount = vertCount / 3;
			int vert = 0;

			VMatrix tmp = SetupMatrixOrgAngles( vecOrigin, angAngles );
			int i;
			for ( i = 0; i < vertCount; i++ )
			{
				outVerts[i] = tmp.VMul4x3( outVerts[i] );
			}

			for ( i = 0; i < triCount; i++ )
			{
				NDebugOverlay::Line( outVerts[ vert + 0 ], outVerts[ vert + 1 ], 0, 255, 255, false, 0.0f );
				NDebugOverlay::Line( outVerts[ vert + 1 ], outVerts[ vert + 2 ], 0, 255, 255, false, 0.0f );
				NDebugOverlay::Line( outVerts[ vert + 2 ], outVerts[ vert + 0 ], 0, 255, 255, false, 0.0f );
				vert += 3;
			}

			physcollision->DestroyDebugMesh( vertCount, outVerts );
		}
	}
}
Ejemplo n.º 2
0
IMotionEvent::simresult_e CGravControllerPoint::Simulate( IPhysicsMotionController *pController, IPhysicsObject *pObject, float deltaTime, Vector &linear, AngularImpulse &angular )
{
	Vector vel;
	AngularImpulse angVel;

	float fracRemainingSimTime = 1.0;
	if ( m_timeToArrive > 0 )
	{
		fracRemainingSimTime *= deltaTime / m_timeToArrive;
		if ( fracRemainingSimTime > 1 )
		{
			fracRemainingSimTime = 1;
		}
	}
	
	m_timeToArrive -= deltaTime;
	if ( m_timeToArrive < 0 )
	{
		m_timeToArrive = 0;
	}

	float invDeltaTime = (1.0f / deltaTime);
	Vector world;
	pObject->LocalToWorld( &world, m_localPosition );
	m_worldPosition = world;
	pObject->GetVelocity( &vel, &angVel );
	//pObject->GetVelocityAtPoint( world, &vel );
	float damping = 1.0;
	world += vel * deltaTime * damping;
	Vector delta = (m_targetPosition - world) * fracRemainingSimTime * invDeltaTime;
	Vector alignDir;
	linear = vec3_origin;
	angular = vec3_origin;

	if ( m_align )
	{
		QAngle angles;
		Vector origin;
		Vector axis;
		AngularImpulse torque;

		pObject->GetShadowPosition( &origin, &angles );
		// align local normal to target normal
		VMatrix tmp = SetupMatrixOrgAngles( origin, angles );
		Vector worldNormal = tmp.VMul3x3( m_localAlignNormal );
		axis = CrossProduct( worldNormal, m_targetAlignNormal );
		float trig = VectorNormalize(axis);
		float alignRotation = RAD2DEG(asin(trig));
		axis *= alignRotation;
		if ( alignRotation < 10 )
		{
			float dot = DotProduct( worldNormal, m_targetAlignNormal );
			// probably 180 degrees off
			if ( dot < 0 )
			{
				if ( worldNormal.x < 0.5 )
				{
					axis.Init(10,0,0);
				}
				else
				{
					axis.Init(0,0,10);
				}
				alignRotation = 10;
			}
		}
		
		// Solve for the rotation around the target normal (at the local align pos) that will 
		// move the grabbed spot to the destination.
		Vector worldRotCenter = tmp.VMul4x3( m_localAlignPosition );
		Vector rotSrc = world - worldRotCenter;
		Vector rotDest = m_targetPosition - worldRotCenter;

		// Get a basis in the plane perpendicular to m_targetAlignNormal
		Vector srcN = rotSrc;
		VectorNormalize( srcN );
		Vector tangent = CrossProduct( srcN, m_targetAlignNormal );
		float len = VectorNormalize( tangent );

		// needs at least ~5 degrees, or forget rotation (0.08 ~= sin(5))
		if ( len > 0.08 )
		{
			Vector binormal = CrossProduct( m_targetAlignNormal, tangent );

			// Now project the src & dest positions into that plane
			Vector planeSrc( DotProduct( rotSrc, tangent ), DotProduct( rotSrc, binormal ), 0 );
			Vector planeDest( DotProduct( rotDest, tangent ), DotProduct( rotDest, binormal ), 0 );

			float rotRadius = VectorNormalize( planeSrc );
			float destRadius = VectorNormalize( planeDest );
			if ( rotRadius > 0.1 )
			{
				if ( destRadius < rotRadius )
				{
					destRadius = rotRadius;
				}
				//float ratio = rotRadius / destRadius;
				float angleSrc = atan2( planeSrc.y, planeSrc.x );
				float angleDest = atan2( planeDest.y, planeDest.x );
				float angleDiff = angleDest - angleSrc;
				angleDiff = RAD2DEG(angleDiff);
				axis += m_targetAlignNormal * angleDiff;
				//world = m_targetPosition;// + rotDest * (1-ratio);
//				NDebugOverlay::Line( worldRotCenter, worldRotCenter-m_targetAlignNormal*50, 255, 0, 0, false, 0.1 );
//				NDebugOverlay::Line( worldRotCenter, worldRotCenter+tangent*50, 0, 255, 0, false, 0.1 );
//				NDebugOverlay::Line( worldRotCenter, worldRotCenter+binormal*50, 0, 0, 255, false, 0.1 );
			}
		}

		torque = WorldToLocalRotation( tmp, axis, 1 );
		torque *= fracRemainingSimTime * invDeltaTime;
		torque -= angVel * 1.0;	 // damping
		for ( int i = 0; i < 3; i++ )
		{
			if ( torque[i] > 0 )
			{
				if ( torque[i] > m_maxAngularAcceleration[i] )
					torque[i] = m_maxAngularAcceleration[i];
			}
			else
			{
				if ( torque[i] < -m_maxAngularAcceleration[i] )
					torque[i] = -m_maxAngularAcceleration[i];
			}
		}
		torque *= invDeltaTime;
		angular += torque;
		// Calculate an acceleration that pulls the object toward the constraint
		// When you're out of alignment, don't pull very hard
		float factor = fabsf(alignRotation);
		if ( factor < 5 )
		{
			factor = clamp( factor, 0, 5 ) * (1/5);
			alignDir = m_targetAlignPosition - worldRotCenter;
			// Limit movement to the part along m_targetAlignNormal if worldRotCenter is on the backside of 
			// of the target plane (one inch epsilon)!
			float planeForward = DotProduct( alignDir, m_targetAlignNormal );
			if ( planeForward > 1 )
			{
				alignDir = m_targetAlignNormal * planeForward;
			}
			Vector accel = alignDir * invDeltaTime * fracRemainingSimTime * (1-factor) * 0.20 * invDeltaTime;
			float mag = accel.Length();
			if ( mag > m_maxAcceleration )
			{
				accel *= (m_maxAcceleration/mag);
			}
			linear += accel;
		}
		linear -= vel*damping*invDeltaTime;
		// UNDONE: Factor in the change in worldRotCenter due to applied torque!
	}
	else
	{
		// clamp future velocity to max speed
		Vector nextVel = delta + vel;
		float nextSpeed = nextVel.Length();
		if ( nextSpeed > m_maxVel )
		{
			nextVel *= (m_maxVel / nextSpeed);
			delta = nextVel - vel;
		}

		delta *= invDeltaTime;

		float linearAccel = delta.Length();
		if ( linearAccel > m_maxAcceleration )
		{
			delta *= m_maxAcceleration / linearAccel;
		}

		Vector accel;
		AngularImpulse angAccel;
		pObject->CalculateForceOffset( delta, world, &accel, &angAccel );
		
		linear += accel;
		angular += angAccel;
	}
	
	return SIM_GLOBAL_ACCELERATION;
}