void kalman_2D_update(kalman_filter_2D_t *kalman, vector_2_t measurement)
{
vector_2_t innovation = vsub2( measurement,
mvmul2(kalman->observation_model, kalman->state));
matrix_2x2_t innovation_covariance = madd2( mmul2( mmul2(kalman->observation_model, kalman->covariance),
trans2(kalman->observation_model)),
kalman->noise_measurement);
matrix_2x2_t kalman_gain = mmul2( mmul2(kalman->covariance, trans2(kalman->observation_model)),
inv2(innovation_covariance));
kalman->state = vadd2(kalman->state, mvmul2(kalman_gain, innovation));
kalman->covariance = mmul2( msub2(ident_2x2, mmul2(kalman_gain, kalman->observation_model)),
kalman->covariance);
}
void kalman_2D_prediction(kalman_filter_2D_t *kalman, vector_2_t control)
{
kalman->state = vadd2( mvmul2(kalman->system_model, kalman->state),
mvmul2(kalman->control_model, control));
kalman->covariance= madd2( mmul2( mmul2( kalman->system_model,
kalman->covariance),
trans2(kalman->system_model)),
kalman->noise_prediction);
}
//-------------------------------------------------------------------------------
bool Crate::isPlaceFree(const Ogre::Vector3 &dir, bool ignorePlayer)
{
//The cast result callback.
struct CrateCheckResult : public btDynamicsWorld::ConvexResultCallback
{
btCollisionObject *mIgnore;
int mDimension;
bool mHit, mIgnorePlayer;
CrateCheckResult(btCollisionObject *ignore, int dimension, bool ignorePlayer)
: mIgnore(ignore),
mDimension(dimension),
mHit(false),
mIgnorePlayer(ignorePlayer)
{
}
btScalar addSingleResult(btDynamicsWorld::LocalConvexResult &convexResult, bool)
{
mHit = true;
return convexResult.m_hitFraction;
}
bool needsCollision(btBroadphaseProxy* proxy0) const
{
//If it's us, or isn't in our dimension, we don't care.
return ((btCollisionObject*) proxy0->m_clientObject != mIgnore)
&& !(proxy0->m_collisionFilterGroup & DimensionManager::NO_CRATE_CHECK)
&& !(mIgnorePlayer && proxy0->m_collisionFilterGroup & DimensionManager::PLAYER)
&& (proxy0->m_collisionFilterGroup & mDimension);
//&& !(((btCollisionObject*) proxy0->m_clientObject)->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE);
}
};
//Where to cast from, where to cast to, etc.
btTransform myTrans;
mBody->getMotionState()->getWorldTransform(myTrans);
btVector3 pos1 = myTrans.getOrigin();
btVector3 pos2 = myTrans.getOrigin() + BtOgre::Convert::toBullet(dir);
btQuaternion rot = myTrans.getRotation();
btTransform trans1(rot, pos1);
btTransform trans2(rot, pos2);
//Do the cast.
CrateCheckResult res(mBody, mDimensions, ignorePlayer);
GlbVar.phyWorld->convexSweepTest(mCastShape, trans1, trans2, res);
//If not hit, return true.
return !(res.mHit);
}
TEST( utility, Atelier_TransformXTransform)
{
Eigen::Matrix4f m1;
m1 << 1, 2, 3, 4,
5, 6, 7, 8,
9, 8, 5, 1,
0, 0, 0, 1;
//std::cout << "Here is the Matrix4f m: \n" << m4x4 << std::endl;
ATELIER::UTILITY::Transform trans1(m1);
ATELIER::UTILITY::Transform trans2(Eigen::Matrix4f::Identity());
Timer t;
t.start();
for(unsigned int i = 0; i < COUNT; ++i) {
trans1 = trans1 * trans2;
}
double cost = t.getElapsedTimeInMilliSec();
std::cout << "Atelier_TransformXTransform: " << cost <<"ms"<< std::endl;
std::cout << "Result: \n" << trans1.getMatrix() << std::endl;
EXPECT_EQ( true, true);
}
TEST( utility, Eigen_TransformXTransform)
{
Eigen::Matrix4f m1;
m1 << 1, 2, 3, 4,
5, 6, 7, 8,
9, 8, 5, 1,
0, 0, 0, 1;
Eigen::Affine3f trans1(m1);
Eigen::Affine3f trans2(Eigen::Matrix4f::Identity());
Timer t;
t.start();
for(unsigned int i = 0; i < COUNT; ++i) {
trans1 = trans1 * trans2;
}
double cost = t.getElapsedTimeInMilliSec();
std::cout << "Eigen_TransformXTransform: " << cost <<"ms"<< std::endl;
Eigen::Matrix4f result = trans1.matrix();
std::cout << "Result: \n" << result << std::endl;
EXPECT_EQ( true, true);
}
* Test functions for small matrix
*/
#include "test_small_matrix.h"
#include "small_matrix.h"
#include "linear_algebra.h"
#include "print_util.h"
bool run_matrix_tests(void)
{
matrix_2x2_t m2={.v={{1, 2},{3,2}}};
print_util_print_matrix(print_util_get_debug_stream(), m2.v[0], 2, 2, 2); print_util_dbg_print("\n");
matrix_2x2_t m2t=trans2(m2);
print_util_print_matrix(print_util_get_debug_stream(), m2t.v[0], 2, 2, 2); print_util_dbg_print("\n");
matrix_2x2_t m2i=inv2(m2);
print_util_print_matrix(print_util_get_debug_stream(), m2i.v[0], 2, 2, 2); print_util_dbg_print("\n");
print_util_print_matrix(print_util_get_debug_stream(), madd2(m2, m2t).v[0], 2, 2, 2); print_util_dbg_print("\n");
matrix_2x2_t m2_=mmul2(m2, m2i);
print_util_print_matrix(print_util_get_debug_stream(), m2_.v[0], 2, 2, 2); print_util_dbg_print("\n");
matrix_3x3_t m3={.v={{1, 2, 4},{2,4,2},{4,2,1}}};
print_util_print_matrix(print_util_get_debug_stream(), m3.v[0], 3, 3, 2); print_util_dbg_print("\n");
matrix_3x3_t m3d=diag_3x3(row3(m3, 0));
//-------------------------------------------------------------------------------
void SlidingBrush::unpausedTick(const Ogre::FrameEvent &evt)
{
GraLL2GameObject::unpausedTick(evt);
if (mEnabled)
{
//Get old place (current place actually).
btTransform oldTrans;
mBody->getMotionState()->getWorldTransform(oldTrans);
btVector3 prevPos = oldTrans.getOrigin();
//Find next point position, and velocity.
int currPlace = mForward ? mCurrentPlace + 0.5 : mCurrentPlace -0.5;
Ogre::Vector3 currPoint = mPoints[currPlace];
Ogre::Vector3 velocity = (currPoint - BtOgre::Convert::toOgre(prevPos)).normalisedCopy() * mSpeed;
//Check if we're near next point.
Ogre::Real sqSpeed = mSpeed * mSpeed * evt.timeSinceLastFrame * evt.timeSinceLastFrame;
Ogre::Real sqDist = (currPoint - BtOgre::Convert::toOgre(prevPos)).squaredLength();
if (sqDist < sqSpeed)
{
//If near, jump to it.
mBody->getMotionState()->setWorldTransform(btTransform(oldTrans.getRotation(), BtOgre::Convert::toBullet(currPoint)));
mNode->setPosition(currPoint);
//If next 'place' exists, get there..
unsigned int nextPlace = currPlace + (mForward ? 1 : -1);
if (nextPlace >= 0 && nextPlace < mPoints.size())
mCurrentPlace += mForward ? 1 : -1;
//Call the Python 'point' event. Cut out repeated events using a 'last place' idea.
if (mLastPlace != currPlace)
NGF_PY_CALL_EVENT(point, currPlace);
mLastPlace = currPlace;
}
else
{
//Otherwise, usual velocity movement.
bool canMove = true;
btVector3 currVel = BtOgre::Convert::toBullet(velocity) * evt.timeSinceLastFrame;
btVector3 newPos = prevPos + currVel;
//If we're not ignoring collisions, check for 'em.
if (!mIgnoreCollisions)
{
//The cast result callback.
struct SlidingBrushCheckResult : public btDynamicsWorld::ConvexResultCallback
{
btCollisionObject *mIgnore;
int mDimension;
bool mHit;
Ogre::Real mHitFraction;
bool mYCast;
SlidingBrushCheckResult(btCollisionObject *ignore, int dimension, bool yCast)
: mIgnore(ignore),
mDimension(dimension),
mHit(false),
mHitFraction(1),
mYCast(yCast)
{
}
btScalar addSingleResult(btDynamicsWorld::LocalConvexResult &convexResult, bool)
{
mHit = true;
mHitFraction = convexResult.m_hitFraction < mHitFraction ? convexResult.m_hitFraction : mHitFraction;
return convexResult.m_hitFraction;
}
bool needsCollision(btBroadphaseProxy* proxy0) const
{
return ((btCollisionObject*) proxy0->m_clientObject != mIgnore) //Shouldn't be us.
&& (proxy0->m_collisionFilterGroup & mDimension) //Should be in our dimension.
&& (!(proxy0->m_collisionFilterGroup & DimensionManager::STATIC)) //No need to check with static.
&& !(mYCast && (proxy0->m_collisionFilterGroup & DimensionManager::LIFTABLE)) //If moving up, forget the liftables.
&& !(((btCollisionObject*) proxy0->m_clientObject)->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE); //If no contact response, ignore.
}
};
//Where to cast from, where to cast to, etc.
btVector3 normVel = currVel.normalized();
btVector3 pos1 = prevPos;
btVector3 pos2 = prevPos + currVel + (normVel * CAST_SHAPE_SHRINK);
btQuaternion rot = mBody->getWorldTransform().getRotation();
btTransform trans1(rot, pos1);
btTransform trans2(rot, pos2);
//Do the cast.
SlidingBrushCheckResult res(mBody, mDimensions, GlbVar.gravMgr->isUp() ? pos1.y() < pos2.y() : pos1.y() > pos2.y());
GlbVar.phyWorld->convexSweepTest(mCastShape, trans1, trans2, res);
//If hit, don't move.
if (res.mHit)
{
goto skip;
}
}
if (canMove)
{
oldTrans.setOrigin(newPos);
mBody->getMotionState()->setWorldTransform(oldTrans);
mNode->setPosition(BtOgre::Convert::toOgre(newPos));
}
}
}
skip:
//Python utick event.
NGF_PY_CALL_EVENT(utick, evt.timeSinceLastFrame);
}
