static void matrixUpdateJob( void* pData )
{
tMatrixJobData* pMatrixData = (tMatrixJobData *)pData;
tVector4 const* pScale = pMatrixData->mpScale;
//tJoint* pJoint = pMatrixData->mpJoint;
// concat for animation matrix
tMatrix44 posMatrix, scaleMatrix, rotMatrix, posRotMatrix;
//quaternionToMatrix( &rotMatrix, pMatrixData->mpRotation );
tMatrix44 rotMatZY, rotMatX, rotMatY, rotMatZ;
Matrix44RotateX( &rotMatX, pMatrixData->mpRotationVec->fX );
Matrix44RotateY( &rotMatY, pMatrixData->mpRotationVec->fY );
Matrix44RotateZ( &rotMatZ, pMatrixData->mpRotationVec->fZ );
Matrix44Multiply( &rotMatZY, &rotMatZ, &rotMatY );
Matrix44Multiply( &rotMatrix, &rotMatZY, &rotMatX );
Matrix44Scale( &scaleMatrix, pScale->fX, pScale->fY, pScale->fZ );
Matrix44Translate( &posMatrix, pMatrixData->mpPosition );
Matrix44Multiply( &posRotMatrix, &posMatrix, &rotMatrix );
// result
Matrix44Multiply( pMatrixData->mpResultMat, &posRotMatrix, &scaleMatrix );
//Matrix44Multiply( pJoint->mpAnimMatrix, &posRotMatrix, &scaleMatrix );
}
void animSequenceUpdateAnimMatrices( tAnimSequence const* pAnimSequence,
tAnimHierarchy const* pAnimHierarchy,
tVector4* aPositions,
tVector4* aScalings,
tQuaternion* aRotations,
tVector4* aRotationVecs,
tMatrix44* aAnimMatrices )
{
int iNumJoints = pAnimSequence->miNumJoints;
for( int i = 0; i < iNumJoints; i++ )
{
int iStart = pAnimSequence->maiStartFrames[i];
int iEnd = pAnimSequence->maiEndFrames[i];
// no valid animation frame for this joint
if( iStart < 0 || iEnd < 0 )
{
continue;
}
tJoint* pJoint = pAnimSequence->mapJoints[iStart];
// get corresponding joint index from hierarchy
int iJointIndex = 0;
int iNumHierarchyJoints = pAnimHierarchy->miNumJoints;
for( iJointIndex = 0; iJointIndex < iNumHierarchyJoints; iJointIndex++ )
{
if( !strcmp( pAnimHierarchy->maJoints[iJointIndex].mszName, pJoint->mszName ) )
{
break;
}
}
WTFASSERT2( iJointIndex < iNumHierarchyJoints, "can't find corresponding joint" );
tMatrix44 posMatrix, scaleMatrix, rotMatrix, posRotMatrix;
//quaternionToMatrix( &rotMatrix, pMatrixData->mpRotation );
tMatrix44 rotMatZY, rotMatX, rotMatY, rotMatZ;
Matrix44RotateX( &rotMatX, aRotationVecs[i].fX );
Matrix44RotateY( &rotMatY, aRotationVecs[i].fY );
Matrix44RotateZ( &rotMatZ, aRotationVecs[i].fZ );
Matrix44Multiply( &rotMatZY, &rotMatZ, &rotMatY );
Matrix44Multiply( &rotMatrix, &rotMatZY, &rotMatX );
Matrix44Scale( &scaleMatrix, aScalings[i].fX, aScalings[i].fY, aScalings[i].fZ );
Matrix44Translate( &posMatrix, &aPositions[i] );
Matrix44Multiply( &posRotMatrix, &posMatrix, &rotMatrix );
Matrix44Multiply( &aAnimMatrices[iJointIndex], &posRotMatrix, &scaleMatrix );
#if 0
tJob job;
tMatrixJobData matrixJobData;
matrixJobData.mpPosition = &aPositions[i];
matrixJobData.mpRotation = &aRotations[i];
matrixJobData.mpScale = &aScalings[i];
matrixJobData.mpResultMat = &aAnimMatrices[iJointIndex];
matrixJobData.mpJoint = pAnimSequence->mapUniqueJoints[i];
matrixJobData.mpRotationVec = &aRotationVecs[i];
job.mpfnFunc = &matrixUpdateJob;
job.mpData = &matrixJobData;
job.miDataSize = sizeof( tMatrixJobData );
jobManagerAddJob( gpJobManager, &job );
#endif // #if 0
} // for i = 0 to num joints
}
void animSequencePlay( tAnimSequence const* pAnimSequence,
tAnimHierarchy const* pAnimHierarchy,
tVector4* aPositions,
tVector4* aScalings,
tQuaternion* aRotations,
tVector4* aRotationVecs,
tMatrix44* aAnimMatrices,
float fTime )
{
float fNumLoops = floorf( fTime / pAnimSequence->mfLastTime );
float fFrameTime = fTime - fNumLoops * pAnimSequence->mfLastTime;
// key frame for the joints
int iNumJoints = pAnimSequence->miNumJoints;
for( int i = 0; i < iNumJoints; i++ )
{
int iStart = pAnimSequence->maiStartFrames[i];
int iEnd = pAnimSequence->maiEndFrames[i];
// no valid animation frame for this joint
if( iStart < 0 || iEnd < 0 )
{
continue;
}
// find the ending frame
int iEndFrame = iStart;
for( iEndFrame = iStart; iEndFrame <= iEnd; iEndFrame++ )
{
if( pAnimSequence->mafTime[iEndFrame] > fFrameTime )
{
break;
}
} // for end frame = start to end
// didn't find end frame
if( iEndFrame > iEnd )
{
iEndFrame = iEnd;
}
int iStartFrame = iEndFrame - 1;
if( iStartFrame < 0 )
{
iStartFrame = 0;
}
// just at the start
if( iStartFrame < iStart )
{
iStartFrame = iStart;
}
WTFASSERT2( iStartFrame >= iStart && iStartFrame <= iEnd, "out of bounds looking for start animation frame" );
WTFASSERT2( iEndFrame >= iStart && iEndFrame <= iEnd, "out of bounds looking for end animation frame" );
float fTimeDuration = pAnimSequence->mafTime[iEndFrame] - pAnimSequence->mafTime[iStartFrame];
float fTimeFromStart = fFrameTime - pAnimSequence->mafTime[iStartFrame];
float fPct = 0.0f;
if( fTimeDuration > 0.0f )
{
fPct = fTimeFromStart / fTimeDuration;
}
// clamp pct
if( fPct > 1.0f )
{
fPct = 1.0f;
}
Vector4Lerp( &aPositions[i],
&pAnimSequence->maPositions[iStartFrame],
&pAnimSequence->maPositions[iEndFrame],
fPct );
Vector4Lerp( &aScalings[i],
&pAnimSequence->maScalings[iStartFrame],
&pAnimSequence->maScalings[iEndFrame],
fPct );
quaternionSlerp( &aRotations[i],
&pAnimSequence->maRotation[iStartFrame],
&pAnimSequence->maRotation[iEndFrame],
fPct );
Vector4Lerp( &aRotationVecs[i],
&pAnimSequence->maRotationVec[iStartFrame],
&pAnimSequence->maRotationVec[iEndFrame],
fPct );
#if 0
tJoint* pJoint = pAnimSequence->mapJoints[iStart];
tJob job;
tKeyFrameJobData interpJobData;
interpJobData.mfPct = fPct;
interpJobData.mpPos0 = &pAnimSequence->maPositions[iStartFrame];
interpJobData.mpPos1 = &pAnimSequence->maPositions[iEndFrame];
interpJobData.mpScale0 = &pAnimSequence->maScalings[iStartFrame];
interpJobData.mpScale1 = &pAnimSequence->maScalings[iEndFrame];
interpJobData.mpRot0 = &pAnimSequence->maRotation[iStartFrame];
interpJobData.mpRot1 = &pAnimSequence->maRotation[iEndFrame];
interpJobData.mpResultPos = &aPositions[i];
interpJobData.mpResultScale = &aScalings[i];
interpJobData.mpResultRot = &aRotations[i];
interpJobData.mpJoint = pJoint;
interpJobData.mpRotVec0 = &pAnimSequence->maRotationVec[iStartFrame];
interpJobData.mpRotVec1 = &pAnimSequence->maRotationVec[iEndFrame];
interpJobData.mpResultRotVec = &aRotationVecs[i];
job.mpfnFunc = interpolateJob;
job.mpData = &interpJobData;
job.miDataSize = sizeof( tKeyFrameJobData );
jobManagerAddJob( gpJobManager, &job );
#endif // #if 0
}
#if 0
tVector4* pScale = &aScalings[i];
tVector4* pPosition = &aPositions[i];
// interpolate position, scale, and rotation
Vector4Lerp( pPosition,
&pAnimSequence->maPositions[iEndFrame],
&pAnimSequence->maPositions[iStartFrame],
fPct );
Vector4Lerp( pScale,
&pAnimSequence->maScalings[iEndFrame],
&pAnimSequence->maScalings[iStartFrame],
fPct );
quaternionSlerp( &aRotations[i],
&pAnimSequence->maRotation[iEndFrame],
&pAnimSequence->maRotation[iStartFrame],
fPct );
jobManagerWait( gpJobManager );
#endif // #if 0
for( int i = 0; i < iNumJoints; i++ )
{
int iStart = pAnimSequence->maiStartFrames[i];
int iEnd = pAnimSequence->maiEndFrames[i];
// no valid animation frame for this joint
if( iStart < 0 || iEnd < 0 )
{
continue;
}
#if 0
tJob job;
tMatrixJobData matrixJobData;
matrixJobData.mpPosition = &aPositions[i];
matrixJobData.mpRotation = &aRotations[i];
matrixJobData.mpScale = &aScalings[i];
matrixJobData.mpResultMat = &aAnimMatrices[i];
matrixJobData.mpJoint = pAnimSequence->mapUniqueJoints[i];
matrixJobData.mpRotationVec = &aRotationVecs[i];
job.mpfnFunc = &matrixUpdateJob;
job.mpData = &matrixJobData;
job.miDataSize = sizeof( tMatrixJobData );
jobManagerAddJob( gpJobManager, &job );
#endif // #if 0
tMatrix44 posMatrix, scaleMatrix, rotMatrix, posRotMatrix;
//quaternionToMatrix( &rotMatrix, pMatrixData->mpRotation );
tMatrix44 rotMatZY, rotMatX, rotMatY, rotMatZ;
Matrix44RotateX( &rotMatX, aRotationVecs[i].fX );
Matrix44RotateY( &rotMatY, aRotationVecs[i].fY );
Matrix44RotateZ( &rotMatZ, aRotationVecs[i].fZ );
Matrix44Multiply( &rotMatZY, &rotMatZ, &rotMatY );
Matrix44Multiply( &rotMatrix, &rotMatZY, &rotMatX );
Matrix44Scale( &scaleMatrix, aScalings[i].fX, aScalings[i].fY, aScalings[i].fZ );
Matrix44Translate( &posMatrix, &aPositions[i] );
Matrix44Multiply( &posRotMatrix, &posMatrix, &rotMatrix );
Matrix44Multiply( &aAnimMatrices[i], &posRotMatrix, &scaleMatrix );
#if 0
// concat for animation matrix
tMatrix44 posMatrix, scaleMatrix, rotMatrix, posRotMatrix;
quaternionToMatrix( &rotMatrix, &aRotations[i] );
Matrix44Scale( &scaleMatrix, pScale->fX, pScale->fY, pScale->fZ );
Matrix44Translate( &posMatrix, pPosition );
Matrix44Multiply( &posRotMatrix, &posMatrix, &rotMatrix );
tMatrix44* pAnimMatrix = &aAnimMatrices[i];
Matrix44Multiply( pAnimMatrix, &posRotMatrix, &scaleMatrix );
#endif // #if 0
} // for i = 0 to num joints
jobManagerWait( gpJobManager );
}
//-----------------------------------------------------------------------------
// Проверка столкновений Sphere-Mesh
//-----------------------------------------------------------------------------
bool vw_SphereMeshCollision(VECTOR3D Object1Location, eObjectBlock *Object1DrawObjectList, float Object1RotationMatrix[9],
float Object2Radius, VECTOR3D Object2Location, VECTOR3D Object2PrevLocation,
VECTOR3D *CollisionLocation)
{
// если не передали геометрию - делать тут нечего
if (Object1DrawObjectList == 0) return false;
// делаем матрицу перемещения точек, для геометрии
float TransMat[16];
TransMat[0] = Object1RotationMatrix[0];
TransMat[1] = Object1RotationMatrix[1];
TransMat[2] = Object1RotationMatrix[2];
TransMat[3] = 0.0f;
TransMat[4] = Object1RotationMatrix[3];
TransMat[5] = Object1RotationMatrix[4];
TransMat[6] = Object1RotationMatrix[5];
TransMat[7] = 0.0f;
TransMat[8] = Object1RotationMatrix[6];
TransMat[9] = Object1RotationMatrix[7];
TransMat[10] = Object1RotationMatrix[8];
TransMat[11] = 0.0f;
TransMat[12] = Object1Location.x;
TransMat[13] = Object1Location.y;
TransMat[14] = Object1Location.z;
TransMat[15] = 1.0f;
// находим точку локального положения объекта в моделе
VECTOR3D LocalLocation(Object1DrawObjectList->Location);
Matrix33CalcPoint(&LocalLocation, Object1RotationMatrix);
// если нужно - создаем матрицу, иначе - копируем ее
if (Object1DrawObjectList->Rotation.x != 0.0f ||
Object1DrawObjectList->Rotation.y != 0.0f ||
Object1DrawObjectList->Rotation.z != 0.0f)
{
float TransMatTMP[16];
Matrix44Identity(TransMatTMP);
Matrix44CreateRotate(TransMatTMP, Object1DrawObjectList->Rotation);
Matrix44Translate(TransMatTMP, LocalLocation);
// и умножаем на основную матрицу, со сведениями по всему объекту
Matrix44Mult(TransMat, TransMatTMP);
}
else
{
Matrix44Translate(TransMat, LocalLocation);
}
// проверяем все треугольники объекта
for (int i=0; i<Object1DrawObjectList->VertexCount; i+=3)
{
// находим 3 точки треугольника (с учетом индекс буфера)
int j2;
if (Object1DrawObjectList->IndexBuffer != 0)
j2 = Object1DrawObjectList->IndexBuffer[Object1DrawObjectList->RangeStart+i]*Object1DrawObjectList->VertexStride;
else
j2 = (Object1DrawObjectList->RangeStart+i)*Object1DrawObjectList->VertexStride;
// находим точки триугольника
VECTOR3D Point1;
Point1.x = Object1DrawObjectList->VertexBuffer[j2];
Point1.y = Object1DrawObjectList->VertexBuffer[j2+1];
Point1.z = Object1DrawObjectList->VertexBuffer[j2+2];
Matrix44CalcPoint(&Point1, TransMat);
if (Object1DrawObjectList->IndexBuffer != 0)
j2 = Object1DrawObjectList->IndexBuffer[Object1DrawObjectList->RangeStart+i+1]*Object1DrawObjectList->VertexStride;
else
j2 = (Object1DrawObjectList->RangeStart+i+1)*Object1DrawObjectList->VertexStride;
VECTOR3D Point2;
Point2.x = Object1DrawObjectList->VertexBuffer[j2];
Point2.y = Object1DrawObjectList->VertexBuffer[j2+1];
Point2.z = Object1DrawObjectList->VertexBuffer[j2+2];
Matrix44CalcPoint(&Point2, TransMat);
if (Object1DrawObjectList->IndexBuffer != 0)
j2 = Object1DrawObjectList->IndexBuffer[Object1DrawObjectList->RangeStart+i+2]*Object1DrawObjectList->VertexStride;
else
j2 = (Object1DrawObjectList->RangeStart+i+2)*Object1DrawObjectList->VertexStride;
VECTOR3D Point3;
Point3.x = Object1DrawObjectList->VertexBuffer[j2];
Point3.y = Object1DrawObjectList->VertexBuffer[j2+1];
Point3.z = Object1DrawObjectList->VertexBuffer[j2+2];
Matrix44CalcPoint(&Point3, TransMat);
// находим 2 вектора, образующих плоскость
VECTOR3D PlaneVector1 = Point2 - Point1;
VECTOR3D PlaneVector2 = Point3 - Point1;
// находим нормаль плоскости
VECTOR3D NormalVector = PlaneVector1;
NormalVector.Multiply(PlaneVector2);
NormalVector.Normalize();
// проверка, сферы
// - в сфере или нет?, т.е. расстояние от плоскости до центра сферы
// - по нормале находим точку на плоскости
// - точка принадлежит треугольнику?
// находим расстояние от точки до плоскости
float Distance = (Object2Location - Point1)*NormalVector;
// если точка дальше радиуса - нам тут делать нечего, берем следующую точку
if (fabsf(Distance) <= Object2Radius)
{
// находим реальную точку пересечения с плоскостью
VECTOR3D IntercPoint = Object2Location - (NormalVector^Distance);
// передаем точку и флаг успешной коллизии
if (vw_PointInTriangle(IntercPoint, Point1, Point2, Point3))
{
*CollisionLocation = IntercPoint;
return true;
}
}
// проверка, сферы
// если от точек до текущего положения расстояние меньше
float Object2Radius2 = Object2Radius*Object2Radius;
// находим расстояние
VECTOR3D DistancePoint1 = Object2Location - Point1;
float Distance2Point1 = DistancePoint1.x*DistancePoint1.x + DistancePoint1.y*DistancePoint1.y + DistancePoint1.z*DistancePoint1.z;
if (Distance2Point1 <= Object2Radius2)
{
*CollisionLocation = Point1;
return true;
}
// находим расстояние
VECTOR3D DistancePoint2 = Object2Location - Point2;
float Distance2Point2 = DistancePoint2.x*DistancePoint2.x + DistancePoint2.y*DistancePoint2.y + DistancePoint2.z*DistancePoint2.z;
if (Distance2Point2 <= Object2Radius2)
{
*CollisionLocation = Point2;
return true;
}
// находим расстояние
VECTOR3D DistancePoint3 = Object2Location - Point3;
float Distance2Point3 = DistancePoint3.x*DistancePoint3.x + DistancePoint3.y*DistancePoint3.y + DistancePoint3.z*DistancePoint3.z;
if (Distance2Point3 <= Object2Radius2)
{
*CollisionLocation = Point3;
return true;
}
// проверка луч, старое положение - новое положение объекта
// - пересекает плоскость (обе точки с одной стороны? знак, или ноль)
// - точка пересечения плоскости
// - принадлежит треугольнику?
// проверка, если это фронт-часть, нормально... иначе - берем следующую
VECTOR3D vDir1 = Point1 - Object2PrevLocation;
float d1 = vDir1*NormalVector;
if (d1>0.001f) continue;
// находим расстояние от плоскости до точки
float originDistance = NormalVector*Point1;
// пересечение прямой с плоскостью
VECTOR3D vLineDir = Object2Location - Object2PrevLocation;
// может и не нужно этого делать... т.к. все и так хорошо работает в нашем случае
//vLineDir.Normalize();
float Numerator = - (NormalVector.x * Object2PrevLocation.x + // Use the plane equation with the normal and the line
NormalVector.y * Object2PrevLocation.y +
NormalVector.z * Object2PrevLocation.z - originDistance);
float Denominator = NormalVector*vLineDir;
if( Denominator == 0.0) continue;
float dist = Numerator / Denominator;
// зная расстояние, находим точку пересечения с плоскостью
VECTOR3D IntercPoint = Object2PrevLocation + (vLineDir ^ dist);
// проверяем, на отрезке или нет (до этого работали с прямой/лучем)
if ((Object2PrevLocation-IntercPoint)*(Object2Location-IntercPoint) >= 0.0f) continue;
// передаем точку и флаг успешной коллизии
if (vw_PointInTriangle(IntercPoint, Point1, Point2, Point3))
{
*CollisionLocation = IntercPoint;
return true;
}
}
return false;
}
