public static bool _LimitYZ(
bool isRight,
ref Vector3 dir, // dirX
float limitYMinus, // Y-
float limitYPlus, // Y+
float limitZMinus, // Z-
float limitZPlus ) // Z+
{
bool isYPlus = (dir.y >= 0.0f);
bool isZPlus = (dir.z >= 0.0f);
float yLimit = isYPlus ? limitYPlus : limitYMinus;
float zLimit = isZPlus ? limitZPlus : limitZMinus;
bool isLimited = false;
if( yLimit <= IKEpsilon && zLimit <= IKEpsilon ) {
Vector3 limitedDir = isRight ? new Vector3( 1.0f, 0.0f, 0.0f ) : new Vector3( -1.0f, 0.0f, 0.0f );
Vector3 temp = limitedDir - dir;
if( Mathf.Abs( temp.x ) > IKEpsilon || Mathf.Abs( temp.y ) > IKEpsilon || Mathf.Abs( temp.z ) > IKEpsilon ) {
dir = limitedDir;
isLimited = true;
}
} else {
float inv_yLimit = (yLimit >= IKEpsilon) ? (1.0f / yLimit) : 0.0f;
float inv_zLimit = (zLimit >= IKEpsilon) ? (1.0f / zLimit) : 0.0f;
float localY = dir.y * inv_yLimit;
float localZ = dir.z * inv_zLimit;
float localLen = SAFBIKSqrt( dir.x * dir.x + localY * localY + localZ * localZ );
float inv_localLen = (localLen > IKEpsilon) ? (1.0f / localLen) : 0.0f;
float nrm_localY = localY * inv_localLen; // Counts as sinTheta
float nrm_localZ = localZ * inv_localLen; // Counts as cosTheta
if( localLen > 1.0f ) { // Outer circle.
if( !isLimited ) {
isLimited = true;
localY = nrm_localY;
localZ = nrm_localZ;
}
}
float worldY = isLimited ? (localY * yLimit) : dir.y;
float worldZ = isLimited ? (localZ * zLimit) : dir.z;
bool isInverse = ((dir.x >= 0.0f) != isRight);
if( isLimited ) {
float limitSinSq = (worldY * worldY + worldZ * worldZ);
float limitSin = SAFBIKSqrt( limitSinSq );
float limitCos = SAFBIKSqrt( 1.0f - limitSin * limitSin );
dir.x = isRight ? limitCos : -limitCos;
dir.y = worldY;
dir.z = worldZ;
} else if( isInverse ) {
isLimited = true;
dir.x = -dir.x;
}
}
return isLimited;
}
