void KG3DTransformation::NormalizeRotation()
{
D3DXQuaternionNormalize(&m_Rotation, &m_Rotation);
ASSERT_FLOAT_IS_VALIED(m_Rotation.x);
D3DXQuaternionNormalize(&m_ScalingRotation, &m_ScalingRotation);
UpdateTransformation();
}
void FastSlerp ( D3DXQUATERNION *out_quat,
D3DXQUATERNION *in_first,
D3DXQUATERNION *in_second,
const float in_disp )
{
if ( ( ( in_first->x - in_second->x ) < 0.001f ) &&
( ( in_first->y - in_second->y ) < 0.001f ) &&
( ( in_first->z - in_second->z ) < 0.001f ) &&
( ( in_first->w - in_second->w ) < 0.001f ) )
{
memcpy ( out_quat, in_first, sizeof(D3DXQUATERNION) );
return;
}
if ( ( ( in_first->x + in_second->x ) < 0.001f ) &&
( ( in_first->y + in_second->y ) < 0.001f ) &&
( ( in_first->z + in_second->z ) < 0.001f ) &&
( ( in_first->w + in_second->w ) < 0.001f ) )
{
memcpy ( out_quat, in_first, sizeof(D3DXQUATERNION) );
return;
}
D3DXQuaternionNormalize ( in_first, in_first );
D3DXQuaternionNormalize ( in_second, in_second );
double cosval = in_first ->x * in_second ->x + in_first ->y * in_second ->y + in_first ->z * in_second ->z + in_first ->w * in_second ->w;
double theta = acos ( cosval );
char temp [256];
if ( sin ( theta ) < 0.0001f )
{
sprintf ( temp, "ERROR!: sin value is too small!, cosval : %f", cosval );
ShowError ( temp, __FILE__, __LINE__ );
}
double invsin = 1.0f / sin ( theta );
double a = sin ( theta * ( 1.0f - in_disp ) ) * invsin;
double b = sin ( theta * in_disp ) * invsin;
out_quat ->x = a * in_first ->x + b * in_second ->x;
out_quat ->y = a * in_first ->y + b * in_second ->y;
out_quat ->z = a * in_first ->z + b * in_second ->z;
out_quat ->w = a * in_first ->w + b * in_second ->w;
D3DXQuaternionNormalize ( out_quat, out_quat );
}
OBB* RagDoll::CreateBoneBox(Bone* parent, Bone *bone, D3DXVECTOR3 size, D3DXQUATERNION rot) {
if (bone == NULL || parent == NULL)
return NULL;
D3DXMATRIX &parentMat = parent->CombinedTransformationMatrix;
D3DXMATRIX &boneMat = bone->CombinedTransformationMatrix;
D3DXVECTOR3 parentPos(parentMat(3, 0), parentMat(3, 1), parentMat(3, 2));
D3DXVECTOR3 bonePos(boneMat(3, 0), boneMat(3, 1), boneMat(3, 2));
D3DXQUATERNION q;
D3DXVECTOR3 p, s;
D3DXMatrixDecompose(&s, &q, &p, &parentMat);
q *= rot;
D3DXQuaternionNormalize(&q, &q);
p = (parentPos + bonePos) * 0.5f;
OBB *obb = new OBB(p, size, q, true);
physicsEngine.GetWorld()->addRigidBody(obb->m_pBody);
m_boxes.push_back(obb);
parent->m_pObb = obb;
parent->m_pivot = obb->SetPivot(parentPos);
return obb;
}
void quater::normalize(const quater& a)
{
#ifdef USE_D3DFUNC
D3DXQuaternionNormalize(*this, a);
#else
mult(a, 1.f/a.length());
#endif
}
void RagDoll::InitBones(Bone *bone) {
if (bone == NULL)
return;
D3DXVECTOR3 sca, pos;
D3DXQUATERNION rot;
D3DXMatrixDecompose(&sca, &rot, &pos, &bone->CombinedTransformationMatrix);
D3DXQuaternionNormalize(&rot, &rot);
bone->m_originalRot = rot;
InitBones((Bone*)bone->pFrameSibling);
InitBones((Bone*)bone->pFrameFirstChild);
}
/*****************************************************************
* D3DXMATRIX Interpolate(D3DXMATRIX _d3dPrevFrame, D3DXMATRIX _d3dNextFrame, float _fLambda):
* Interpolates between two Matrices based on Lambda time
*
* Ins: D3DXMATRIX _d3dPrevFrame
* D3DXMATRIX _d3dNextFrame
* float _fLambda
*
* Outs: void
*
* Returns: D3DXMATRIX
*
* Mod. Date: 02/20/2013
* Mod. Initials: SD
*****************************************************************/
D3DXMATRIX Interpolate(D3DXMATRIX& d3dMatrixA, D3DXMATRIX& d3dMatrixB, float fLambda)
{
D3DXQUATERNION quatA, quatB, tempQuat;
D3DXQuaternionRotationMatrix(&quatA, &d3dMatrixA);
D3DXQuaternionRotationMatrix(&quatB, &d3dMatrixB);
tempQuat = quatA;
D3DXQuaternionNormalize(&quatA, &tempQuat);
tempQuat = quatB;
D3DXQuaternionNormalize(&quatB, &tempQuat);
D3DXQuaternionSlerp(&tempQuat, &quatA, &quatB, fLambda);
D3DXMATRIX result;
D3DXMatrixRotationQuaternion(&result, &tempQuat);
D3DXVec3Lerp((D3DXVECTOR3*)&result[12], (D3DXVECTOR3*)&d3dMatrixA[12], (D3DXVECTOR3*)&d3dMatrixB[12], fLambda);
D3DXVECTOR3 scaleA = D3DXVECTOR3(d3dMatrixA[0],d3dMatrixA[5],d3dMatrixA[10]);
D3DXVECTOR3 scaleB = D3DXVECTOR3(d3dMatrixB[0],d3dMatrixB[5],d3dMatrixB[10]);
D3DXVECTOR3 finalScale;
D3DXVec3Lerp(&finalScale, &scaleA, &scaleB, fLambda);
result[0] = finalScale[0];
result[5] = finalScale[1];
result[10] = finalScale[2];
return result;
}
D3DXQUATERNION ArcBall::QuatFromBallPoints(D3DXVECTOR3& start_point, D3DXVECTOR3& end_point)
{
// Calculate rotate angle
float angle = D3DXVec3Dot(&start_point, &end_point);
// Calculate rotate axis
D3DXVECTOR3 axis;
D3DXVec3Cross(&axis, &start_point, &end_point);
// Build and Normalize the Quaternion
D3DXQUATERNION quat(axis.x, axis.y, axis.z, angle);
D3DXQuaternionNormalize(&quat, &quat);
return quat;
}
void BuildQuaternion ( const vec3_t in_base,
const vec3_t in_vector,
vec4_t out_quat )
{
double theta;
double s, c;
const float error = 0.0001f;
if ( (in_vector [0] + 1.0f) < error )
{
out_quat [0] = 0.0f;
out_quat [1] = 1.0f;
out_quat [2] = 0.0f;
out_quat [3] = 0.0f;
}
else if ( (1.0f - in_vector [0]) < error )
{
out_quat [0] = 0.0f;
out_quat [1] = 0.0f;
out_quat [2] = 0.0f;
out_quat [3] = 1.0f;
}
else
{
VectorCrossProduct ( g_nVector, in_base, in_vector );
D3DXVec3Normalize ( (D3DXVECTOR3 *)g_nVector, (D3DXVECTOR3 *)g_nVector );
theta = acos ( VectorDotProduct ( in_base, in_vector ) / VectorLength ( in_vector ) );
s = sin ( theta / 2 );
c = cos ( theta / 2 );
out_quat [0] = (float)(s * g_nVector [0]);
out_quat [1] = (float)(s * g_nVector [1]);
out_quat [2] = (float)(s * g_nVector [2]);
out_quat [3] = (float)c;
D3DXQuaternionNormalize ( (D3DXQUATERNION *)out_quat, (D3DXQUATERNION *)out_quat );
}
return;
}
void DisplayObject::MoveMesh(){
//D3DXMATRIX matRotateX;
//D3DXMATRIX matRotateY;
D3DXMATRIX matRotate;
D3DXMATRIX matTrans;
D3DXMATRIX matFinal;
D3DXQuaternionNormalize(&_m_rotation, &_m_rotation);
D3DXMatrixRotationQuaternion(&matRotate, &_m_rotation);
//D3DXMatrixRotationX(&matRotateX, rotation.x);
//D3DXMatrixRotationY(&matRotateY, rotation.y);
//D3DXMatrixRotationZ(&matRotateZ, rotation.z);
D3DXMatrixTranslation(&matTrans, _m_pos.x, _m_pos.y, _m_pos.z);
D3DXMatrixMultiply(&matFinal, &matRotate, &matTrans);
//matFinal = matRotate * matTrans;
//matRotate = matRotateZ * matRotateY * matRotateX * matTrans;
m_world = matFinal;
//DXUTGetD3D9Device()->SetTransform(m_world, &(matRotate));
}
bool ActionBox::isPointInside(Vec3 *point)
{
/*if(!(point->y < m_currentMax.y && point->y > m_currentMin.y))
return false;*/
/*point->y = 100.0f;
Vec3 p0 = m_min;
Vec3 p1 = m_max;
Vec3 p2 = Vec3(m_max.x, 0, m_min.z);
Vec3 p2_2 = Vec3(m_min.x, 0, m_max.z);
D3DXVec3TransformCoord(&p0, &p0, &m_world);
D3DXVec3TransformCoord(&p1, &p1, &m_world);
D3DXVec3TransformCoord(&p2, &p2, &m_world);
D3DXVec3TransformCoord(&p2_2, &p2_2, &m_world);
return D3DXIntersectTri(&p0, &p1, &p2, point, &Vec3(0, -1, 0), 0, 0, 0) || D3DXIntersectTri(&p0, &p1, &p2_2, point, &Vec3(0, -1, 0), 0, 0, 0);*/
Vec3 checkPoint = *point;
checkPoint = (*point) - m_position;
Mat temp;
temp = m_world;
D3DXQUATERNION quat;
D3DXQuaternionRotationMatrix(&quat, &temp);
D3DXQuaternionNormalize(&quat, &quat);
D3DXQuaternionInverse(&quat, &quat);
temp._41 = 0;
temp._42 = 0;
temp._43 = 0;
D3DXMatrixRotationQuaternion(&temp, &quat);
D3DXVec3TransformCoord(&checkPoint, &checkPoint, &temp);
if(checkPoint.x > m_min.x && checkPoint.x < m_max.x &&
//checkPoint.y > m_min.y && checkPoint.y < m_max.y &&
checkPoint.z > m_min.z && checkPoint.z < m_max.z)
return true;
return false;
}
void State::Update() {
// TODO: Add decay speed
m_acceleration -= m_posvel * m_friction;
// Apply positional velocity
if(!m_follow) {
m_pos += (m_right * m_posvel.x) * m_time + (1/2) * (m_right * m_acceleration.x) * pow(m_time, 2);
m_pos += (m_up * m_posvel.y) * m_time + (1/2) * (m_up * m_acceleration.y) * pow(m_time, 2);
m_pos += (m_front * m_posvel.z) * m_time + (1/2) * (m_front * m_acceleration.z) * pow(m_time, 2);
} else {
m_pos = *m_follow->GetPosition();
m_pos += (*m_follow->GetRight() * m_offset.x);
m_pos += (*m_follow->GetUp() * m_offset.y);
m_pos += (*m_follow->GetForward() * m_offset.z);
}
m_posvel += m_acceleration * m_time;
D3DXQUATERNION rot;
D3DXQuaternionRotationYawPitchRoll(
&rot,
m_rotvel.x,
m_rotvel.y,
m_rotvel.z
);
if (m_follow)
{
/*D3DXQUATERNION xrot,yrot,zrot;
D3DXQuaternionRotationAxis(&xrot,&m_right,m_rotvel.y);
D3DXQuaternionRotationAxis(&yrot,m_follow->GetUp(),m_rotvel.x);
rot = yrot * xrot;
static D3DXQUATERNION totalRot = rot;
totalRot *= rot;
D3DXQUATERNION temp;
temp = *m_follow->GetRotation();*/
//m_rot = temp * totalRot;
}
else
{
m_rot *= rot;
}
/*
if(m_rotvel_on) {
// Apply rotational velocity
D3DXQUATERNION rot;
D3DXQuaternionRotationYawPitchRoll(
&rot,
m_rotvel.x,
m_rotvel.y,
m_rotvel.z
);
m_rot *= rot;
} else {
D3DXQUATERNION rot, temp;
D3DXQuaternionRotationYawPitchRoll(
&rot,
m_rotvel.x,
m_rotvel.y,
m_rotvel.z
);
temp = *m_follow->GetRotation();
//temp.y = 0;
m_rot = temp;
D3DXQuaternionNormalize(&m_rot, &m_rot);
m_rot = rot * m_rot;
}*/
D3DXQuaternionNormalize(&m_rot, &m_rot);
// Calculate up, front, left
m_up = D3DXVECTOR3(
2 * (m_rot.x * m_rot.y - m_rot.w * m_rot.z),
1 - 2 * (m_rot.x * m_rot.x + m_rot.z * m_rot.z),
2 * (m_rot.y * m_rot.z + m_rot.w * m_rot.x)
);
m_front = D3DXVECTOR3(
2 * (m_rot.x * m_rot.z + m_rot.w * m_rot.y),
2 * (m_rot.y * m_rot.z - m_rot.w * m_rot.x),
1 - 2 * (m_rot.x * m_rot.x + m_rot.y * m_rot.y)
);
m_right = D3DXVECTOR3(
1 - 2 * (m_rot.y * m_rot.y + m_rot.z * m_rot.z),
2 * (m_rot.x * m_rot.y + m_rot.w * m_rot.z),
2 * (m_rot.x * m_rot.z - m_rot.w * m_rot.y)
);;
m_rotvel = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
m_acceleration = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
m_prevRot = m_rot;
}
//----[ buildAnimatedBones ]-------------------------------------------------
void AnimatedMeshRenderer::buildAnimatedBones(
const D3DXMATRIX* root_transform,
AnimatedMeshIndex animated_mesh,
AnimatedMeshAnimationTrack* animation_track,
D3DXMATRIX* bone_matrices) const {
assert(animated_mesh < animated_meshes_.size());
assert(animation_track);
assert(bone_matrices);
const RenderableAnimatedMesh& renderable_animated_mesh
= animated_meshes_.at(animated_mesh);
AnimatedMeshAnimationTrackElement* internal_animation_track =
reinterpret_cast<AnimatedMeshAnimationTrackElement*>(animation_track);
RenderableAnimatedMesh::Frame* frames = renderable_animated_mesh.frames;
size_t number_of_frames = renderable_animated_mesh.number_of_frames;
RenderableAnimatedMesh::Bone* bones = renderable_animated_mesh.bones;
size_t number_of_bones = renderable_animated_mesh.number_of_bones;
// Build the frames using the data in the animation track
D3DXMATRIXA16 matrix;
for (size_t frame_index = 0; frame_index < number_of_frames; ++frame_index) {
RenderableAnimatedMesh::Frame* frame = &frames[frame_index];
AnimatedMeshAnimationTrackElement* track_element
= &internal_animation_track[frame_index];
AnimatedMeshAnimationTrackElement::FrameTransform* frame_transform
= &track_element->frame_transform;
// Build the transform matrix from the scale/rotate/translate settings
//D3DXMatrixTransformation(&matrix,
// NULL,
// NULL,
// frame_transform->scaling(),
// NULL,
// frame_transform->rotation(),
// frame_transform->translation());
// This method of constructing the transform matrices is necessary because
// it allows us to transpose the quaternion's matrix during the build.
D3DXMATRIXA16 cumulative, builder;
D3DXMatrixScaling(&cumulative,
frame_transform->s[0],
frame_transform->s[1],
frame_transform->s[2]);
D3DXQUATERNION quat;
D3DXQuaternionNormalize(&quat, frame_transform->rotation());
D3DXMatrixRotationQuaternion(&builder, &quat);
D3DXMatrixTranspose(&builder, &builder);
D3DXMatrixMultiply(&cumulative, &cumulative, &builder);
D3DXMatrixTranslation(&builder,
frame_transform->t[0],
frame_transform->t[1],
frame_transform->t[2]);
D3DXMatrixMultiply(&matrix, &cumulative, &builder);
// Use the frame hierarchy to construct this frame's final transformation
size_t parent_frame_index = frame->parent_frame_index;
assert(!frame_index || (parent_frame_index < frame_index));
const D3DXMATRIX* parent_frame_matrix
= frame_index == 0 ? root_transform
: internal_animation_track[parent_frame_index]
.frameMatrix();
D3DXMatrixMultiply(track_element->frameMatrix(),
&matrix,
parent_frame_matrix);
}
// Construct the bone transformations for this framte state
for (size_t bone_index = 0; bone_index < number_of_bones; ++bone_index) {
D3DXMATRIX* bone_matrix = bone_matrices + bone_index;
const RenderableAnimatedMesh::Bone* bone = bones + bone_index;
const D3DXMATRIXA16* frame_matrix
= (internal_animation_track + bone->frame_index)->frameMatrix();
D3DXMatrixMultiply(bone_matrix,
&bone->inverse_offset,
frame_matrix);
}
}
void Quaternion::Normalize()
{
D3DXQuaternionNormalize(this, this);
}
