Matrix3x3f Matrix3x3f::IdentityMatrix()
{
Matrix3x3f identity;
identity.SetIdentity();
return identity;
}
void CameraUtility::rotateWorldYInPlace(Single angleDegrees) {
Matrix3x3f rotation;
rotation.setToRotationY(angleDegrees);
Vector3f up = rotation * getUp();
Vector3f forward = rotation * getForward();
ms_gameControllerInstance->m_vmdApp->display->set_eye_dir(&forward[0]);
ms_gameControllerInstance->m_vmdApp->display->set_eye_up(&up[0]);
}
Matrix3x3f
Quaternion::toMatrix () const
{
Matrix3x3f rotation;
rotation.setRight (getRight ());
rotation.setUp (getUp ());
rotation.setBackward (getBackward ());
return (rotation);
}
Matrix3x3f Matrix3x3f::Transpose() const
{
Matrix3x3f transpose;
for(int i = 0; i < 3; i++)
for(int j = 0; j < 3; j++)
transpose.Set(j, i, Get(i, j));
return transpose;
}
Matrix3x3f Matrix3x3f::TranslateMatrix(const Vec2f translation)
{
Matrix3x3f translateMatrix;
translateMatrix.SetIdentity();
translateMatrix.Set(2, 0, translation.x);
translateMatrix.Set(2, 1, translation.y);
return translateMatrix;
}
Matrix3x3f Matrix3x3f::ScaleMatrix(const Vec2f &scale)
{
Matrix3x3f scaleMatrix;
scaleMatrix.SetIdentity();
scaleMatrix.Set(0, 0, scale.x);
scaleMatrix.Set(1, 1, scale.y);
return scaleMatrix;
}
void CameraUtility::rotateWorld(Single angleDegrees, const Vector3f& axis) {
Matrix3x3f rotation;
rotation.setFromAngleAxis(angleDegrees, axis);
Vector3f up = rotation * getUp();
Vector3f forward = rotation * getForward();
Vector3f position = rotation * getPosition();
ms_gameControllerInstance->m_vmdApp->display->set_eye_dir(&forward[0]);
ms_gameControllerInstance->m_vmdApp->display->set_eye_up(&up[0]);
ms_gameControllerInstance->m_vmdApp->display->set_eye_pos(&position[0]);
}
Matrix3x3f Matrix3x3f::RotateMatrix(float angle)
{
float cosOfAngle = cosf(angle);
float sinOfAngle = sinf(angle);
Matrix3x3f rotationMatrix;
rotationMatrix.SetIdentity();
rotationMatrix.Set(0, 0, cosOfAngle);
rotationMatrix.Set(1, 0, sinOfAngle);
rotationMatrix.Set(0, 1, -sinOfAngle);
rotationMatrix.Set(1, 1, cosOfAngle);
return rotationMatrix;
}
void UKFSample::poseSensorUpdate(const Vector3f& reading, const Matrix3x3f& readingCov)
{
generateSigmaPoints();
// computePoseReadings
Vector3f poseReadings[7];
for(int i = 0; i < 7; ++i)
poseReadings[i] = Vector3f(sigmaPoints[i].x, sigmaPoints[i].y, sigmaPoints[i].z);
// computeMeanOfPoseReadings
Vector3f poseReadingMean = poseReadings[0];
for(int i = 1; i < 7; ++i)
poseReadingMean += poseReadings[i];
poseReadingMean *= 1.f / 7.f;
// computeCovOfPoseReadingsAndSigmaPoints
Matrix3x3f poseReadingAndMeanCov;
for(int i = 0; i < 3; ++i)
{
Vector3f d1 = poseReadings[i * 2 + 1] - poseReadingMean;
poseReadingAndMeanCov += Matrix3x3f(d1 * l[i].x, d1 * l[i].y, d1 * l[i].z);
Vector3f d2 = poseReadings[i * 2 + 2] - poseReadingMean;
poseReadingAndMeanCov += Matrix3x3f(d2 * -l[i].x, d2 * -l[i].y, d2 * -l[i].z);
}
poseReadingAndMeanCov *= 0.5f;
// computeCovOfPoseReadingsReadings
Matrix3x3f poseReadingCov;
Vector3f d = poseReadings[0] - poseReadingMean;
poseReadingCov = Matrix3x3f(d * d.x, d * d.y, d * d.z);
for(int i = 1; i < 7; ++i)
{
Vector3f d = poseReadings[i] - poseReadingMean;
poseReadingCov += Matrix3x3f(d * d.x, d * d.y, d * d.z);
}
poseReadingCov *= 0.5f;
poseReadingMean.z = normalize(poseReadingMean.z);
const Matrix3x3f kalmanGain = poseReadingAndMeanCov.transpose() * (poseReadingCov + readingCov).invert();
Vector3f innovation = reading - poseReadingMean;
innovation.z = normalize(innovation.z);
const Vector3f correction = kalmanGain * innovation;
mean += correction;
mean.z = normalize(mean.z);
cov -= kalmanGain * poseReadingAndMeanCov;
}
Matrix3x3f Matrix3x3f::operator*(const Matrix3x3f &other) const
{
Matrix3x3f product;
for(int i = 0; i < 3; i++)
for(int j = 0; j < 3; j++)
{
float sum = 0.0f;
// jth row of this by ith column of other
for(int d = 0; d < 3; d++)
sum += Get(d, j) * other.Get(i, d);
product.Set(i, j, sum);
}
return product;
}
bool Matrix3x3f::operator==(const Matrix3x3f &other) const
{
for(int i = 0; i < 4; i++)
for(int j = 0; j < 4; j++)
{
if(Get(i, j) != other.Get(i, j))
return false;
}
return true;
}
void FlyingLayer::RotationMatrix_VectorToVector(const Vector3f& from, const Vector3f& to, Matrix3x3f &retval) const {
Vector3f fromVec = from.normalized();
Vector3f toVec = to.normalized();
Vector3f axis(fromVec.cross(toVec));
if (axis.squaredNorm() < 1e-10f) {
retval.setIdentity();
} else {
float angle = std::acos(fromVec.dot(toVec));
AngleAxisRotationMatrix(angle, axis.normalized(), retval);
}
}
void FlyingLayer::Update(TimeDelta real_time_delta) {
static const float PERIOD_TRANS = 0.0045f;
static const float PERIOD_ROT = 1.0f;
static const float FILTER = 0.2f;
if (m_Palms.size() > 0) {
Vector3f positionSum = Vector3f::Zero();
Vector3f rotationAASum = Vector3f::Zero();
for (int i = 0; i < m_Palms.size(); i++) {
positionSum += m_GridOrientation.block<3, 3>(0, 0).transpose()*(m_Palms[i] - m_EyePos - m_EyeView.transpose()*Vector3f(0, -0.15, -0.05));
//rotationAASum += RotationMatrixToVector(m_EyeView.transpose()*m_PalmOrientations[i]*m_EyeView.transpose());
Matrix3x3f rot;
RotationMatrix_VectorToVector(-Vector3f::UnitZ(), m_EyeView*(m_Palms[i] - m_EyePos) - Vector3f(0, -0.15, -0.05), rot);
//std::cout << __LINE__ << ":\t rot = " << (rot) << std::endl;
rotationAASum += RotationMatrixToVector(rot);
}
if (m_Palms.size() == 2) {
const Vector3f dir0 = m_EyeView*(m_Palms[0] - m_EyePos) - Vector3f(0, -0.15, -0.05);
const Vector3f dir1 = m_EyeView*(m_Palms[1] - m_EyePos) - Vector3f(0, -0.15, -0.05);
Matrix3x3f rot;
RotationMatrix_VectorToVector((dir0.x() < dir1.x() ? 1.0f : -1.0f) * Vector3f::UnitX(), dir1 - dir0, rot);
//std::cout << __LINE__ << ":\t positionSum = " << (positionSum) << std::endl;
rotationAASum += 2.0f*RotationMatrixToVector(rot);
}
m_Velocity = (1 - FILTER)*m_Velocity + FILTER*positionSum/m_Palms.size();
m_RotationAA = (1 - FILTER)*m_RotationAA + FILTER*rotationAASum/m_Palms.size();
} else {
m_Velocity = (1 - 0.3f*FILTER)*m_Velocity;
m_RotationAA = (1 - 0.3f*FILTER)*m_RotationAA;
}
m_GridCenter -= m_Velocity*m_Velocity.squaredNorm()*(real_time_delta/PERIOD_TRANS);
const Matrix3x3f rot = RotationVectorToMatrix((real_time_delta/PERIOD_ROT)*m_RotationAA*m_RotationAA.squaredNorm());
//std::cout << __LINE__ << ":\t rot = " << (rot) << std::endl;
//Matrix3x3f foo = ;
m_GridOrientation.block<3, 3>(0, 0) = m_EyeView.transpose()*rot.transpose()*m_EyeView*m_GridOrientation.block<3, 3>(0, 0);
}
Vector3f FlyingLayer::RotationMatrixToVector(const Matrix3x3f& rotationMatrix) const {
static const float epsilon = 1e-6;
const float cs = (rotationMatrix.trace() - 1.0)*0.5;
if (cs > 1.0 - epsilon) {
return Vector3f::Zero();
} else if (cs < epsilon - 1.0) {
Eigen::SelfAdjointEigenSolver<Matrix3x3f> evals(rotationMatrix, Eigen::ComputeEigenvectors);
Vector3f rotVector = evals.eigenvectors().col(2).transpose();
return rotVector.normalized()*M_PI;
} else {
const float sn = std::sqrt(1.0 - cs*cs);
const float angle = std::acos(cs);
const float multiplier = angle * 0.5 / sn;
return Vector3f((rotationMatrix(2, 1) - rotationMatrix(1, 2))*multiplier,
(rotationMatrix(0, 2) - rotationMatrix(2, 0))*multiplier,
(rotationMatrix(1, 0) - rotationMatrix(0, 1))*multiplier);
}
}
bool UndistortAndRectify::initUndistortRectifyMap(Matrix3x3f K, float k1, float k2, float k3, float p1, float p2,
Matrix3x3f Knew, int width, int height)
{
// distorted camera matrix
float cX = K.at(0,2),
cY = K.at(1,2),
fX = K.at(0,0),
fY = K.at(1,1);
// Optimal camera matrix
float cX_ = Knew.at(0,2),
cY_ = Knew.at(1,2),
fX_ = Knew.at(0,0),
fY_ = Knew.at(1,1);
if(mapX == NULL)
delete[] mapX;
if(mapY == NULL)
delete[] mapY;
mapX = new float[width*height];
mapY = new float[width*height];
float* mapXPtr = mapX;
float* mapYPtr = mapY;
float xU,yU, // Undistorted coordinates
xD,yD, // Distorted coordinates
rU2,xU2,yU2,xyU2;
for (int y = 0; y < height; y++) {
for (int x = 0; x < width; x++) {
xU = (x - cX_)/fX_;
yU = (y - cY_)/fY_;
xU2 = xU*xU;
yU2 = yU*yU;
xyU2 = 2*xU*yU;
rU2 = xU2 + yU2;
// Apply distortion function
xD = xU*(1 + (k1 + (k2 + k3*rU2)*rU2)*rU2) + p1*xyU2 + p2*(rU2 + 2*xU2);
yD = yU*(1 + (k1 + (k2 + k3*rU2)*rU2)*rU2) + p2*xyU2 + p1*(rU2 + 2*yU2);
// Reprojected x
*mapXPtr++ = xD*fX + cX;
// Reprojected y
*mapYPtr++ = yD*fY + cY;
}
}
return true;
}
Matrix3x3f FlyingLayer::RotationMatrixLinearInterpolation(const Matrix3x3f& A0, const Matrix3x3f& A1, float t) const {
Vector3f dA = RotationMatrixToVector(A0.transpose()*A1);
float angle = std::fmod(t*dA.norm(), M_PI);
Matrix3x3f At = A0*RotationVectorToMatrix(angle*dA.normalized());
return At;
}
void
Quaternion::setFromMatrix (const float* const matrix4x4)
{
Matrix3x3f rotation (matrix4x4);
*this = rotation.toQuaternion ();
}
void
Quaternion::getAsArray (float array[16]) const
{
Matrix3x3f rotation = toMatrix ();
rotation.getAsArray (array);
}
void AIRacer::apply_controls(double deltaTime)
{
if (!m_entity) return;
Vector3f ForceContainer;
TEXEngine::Physics::VehicleObject* SledObj = static_cast<VehicleObject*>(m_entity->physics_obj());
if (!SledObj) return;
// AI CODE
////////////////////////////
if(m_pack.thrust() < 0.4f)
m_pack.apply_whip();
if(m_pack.edm().x < 0.4f)
m_pack.apply_treat();
if(m_pack.edm().y < 0.7f)
m_pack.apply_whip();
// STEERING
if (!m_lastWaypointObject || !m_nextWaypointObject ) return;
Vector3f r2n = m_nextWaypointObject->get_pos() - SledObj->get_pos();
r2n.normalize();
Quaternion q = SledObj->get_quaternion();
Vector3f v = Vector3f(1.0f,0.0f,0.0f);
Vector3f ori = v + cross( Vector3f(q.x,q.y,q.z) , cross( Vector3f(q.x,q.y,q.z) , v )+( v * q.w ) )*2.0f;
//Vector3f ori = SledObj->get_front_vector();
Vector3f ori3f = Vector3f(ori.x,0.0f,ori.z).normalized();
Matrix3x3f rotmat;
rotmat.load_identity();
rotmat.rotate(Vector3f(0.0f,1.0f,0.0f),1.57f);
ori3f = rotmat * ori3f;
m_steerValue = TEXEngine::Math::dot(r2n,ori3f);
// BRAKING??
/*
m_brakeValue = max(0.0f, m_steerValue - 0.8f) * 5.0f;
if ( m_brakeValue > 0.0f )
SledObj->brake(100.0f);
else
SledObj->not_braking();*/
// APPLY FORCES
if(m_entity)
{
TEXEngine::Physics::VehicleObject* po = static_cast<VehicleObject*>(m_entity->physics_obj());
if(po)
{
// THRUST
if(m_pack.thrust() > 0)
po->accelerate(800.0f*m_pack.thrust());
else
po->not_accelerating();
// STEERING
if(!m_is_easy)
{
if(m_steerValue > 0.1)
po->turn_right(po->get_steer_inc());
else if(m_steerValue < -0.1)
po->turn_left(po->get_steer_inc());
else
po->not_turning();
}
else
{
if(abs(m_pack.deviation()) < 0.1f && abs(m_steerValue) < 0.0001f)
po->not_turning();
else
{
if(m_steerValue > 0.1f)
po->turn_right(po->get_steer_inc() - m_pack.deviation()*(po->get_steer_inc()));
else if(m_steerValue < -0.1f)
po->turn_left(po->get_steer_inc() + m_pack.deviation()*(po->get_steer_inc()));
else
{
if(m_pack.deviation() > 0.1f)
po->turn_left(m_pack.deviation()*(po->get_steer_inc()));
else if(m_pack.deviation() < -0.1f)
po->turn_right(-m_pack.deviation()*(po->get_steer_inc()));
}
}
}
m_pack.speed((float)Vector2f((scalar_t)po->get_velocity().x, (scalar_t)po->get_velocity().z).length());
}
}
}