hl2sdk-ob-valve: public/jigglebones.cpp@4f3ac1d94787

     1 //===== Copyright � 1996-2005, Valve Corporation, All rights reserved. ======//

     2 //

     3 // Purpose:

     4 //

     5 // $NoKeywords: $

     6 //===========================================================================//

     8 #include "tier1/convar.h"

     9 #include "jigglebones.h"

    11 // memdbgon must be the last include file in a .cpp file!!!

    12 #include "tier0/memdbgon.h"

    14 //-----------------------------------------------------------------------------

    15 ConVar JiggleBoneDebug( "cl_jiggle_bone_debug", "0", FCVAR_CHEAT, "Display physics-based 'jiggle bone' debugging information" );

    16 ConVar JiggleBoneDebugYawConstraints( "cl_jiggle_bone_debug_yaw_constraints", "0", FCVAR_CHEAT, "Display physics-based 'jiggle bone' debugging information" );

    17 ConVar JiggleBoneDebugPitchConstraints( "cl_jiggle_bone_debug_pitch_constraints", "0", FCVAR_CHEAT, "Display physics-based 'jiggle bone' debugging information" );

    19 class CDummyOverlay

    20 {

    21 public:

    22 	void AddLineOverlay(const Vector& origin, const Vector& dest, int r, int g, int b, bool noDepthTest, float duration) {};

    23 };

    25 CDummyOverlay *debugoverlay = new CDummyOverlay;

    27 //-----------------------------------------------------------------------------

    28 JiggleData * CJiggleBones::GetJiggleData( int bone, float currenttime, const Vector &initBasePos, const Vector &initTipPos )

    29 {

    30 	FOR_EACH_LL( m_jiggleBoneState, it )

    31 	{

    32 		if ( m_jiggleBoneState[it].bone == bone )

    33 		{

    34 			return &m_jiggleBoneState[it];

    35 		}

    36 	}

    38 	JiggleData data;

    39 	data.Init( bone, currenttime, initBasePos, initTipPos );

    41 	int idx = m_jiggleBoneState.AddToHead( data );

    42 	if ( idx == m_jiggleBoneState.InvalidIndex() )

    43 		return NULL;

    45 	return &m_jiggleBoneState[idx];

    46 }

    49 //-----------------------------------------------------------------------------

    50 /**

    51  * Do spring physics calculations and update "jiggle bone" matrix

    52  * (Michael Booth, Turtle Rock Studios)

    53  */

    54 void CJiggleBones::BuildJiggleTransformations( int boneIndex, float currenttime, const mstudiojigglebone_t *jiggleInfo, const matrix3x4_t &goalMX, matrix3x4_t &boneMX )

    55 {

    56 	Vector goalBasePosition;

    57 	MatrixPosition( goalMX, goalBasePosition );

    59 	Vector goalForward, goalUp, goalLeft;

    60 	MatrixGetColumn( goalMX, 0, goalLeft );

    61 	MatrixGetColumn( goalMX, 1, goalUp );

    62 	MatrixGetColumn( goalMX, 2, goalForward );

    64 	// compute goal tip position

    65 	Vector goalTip = goalBasePosition + jiggleInfo->length * goalForward;

    67 	JiggleData *data = GetJiggleData( boneIndex, currenttime, goalBasePosition, goalTip );

    68 	if ( !data )

    69 	{

    70 		return;

    71 	}

    73 	if ( currenttime - data->lastUpdate > 0.5f )

    74 	{

    75 		data->Init( boneIndex, currenttime, goalBasePosition, goalTip );

    76 	}

    78 	//Vector bodyVel;

    79 	//EstimateAbsVelocity( bodyVel );

    81 	// limit maximum deltaT to avoid simulation blowups

    82 	// if framerate gets very low, jiggle will run in slow motion

    83 	const float thirtyHZ = 0.0333f;

    84 	const float thousandHZ = 0.001f;

    85 	float deltaT = clamp( currenttime - data->lastUpdate, thousandHZ, thirtyHZ );

    86 	data->lastUpdate = currenttime;

    88 	//

    89 	// Bone tip flex

    90 	//

    91 	if (jiggleInfo->flags & (JIGGLE_IS_FLEXIBLE | JIGGLE_IS_RIGID))

    92 	{

    93 		// apply gravity in global space

    94 		data->tipAccel.z -= jiggleInfo->tipMass;

    96 		if (jiggleInfo->flags & JIGGLE_IS_FLEXIBLE)

    97 		{

    98 			// decompose into local coordinates

    99 			Vector error = goalTip - data->tipPos;

   101 			Vector localError;

   102 			localError.x = DotProduct( goalLeft, error );

   103 			localError.y = DotProduct( goalUp, error );

   104 			localError.z = DotProduct( goalForward, error );

   106 			Vector localVel;

   107 			localVel.x = DotProduct( goalLeft, data->tipVel );

   108 			localVel.y = DotProduct( goalUp, data->tipVel );

   110 			// yaw spring

   111 			float yawAccel = jiggleInfo->yawStiffness * localError.x - jiggleInfo->yawDamping * localVel.x;

   113 			// pitch spring

   114 			float pitchAccel = jiggleInfo->pitchStiffness * localError.y - jiggleInfo->pitchDamping * localVel.y;

   116 			if (jiggleInfo->flags & JIGGLE_HAS_LENGTH_CONSTRAINT)

   117 			{

   118 				// drive tip towards goal tip position

   119 				data->tipAccel += yawAccel * goalLeft + pitchAccel * goalUp;

   120 			}

   121 			else

   122 			{

   123 				// allow flex along length of spring

   124 				localVel.z = DotProduct( goalForward, data->tipVel );

   126 				// along spring

   127 				float alongAccel = jiggleInfo->alongStiffness * localError.z - jiggleInfo->alongDamping * localVel.z;

   129 				// drive tip towards goal tip position

   130 				data->tipAccel += yawAccel * goalLeft + pitchAccel * goalUp + alongAccel * goalForward;

   131 			}

   132 		}

   135 		// simple euler integration

   136 		data->tipVel += data->tipAccel * deltaT;

   137 		data->tipPos += data->tipVel * deltaT;

   139 		// clear this timestep's accumulated accelerations

   140 		data->tipAccel = vec3_origin;

   142 		//

   143 		// Apply optional constraints

   144 		//

   145 		if (jiggleInfo->flags & (JIGGLE_HAS_YAW_CONSTRAINT | JIGGLE_HAS_PITCH_CONSTRAINT))

   146 		{

   147 			// find components of spring vector in local coordinate system

   148 			Vector along = data->tipPos - goalBasePosition;

   149 			Vector localAlong;

   150 			localAlong.x = DotProduct( goalLeft, along );

   151 			localAlong.y = DotProduct( goalUp, along );

   152 			localAlong.z = DotProduct( goalForward, along );

   154 			Vector localVel;

   155 			localVel.x = DotProduct( goalLeft, data->tipVel );

   156 			localVel.y = DotProduct( goalUp, data->tipVel );

   157 			localVel.z = DotProduct( goalForward, data->tipVel );

   159 			if (jiggleInfo->flags & JIGGLE_HAS_YAW_CONSTRAINT)

   160 			{

   161 				// enforce yaw constraints in local XZ plane

   162 				float yawError = atan2( localAlong.x, localAlong.z );

   164 				bool isAtLimit = false;

   165 				float yaw = 0.0f;

   167 				if (yawError < jiggleInfo->minYaw)

   168 				{

   169 					// at angular limit

   170 					isAtLimit = true;

   171 					yaw = jiggleInfo->minYaw;

   172 				}

   173 				else if (yawError > jiggleInfo->maxYaw)

   174 				{

   175 					// at angular limit

   176 					isAtLimit = true;

   177 					yaw = jiggleInfo->maxYaw;

   178 				}

   180 				if (isAtLimit)

   181 				{

   182 					float sy, cy;

   183 					SinCos( yaw, &sy, &cy );

   185 					// yaw matrix

   186 					matrix3x4_t yawMatrix;

   188 					yawMatrix[0][0] = cy;

   189 					yawMatrix[1][0] = 0;

   190 					yawMatrix[2][0] = -sy;

   192 					yawMatrix[0][1] = 0;

   193 					yawMatrix[1][1] = 1.0f;

   194 					yawMatrix[2][1] = 0;

   196 					yawMatrix[0][2] = sy;

   197 					yawMatrix[1][2] = 0;

   198 					yawMatrix[2][2] = cy;

   200 					yawMatrix[0][3] = 0;

   201 					yawMatrix[1][3] = 0;

   202 					yawMatrix[2][3] = 0;

   204 					// global coordinates of limit

   205 					matrix3x4_t limitMatrix;

   206 					ConcatTransforms( goalMX, yawMatrix, limitMatrix );

   208 					Vector limitLeft( limitMatrix.m_flMatVal[0][0],

   209 						limitMatrix.m_flMatVal[1][0],

   210 						limitMatrix.m_flMatVal[2][0] );

   212 					Vector limitUp( limitMatrix.m_flMatVal[0][1],

   213 						limitMatrix.m_flMatVal[1][1],

   214 						limitMatrix.m_flMatVal[2][1] );

   216 					Vector limitForward( limitMatrix.m_flMatVal[0][2],

   217 						limitMatrix.m_flMatVal[1][2],

   218 						limitMatrix.m_flMatVal[2][2] );

   220 					if (JiggleBoneDebugYawConstraints.GetBool())

   221 					{

   222 						float dT = 0.01f;

   223 						const float axisSize = 10.0f;

   224 						debugoverlay->AddLineOverlay( goalBasePosition, goalBasePosition + axisSize * limitLeft, 0, 255, 255, true, dT );

   225 						debugoverlay->AddLineOverlay( goalBasePosition, goalBasePosition + axisSize * limitUp, 255, 255, 0, true, dT );

   226 						debugoverlay->AddLineOverlay( goalBasePosition, goalBasePosition + axisSize * limitForward, 255, 0, 255, true, dT );

   227 					}

   229 					Vector limitAlong( DotProduct( limitLeft, along ),

   230 						DotProduct( limitUp, along ),

   231 						DotProduct( limitForward, along ) );

   233 					// clip to limit plane

   234 					data->tipPos = goalBasePosition + limitAlong.y * limitUp + limitAlong.z * limitForward;

   236 					// yaw friction - rubbing along limit plane

   237 					Vector limitVel;

   238 					limitVel.y = DotProduct( limitUp, data->tipVel );

   239 					limitVel.z = DotProduct( limitForward, data->tipVel );

   241 					data->tipAccel -= jiggleInfo->yawFriction * (limitVel.y * limitUp + limitVel.z * limitForward);

   243 					// update velocity reaction to hitting constraint

   244 					data->tipVel = -jiggleInfo->yawBounce * limitVel.x * limitLeft + limitVel.y * limitUp + limitVel.z * limitForward;

   246 					// update along vectors for use by pitch constraint

   247 					along = data->tipPos - goalBasePosition;

   248 					localAlong.x = DotProduct( goalLeft, along );

   249 					localAlong.y = DotProduct( goalUp, along );

   250 					localAlong.z = DotProduct( goalForward, along );

   252 					localVel.x = DotProduct( goalLeft, data->tipVel );

   253 					localVel.y = DotProduct( goalUp, data->tipVel );

   254 					localVel.z = DotProduct( goalForward, data->tipVel );

   255 				}

   256 			}

   259 			if (jiggleInfo->flags & JIGGLE_HAS_PITCH_CONSTRAINT)

   260 			{

   261 				// enforce pitch constraints in local YZ plane

   262 				float pitchError = atan2( localAlong.y, localAlong.z );

   264 				bool isAtLimit = false;

   265 				float pitch = 0.0f;

   267 				if (pitchError < jiggleInfo->minPitch)

   268 				{

   269 					// at angular limit

   270 					isAtLimit = true;

   271 					pitch = jiggleInfo->minPitch;

   272 				}

   273 				else if (pitchError > jiggleInfo->maxPitch)

   274 				{

   275 					// at angular limit

   276 					isAtLimit = true;

   277 					pitch = jiggleInfo->maxPitch;

   278 				}

   280 				if (isAtLimit)

   281 				{

   282 					float sp, cp;

   283 					SinCos( pitch, &sp, &cp );

   285 					// pitch matrix

   286 					matrix3x4_t pitchMatrix;

   288 					pitchMatrix[0][0] = 1.0f;

   289 					pitchMatrix[1][0] = 0;

   290 					pitchMatrix[2][0] = 0;

   292 					pitchMatrix[0][1] = 0;

   293 					pitchMatrix[1][1] = cp;

   294 					pitchMatrix[2][1] = -sp;

   296 					pitchMatrix[0][2] = 0;

   297 					pitchMatrix[1][2] = sp;

   298 					pitchMatrix[2][2] = cp;

   300 					pitchMatrix[0][3] = 0;

   301 					pitchMatrix[1][3] = 0;

   302 					pitchMatrix[2][3] = 0;

   304 					// global coordinates of limit

   305 					matrix3x4_t limitMatrix;

   306 					ConcatTransforms( goalMX, pitchMatrix, limitMatrix );

   308 					Vector limitLeft( limitMatrix.m_flMatVal[0][0],

   309 						limitMatrix.m_flMatVal[1][0],

   310 						limitMatrix.m_flMatVal[2][0] );

   312 					Vector limitUp( limitMatrix.m_flMatVal[0][1],

   313 						limitMatrix.m_flMatVal[1][1],

   314 						limitMatrix.m_flMatVal[2][1] );

   316 					Vector limitForward( limitMatrix.m_flMatVal[0][2],

   317 						limitMatrix.m_flMatVal[1][2],

   318 						limitMatrix.m_flMatVal[2][2] );

   320 					if (JiggleBoneDebugPitchConstraints.GetBool())

   321 					{

   322 						float dT = 0.01f;

   323 						const float axisSize = 10.0f;

   324 						debugoverlay->AddLineOverlay( goalBasePosition, goalBasePosition + axisSize * limitLeft, 0, 255, 255, true, dT );

   325 						debugoverlay->AddLineOverlay( goalBasePosition, goalBasePosition + axisSize * limitUp, 255, 255, 0, true, dT );

   326 						debugoverlay->AddLineOverlay( goalBasePosition, goalBasePosition + axisSize * limitForward, 255, 0, 255, true, dT );

   327 					}

   329 					Vector limitAlong( DotProduct( limitLeft, along ),

   330 						DotProduct( limitUp, along ),

   331 						DotProduct( limitForward, along ) );

   333 					// clip to limit plane

   334 					data->tipPos = goalBasePosition + limitAlong.x * limitLeft + limitAlong.z * limitForward;

   336 					// pitch friction - rubbing along limit plane

   337 					Vector limitVel;

   338 					limitVel.y = DotProduct( limitUp, data->tipVel );

   339 					limitVel.z = DotProduct( limitForward, data->tipVel );

   341 					data->tipAccel -= jiggleInfo->pitchFriction * (limitVel.x * limitLeft + limitVel.z * limitForward);

   343 					// update velocity reaction to hitting constraint

   344 					data->tipVel = limitVel.x * limitLeft - jiggleInfo->pitchBounce * limitVel.y * limitUp + limitVel.z * limitForward;

   345 				}

   346 			}

   347 		}

   349 		// needed for matrix assembly below

   350 		Vector forward = data->tipPos - goalBasePosition;

   351 		forward.NormalizeInPlace();

   353 		if (jiggleInfo->flags & JIGGLE_HAS_ANGLE_CONSTRAINT)

   354 		{

   355 			// enforce max angular error

   356 			Vector error = goalTip - data->tipPos;

   357 			float dot = DotProduct( forward, goalForward );

   358 			float angleBetween = acos( dot );

   359 			if (dot < 0.0f)

   360 			{

   361 				angleBetween = 2.0f * M_PI - angleBetween;

   362 			}

   364 			if (angleBetween > jiggleInfo->angleLimit)

   365 			{

   366 				// at angular limit

   367 				float maxBetween = jiggleInfo->length * sin( jiggleInfo->angleLimit );

   369 				Vector delta = goalTip - data->tipPos;

   370 				delta.NormalizeInPlace();

   372 				data->tipPos = goalTip - maxBetween * delta;

   374 				forward = data->tipPos - goalBasePosition;

   375 				forward.NormalizeInPlace();

   376 			}

   377 		}

   379 		if (jiggleInfo->flags & JIGGLE_HAS_LENGTH_CONSTRAINT)

   380 		{

   381 			// enforce spring length

   382 			data->tipPos = goalBasePosition + jiggleInfo->length * forward;

   384 			// zero velocity along forward bone axis

   385 			data->tipVel -= DotProduct( data->tipVel, forward ) * forward;

   386 		}

   388 		//

   389 		// Build bone matrix to align along current tip direction

   390 		//

   391 		Vector left = CrossProduct( goalUp, forward );

   392 		left.NormalizeInPlace();

   394 		Vector up = CrossProduct( forward, left );

   396 		boneMX[0][0] = left.x;

   397 		boneMX[1][0] = left.y;

   398 		boneMX[2][0] = left.z;

   399 		boneMX[0][1] = up.x;

   400 		boneMX[1][1] = up.y;

   401 		boneMX[2][1] = up.z;

   402 		boneMX[0][2] = forward.x;

   403 		boneMX[1][2] = forward.y;

   404 		boneMX[2][2] = forward.z;

   406 		boneMX[0][3] = goalBasePosition.x;

   407 		boneMX[1][3] = goalBasePosition.y;

   408 		boneMX[2][3] = goalBasePosition.z;

   409 	}

   412 	//

   413 	// Bone base flex

   414 	//

   415 	if (jiggleInfo->flags & JIGGLE_HAS_BASE_SPRING)

   416 	{

   417 		// gravity

   418 		data->baseAccel.z -= jiggleInfo->baseMass;

   420 		// simple spring

   421 		Vector error = goalBasePosition - data->basePos;

   422 		data->baseAccel += jiggleInfo->baseStiffness * error - jiggleInfo->baseDamping * data->baseVel;

   424 		data->baseVel += data->baseAccel * deltaT;

   425 		data->basePos += data->baseVel * deltaT;

   427 		// clear this timestep's accumulated accelerations

   428 		data->baseAccel = vec3_origin;

   430 		// constrain to limits

   431 		error = data->basePos - goalBasePosition;

   432 		Vector localError;

   433 		localError.x = DotProduct( goalLeft, error );

   434 		localError.y = DotProduct( goalUp, error );

   435 		localError.z = DotProduct( goalForward, error );

   437 		Vector localVel;

   438 		localVel.x = DotProduct( goalLeft, data->baseVel );

   439 		localVel.y = DotProduct( goalUp, data->baseVel );

   440 		localVel.z = DotProduct( goalForward, data->baseVel );

   442 		// horizontal constraint

   443 		if (localError.x < jiggleInfo->baseMinLeft)

   444 		{

   445 			localError.x = jiggleInfo->baseMinLeft;

   447 			// friction

   448 			data->baseAccel -= jiggleInfo->baseLeftFriction * (localVel.y * goalUp + localVel.z * goalForward);

   449 		}

   450 		else if (localError.x > jiggleInfo->baseMaxLeft)

   451 		{

   452 			localError.x = jiggleInfo->baseMaxLeft;

   454 			// friction

   455 			data->baseAccel -= jiggleInfo->baseLeftFriction * (localVel.y * goalUp + localVel.z * goalForward);

   456 		}

   458 		if (localError.y < jiggleInfo->baseMinUp)

   459 		{

   460 			localError.y = jiggleInfo->baseMinUp;

   462 			// friction

   463 			data->baseAccel -= jiggleInfo->baseUpFriction * (localVel.x * goalLeft + localVel.z * goalForward);

   464 		}

   465 		else if (localError.y > jiggleInfo->baseMaxUp)

   466 		{

   467 			localError.y = jiggleInfo->baseMaxUp;

   469 			// friction

   470 			data->baseAccel -= jiggleInfo->baseUpFriction * (localVel.x * goalLeft + localVel.z * goalForward);

   471 		}

   473 		if (localError.z < jiggleInfo->baseMinForward)

   474 		{

   475 			localError.z = jiggleInfo->baseMinForward;

   477 			// friction

   478 			data->baseAccel -= jiggleInfo->baseForwardFriction * (localVel.x * goalLeft + localVel.y * goalUp);

   479 		}

   480 		else if (localError.z > jiggleInfo->baseMaxForward)

   481 		{

   482 			localError.z = jiggleInfo->baseMaxForward;

   484 			// friction

   485 			data->baseAccel -= jiggleInfo->baseForwardFriction * (localVel.x * goalLeft + localVel.y * goalUp);

   486 		}

   488 		data->basePos = goalBasePosition + localError.x * goalLeft + localError.y * goalUp + localError.z * goalForward;

   491 		// fix up velocity

   492 		data->baseVel = (data->basePos - data->baseLastPos) / deltaT;

   493 		data->baseLastPos = data->basePos;

   496 		if (!(jiggleInfo->flags & (JIGGLE_IS_FLEXIBLE | JIGGLE_IS_RIGID)))

   497 		{

   498 			// no tip flex - use bone's goal orientation

   499 			boneMX = goalMX;

   500 		}

   502 		// update bone position

   503 		MatrixSetColumn( data->basePos, 3, boneMX );

   504 	}

   505 	else if (!(jiggleInfo->flags & (JIGGLE_IS_FLEXIBLE | JIGGLE_IS_RIGID)))

   506 	{

   507 		// no flex at all - just use goal matrix

   508 		boneMX = goalMX;

   509 	}

   512 	// debug display

   513 	if ( JiggleBoneDebug.GetBool() )

   514 	{

   515 		float dT = 0.01f;

   516 		const float axisSize = 5.0f;

   517 		debugoverlay->AddLineOverlay( goalBasePosition, goalBasePosition + axisSize * goalLeft, 255, 0, 0, true, dT );

   518 		debugoverlay->AddLineOverlay( goalBasePosition, goalBasePosition + axisSize * goalUp, 0, 255, 0, true, dT );

   519 		debugoverlay->AddLineOverlay( goalBasePosition, goalBasePosition + axisSize * goalForward, 0, 0, 255, true, dT );

   522 		const float sz = 1.0f;

   524 		if (jiggleInfo->flags & (JIGGLE_IS_FLEXIBLE | JIGGLE_IS_RIGID))

   525 		{

   526 			debugoverlay->AddLineOverlay( goalBasePosition,

   527 				data->tipPos, 255, 255, 0, true, dT );

   529 			debugoverlay->AddLineOverlay( data->tipPos + Vector( -sz, 0, 0 ),

   530 				data->tipPos + Vector( sz, 0, 0 ), 0, 255, 255, true, dT );

   531 			debugoverlay->AddLineOverlay( data->tipPos + Vector( 0, -sz, 0 ),

   532 				data->tipPos + Vector( 0, sz, 0 ), 0, 255, 255, true, dT );

   533 			debugoverlay->AddLineOverlay( data->tipPos + Vector( 0, 0, -sz ),

   534 				data->tipPos + Vector( 0, 0, sz ), 0, 255, 255, true, dT );

   535 		}

   537 		if (jiggleInfo->flags & JIGGLE_HAS_BASE_SPRING)

   538 		{

   539 			debugoverlay->AddLineOverlay( data->basePos + Vector( -sz, 0, 0 ),

   540 				data->basePos + Vector( sz, 0, 0 ), 255, 0, 255, true, dT );

   541 			debugoverlay->AddLineOverlay( data->basePos + Vector( 0, -sz, 0 ),

   542 				data->basePos + Vector( 0, sz, 0 ), 255, 0, 255, true, dT );

   543 			debugoverlay->AddLineOverlay( data->basePos + Vector( 0, 0, -sz ),

   544 				data->basePos + Vector( 0, 0, sz ), 255, 0, 255, true, dT );

   545 		}

   546 	}

   547 }

hl2sdk-ob-valve

author	Scott Ehlert <ds@alliedmods.net>
	Sat Apr 27 10:28:01 2013 -0500 (2013-04-27)
changeset 94	4f3ac1d94787
permissions	-rw-r--r--