const bool Test_Matrix_RotationMatrix()
{
const TVector3d kAxis_d = Normalize(TVector3d(), TVector3d{1.0, 1.0, 1.0});
const double kdAngle = s_kdTau / 8.0;
const TVector4d kQuatOrient_d = AxisAngleQuaternion(TVector4d(), kAxis_d, kdAngle);
const TMatrix4d kOrientation_d = RotationMatrix(TMatrix4d(), kQuatOrient_d);
const TVector4d kTransformedA_d = VectorMultiply(TVector4d(), kOrientation_d, TVector4d{1.0, 0.0, 0.0, 1.0});
const TVector3d kTransformedB_d{kTransformedA_d.m_dX, kTransformedA_d.m_dY, kTransformedA_d.m_dZ};
const TVector3d kTransformedC_d = QuaternionRotate(TVector3d(), TVector3d{1.0, 0.0, 0.0}, kQuatOrient_d);
const bool kbPass_d = Equal(kTransformedB_d, kTransformedC_d, s_kdEpsilon);
const TVector3f kAxis_f = Normalize(TVector3f(), TVector3f{1.0f, 1.0f, 1.0f});
const float kfAngle = s_kfTau / 8.0f;
const TVector4f kQuatOrient_f = AxisAngleQuaternion(TVector4f(), kAxis_f, kfAngle);
const TMatrix4f kOrientation_f = RotationMatrix(TMatrix4f(), kQuatOrient_f);
const TVector4f kTransformedA_f = VectorMultiply(TVector4f(), kOrientation_f, TVector4f{1.0f, 0.0f, 0.0f, 1.0f});
const TVector3f kTransformedB_f{kTransformedA_f.m_fX, kTransformedA_f.m_fY, kTransformedA_f.m_fZ};
const TVector3f kTransformedC_f = QuaternionRotate(TVector3f(), TVector3f{1.0f, 0.0f, 0.0f}, kQuatOrient_f);
const bool kbPass_f = Equal(kTransformedB_f, kTransformedC_f, s_kfEpsilon);
return(kbPass_d && kbPass_f);
}
void CCharShape::AdjustOrientation (CControl *ctrl, bool eps,
ETR_DOUBLE dist_from_surface, const TVector3d& surf_nml) {
TVector3d new_y, new_z;
static const TVector3d minus_z_vec(0, 0, -1);
static const TVector3d y_vec(0, 1, 0);
if (dist_from_surface > 0) {
new_y = ctrl->cvel;
new_y.Norm();
new_z = ProjectToPlane (new_y, TVector3d(0, -1, 0));
new_z.Norm();
new_z = AdjustRollvector (ctrl, ctrl->cvel, new_z);
} else {
new_z = -1.0 * surf_nml;
new_z = AdjustRollvector (ctrl, ctrl->cvel, new_z);
new_y = ProjectToPlane (surf_nml, ctrl->cvel);
new_y.Norm();
}
TVector3d new_x = CrossProduct (new_y, new_z);
TMatrix<4, 4> cob_mat(new_x, new_y, new_z);
TQuaternion new_orient = MakeQuaternionFromMatrix (cob_mat);
if (!ctrl->orientation_initialized) {
ctrl->orientation_initialized = true;
ctrl->corientation = new_orient;
}
ETR_DOUBLE time_constant = dist_from_surface > 0 ? TO_AIR_TIME : TO_TIME;
float dtime = eps ? EPS : g_game.time_step;
ctrl->corientation = InterpolateQuaternions (
ctrl->corientation, new_orient,
min (dtime / time_constant, 1.0));
ctrl->plane_nml = RotateVector (ctrl->corientation, minus_z_vec);
ctrl->cdirection = RotateVector (ctrl->corientation, y_vec);
cob_mat = MakeMatrixFromQuaternion(ctrl->corientation);
// Trick rotations
new_y = TVector3d (cob_mat[1][0], cob_mat[1][1], cob_mat[1][2]);
TMatrix<4, 4> rot_mat = RotateAboutVectorMatrix(new_y, (ctrl->roll_factor * 360));
cob_mat = rot_mat * cob_mat;
new_x = TVector3d (cob_mat[0][0], cob_mat[0][1], cob_mat[0][2]);
rot_mat = RotateAboutVectorMatrix (new_x, ctrl->flip_factor * 360);
cob_mat = rot_mat * cob_mat;
TransformNode (0, cob_mat, cob_mat.GetTransposed());
}
const TVector3d& VectorTripleProduct( TVector3d& _rResult,
const TVector3d& _krA,
const TVector3d& _krB,
const TVector3d& _krC)
{
return(CrossProduct(_rResult, _krA, CrossProduct(TVector3d(), _krB, _krC)));
}
const double Distance(const TVector3d& _krA,
const TVector3d& _krB)
{
const double kdDistance = Magnitude(Subtract(TVector3d(), _krA, _krB));
return(kdDistance);
}
const bool Test_Matrix_VectorMultiply()
{
const TMatrix4d kA4d{ 2.0, 0.0, 0.0, 0.0,
0.0, 3.0, 0.0, 0.0,
0.0, 0.0, 4.0, 0.0,
0.0, 0.0, 0.0, 5.0};
const TVector4d kB4d{ 1.0, 1.0, 1.0, 1.0 };
const TVector4d kC4d{ 2.0, 3.0, 4.0, 5.0 };
const TMatrix4f kA4f{ 2.0f, 0.0f, 0.0f, 0.0f,
0.0f, 3.0f, 0.0f, 0.0f,
0.0f, 0.0f, 4.0f, 0.0f,
0.0f, 0.0f, 0.0f, 5.0f};
const TVector4f kB4f{ 1.0f, 1.0f, 1.0f, 1.0f };
const TVector4f kC4f{ 2.0f, 3.0f, 4.0f, 5.0f };
const TMatrix3d kA3d{ 2.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 4.0};
const TVector3d kB3d{ 1.0, 1.0, 1.0 };
const TVector3d kC3d{ 2.0, 3.0, 4.0 };
const TMatrix3f kA3f{ 2.0f, 0.0f, 0.0f,
0.0f, 3.0f, 0.0f,
0.0f, 0.0f, 4.0f};
const TVector3f kB3f{ 1.0f, 1.0f, 1.0f };
const TVector3f kC3f{ 2.0f, 3.0f, 4.0f };
const TMatrix2d kA2d{ 2.0, 0.0,
0.0, 3.0};
const TVector2d kB2d{ 1.0, 1.0 };
const TVector2d kC2d{ 2.0, 3.0 };
const TMatrix2f kA2f{ 2.0f, 0.0f,
0.0f, 3.0f};
const TVector2f kB2f{ 1.0f, 1.0f };
const TVector2f kC2f{ 2.0f, 3.0f };
const bool kbPass4d = Equal(kC4d, VectorMultiply(TVector4d(), kA4d, kB4d), s_kdEpsilon);
const bool kbPass4f = Equal(kC4f, VectorMultiply(TVector4f(), kA4f, kB4f), s_kfEpsilon);
const bool kbPass3d = Equal(kC3d, VectorMultiply(TVector3d(), kA3d, kB3d), s_kdEpsilon);
const bool kbPass3f = Equal(kC3f, VectorMultiply(TVector3f(), kA3f, kB3f), s_kfEpsilon);
const bool kbPass2d = Equal(kC2d, VectorMultiply(TVector2d(), kA2d, kB2d), s_kdEpsilon);
const bool kbPass2f = Equal(kC2f, VectorMultiply(TVector2f(), kA2f, kB2f), s_kfEpsilon);
return( kbPass4d
&& kbPass4f
&& kbPass3d
&& kbPass3f
&& kbPass2d
&& kbPass2f);
}
void srTSpherMirror::SetupInAndOutPlanes(TVector3d* InPlane, TVector3d* OutPlane)
{// Assumes InPlaneNorm and transverse part InPlaneCenPo already defined !!!
TVector3d &InPlaneCenPo = *InPlane, &InPlaneNorm = InPlane[1];
TVector3d &OutPlaneCenPo = *OutPlane, &OutPlaneNorm = OutPlane[1];
TVector3d LocInPlaneNorm = InPlaneNorm;
FromLabToLocFrame_Vector(LocInPlaneNorm);
double xP = -0.5*Dx, yP = -0.5*Dy;
TVector3d P0(xP, yP, SurfaceFunction(xP, yP, 0));
TVector3d P1(-xP, yP, SurfaceFunction(-xP, yP, 0));
TVector3d P2(xP, -yP, SurfaceFunction(xP, -yP, 0));
TVector3d P3(-xP, -yP, SurfaceFunction(-xP, -yP, 0));
TVector3d EdgePoints[] = { P0, P1, P2, P3 };
int LowestInd, UppestInd;
FindLowestAndUppestPoints(LocInPlaneNorm, EdgePoints, 4, LowestInd, UppestInd);
TVector3d PointForInPlane = EdgePoints[LowestInd];
FromLocToLabFrame_Point(PointForInPlane);
InPlaneCenPo.y = PointForInPlane.y;
TVector3d LocInPlaneCenPo = InPlaneCenPo;
FromLabToLocFrame_Point(LocInPlaneCenPo);
TVector3d LocInPlane[] = { LocInPlaneCenPo, LocInPlaneNorm };
TVector3d LocP, LocN;
FindRayIntersectWithSurface(LocInPlane, 0, LocP);
SurfaceNormalAtPoint(LocP.x, LocP.y, 0, LocN);
TVector3d LocOutPlaneNorm = LocInPlaneNorm;
ReflectVect(LocN, LocOutPlaneNorm);
FindLowestAndUppestPoints(LocOutPlaneNorm, EdgePoints, 4, LowestInd, UppestInd);
OutPlaneCenPo = (EdgePoints[UppestInd]*LocOutPlaneNorm)*LocOutPlaneNorm;
*OutPlaneInLocFrame = OutPlaneCenPo; OutPlaneInLocFrame[1] = LocOutPlaneNorm;
FromLocToLabFrame_Point(OutPlaneCenPo);
OutPlaneNorm = LocOutPlaneNorm;
FromLocToLabFrame_Vector(OutPlaneNorm);
TVector3d LabN = LocN;
FromLocToLabFrame_Vector(LabN);
ExRefInLabFrameBeforeProp = TVector3d(1.,0.,0.);
ReflectVect(LabN, ExRefInLabFrameBeforeProp);
EzRefInLabFrameBeforeProp = TVector3d(0.,0.,1.);
ReflectVect(LabN, EzRefInLabFrameBeforeProp);
}
const bool Test_Matrix_TransformationMatrix()
{
const TVector3d kTranslate_d{1.0, 2.0, 3.0};
const TVector3d kAxis_d = Normalize(TVector3d(), TVector3d{1.0, 1.0, 1.0});
const double kdAngle = s_kdTau / 8.0;
const TVector4d kQuatOrient_d = AxisAngleQuaternion(TVector4d(), kAxis_d, kdAngle);
const double kdScale = 2.0;
const TMatrix4d kTranslation_d = TranslationMatrix(TMatrix4d(), kTranslate_d);
const TMatrix4d kOrientation_d = RotationMatrix(TMatrix4d(), kQuatOrient_d);
const TMatrix4d kScaling_d = ScalingMatrix(TMatrix4d(), kdScale, kdScale, kdScale);
const TMatrix4d kTransformA_d = Multiply(TMatrix4d(), kScaling_d, kOrientation_d);
const TMatrix4d kTransformB_d = Multiply(TMatrix4d(), kTranslation_d, kTransformA_d);
const TVector3d kBasisX_d = QuaternionRotate(TVector3d(), TVector3d{kdScale, 0.0, 0.0}, kQuatOrient_d);
const TVector3d kBasisY_d = QuaternionRotate(TVector3d(), TVector3d{0.0, kdScale, 0.0}, kQuatOrient_d);
const TVector3d kBasisZ_d = QuaternionRotate(TVector3d(), TVector3d{0.0, 0.0, kdScale}, kQuatOrient_d);
const TMatrix4d kTransformC_d = TransformationMatrix(TMatrix4d(), kBasisX_d, kBasisY_d, kBasisZ_d, kTranslate_d);
const bool kbPass_d = Equal(kTransformC_d, kTransformB_d, s_kdEpsilon);
//
const TVector3f kTranslate_f{1.0f, 2.0f, 3.0f};
const TVector3f kAxis_f = Normalize(TVector3f(), TVector3f{1.0f, 1.0f, 1.0f});
const float kfAngle = s_kfTau / 8.0f;
const TVector4f kQuatOrient_f = AxisAngleQuaternion(TVector4f(), kAxis_f, kfAngle);
const float kfScale = 2.0f;
const TMatrix4f kTranslation_f = TranslationMatrix(TMatrix4f(), kTranslate_f);
const TMatrix4f kOrientation_f = RotationMatrix(TMatrix4f(), kQuatOrient_f);
const TMatrix4f kScaling_f = ScalingMatrix(TMatrix4f(), kfScale, kfScale, kfScale);
const TMatrix4f kTransformA_f = Multiply(TMatrix4f(), kScaling_f, kOrientation_f);
const TMatrix4f kTransformB_f = Multiply(TMatrix4f(), kTranslation_f, kTransformA_f);
const TVector3f kBasisX_f = QuaternionRotate(TVector3f(), TVector3f{kfScale, 0.0f, 0.0f}, kQuatOrient_f);
const TVector3f kBasisY_f = QuaternionRotate(TVector3f(), TVector3f{0.0f, kfScale, 0.0f}, kQuatOrient_f);
const TVector3f kBasisZ_f = QuaternionRotate(TVector3f(), TVector3f{0.0f, 0.0f, kfScale}, kQuatOrient_f);
const TMatrix4f kTransformC_f = TransformationMatrix(TMatrix4f(), kBasisX_f, kBasisY_f, kBasisZ_f, kTranslate_f);
const bool kbPass_f = Equal(kTransformC_f, kTransformB_f, s_kfEpsilon);
return(kbPass_d && kbPass_f);
}
const TVector3d& Projection( TVector3d& _rResult,
const TVector3d& _krA,
const TVector3d& _krB)
{
const double kdDenom = Square(Magnitude(_krB));
const TVector3d kNumer = ScalarMultiply(TVector3d(), _krB, DotProduct(_krA, _krB));
_rResult = ScalarMultiply(_rResult, kNumer, kdDenom);
return(_rResult);
}
void CKeyframe::Update(float timestep) {
if (!loaded) return;
if (!active) return;
keytime += timestep;
if (keytime >= frames[keyidx].val[0]) {
keyidx++;
keytime = 0;
}
if (keyidx >= frames.size()-1 || frames.size() < 2) {
active = false;
return;
}
double frac;
TVector3d pos;
CCharShape *shape = g_game.character->shape;
if (std::fabs(frames[keyidx].val[0]) < 0.0001) frac = 1.0;
else frac = (frames[keyidx].val[0] - keytime) / frames[keyidx].val[0];
pos.x = interp(frac, frames[keyidx].val[1], frames[keyidx+1].val[1]) + refpos.x;
pos.z = interp(frac, frames[keyidx].val[3], frames[keyidx+1].val[3]) + refpos.z;
pos.y = interp(frac, frames[keyidx].val[2], frames[keyidx+1].val[2]);
pos.y += Course.FindYCoord(pos.x, pos.z);
shape->ResetRoot();
shape->ResetJoints();
g_game.player->ctrl->cpos = pos;
double disp_y = pos.y + TUX_Y_CORR + heightcorr;
shape->ResetNode(0);
shape->TranslateNode(0, TVector3d(pos.x, disp_y, pos.z));
InterpolateKeyframe(keyidx, frac, shape);
}
const double ScalarTripleProduct( const TVector3d& _krA,
const TVector3d& _krB,
const TVector3d& _krC)
{
return(DotProduct(_krA, CrossProduct(TVector3d(), _krB, _krC)));
}
bool CCharShape::Collision (const TVector3d& pos, const TPolyhedron& ph) {
ResetNode (0);
TranslateNode (0, TVector3d (pos.x, pos.y, pos.z));
return CheckCollision (ph);
}
bool CCharShape::Load (const string& dir, const string& filename, bool with_actions) {
CSPList list (500);
useActions = with_actions;
CreateRootNode ();
newActions = true;
if (!list.Load (dir, filename)) {
Message ("could not load character", filename);
return false;
}
for (size_t i=0; i<list.Count(); i++) {
const string& line = list.Line(i);
int node_name = SPIntN (line, "node", -1);
int parent_name = SPIntN (line, "par", -1);
string mat_name = SPStrN (line, "mat");
string name = SPStrN (line, "joint");
string fullname = SPStrN (line, "name");
if (SPIntN (line, "material", 0) > 0) {
CreateMaterial (line);
} else {
float visible = SPFloatN (line, "vis", -1.0);
bool shadow = SPBoolN (line, "shad", false);
string order = SPStrN (line, "order");
CreateCharNode (parent_name, node_name, name, fullname, order, shadow);
TVector3d rot = SPVector3d(line, "rot");
MaterialNode (node_name, mat_name);
for (size_t ii = 0; ii < order.size(); ii++) {
int act = order[ii]-48;
switch (act) {
case 0: {
TVector3d trans = SPVector3d(line, "trans");
TranslateNode (node_name, trans);
break;
}
case 1:
RotateNode (node_name, 1, rot.x);
break;
case 2:
RotateNode (node_name, 2, rot.y);
break;
case 3:
RotateNode (node_name, 3, rot.z);
break;
case 4: {
TVector3d scale = SPVector3(line, "scale", TVector3d(1, 1, 1));
ScaleNode (node_name, scale);
break;
}
case 5:
VisibleNode (node_name, visible);
break;
case 9:
RotateNode (node_name, 2, rot.z);
break;
default:
break;
}
}
}
}
newActions = false;
return true;
}
