glm::mat4 Transform::getModelMatrix(){
if(mDirty){
mMatrix = getTranslationMatrix() * getOrientationMatrix() * getScaleMatrix();
mDirty = false;
}
return mMatrix;
}
void Node::addTranslation(float x, float y, float z)
{
glm::vec3 transfer = glm::vec3(x, y, z);
glm::mat4 newTranslationMatrix = glm::translate(getTranslationMatrix(), transfer);
m_translateMatrix = newTranslationMatrix;
updateModelMatrix(m_translateMatrix);
}
glm::mat4 DisplayObject::getLocalTransformation ()
{
if (_translationIsDirty || _rotationIsDirty || _scalingIsDirty)
{
_transformMatrixCache = getTranslationMatrix() * getRotationMatrix() * getScalingMatrix();
}
return _transformMatrixCache;
}
const glm::mat4& CTransformer::getModelMatrix() const
{
if (m_model.m_matrixDirty)
{
m_model.m_matrix = getTranslationMatrix() * getRotationMatrix() * getScaleMatrix();
m_model.m_matrixDirty = false;
}
return m_model.m_matrix;
}
//
// Calculates a matrix for a Caster if the current Matrix requires updating.
// The dirtyMatrix member tracks the state of this.
//
const Matrix& Caster::getLocalToWorldMatrix (void)
{
if (dirtyMatrix)
{
localToWorld.fromAngles(rot);
Matrix trans = getTranslationMatrix(pos);
localToWorld = trans * localToWorld;
dirtyMatrix = false;
}
return localToWorld;
}
uint32_t rSimpleMesh::getMatrix( rMat4f **_mat, rObjectBase::MATRIX_TYPES _type ) {
switch ( _type ) {
case SCALE: *_mat = getScaleMatrix(); return 0;
case ROTATION: *_mat = getRotationMatrix(); return 0;
case TRANSLATION: *_mat = getTranslationMatrix(); return 0;
case CAMERA_MATRIX: *_mat = getViewProjectionMatrix(); return 0;
case MODEL_MATRIX: *_mat = getModelMatrix(); return 0;
case VIEW_MATRIX: *_mat = getViewMatrix(); return 0;
case PROJECTION_MATRIX: *_mat = getProjectionMatrix(); return 0;
case MODEL_VIEW_MATRIX: *_mat = getModelViewMatrix(); return 0;
case MODEL_VIEW_PROJECTION: *_mat = getModelViewProjectionMatrix(); return 0;
case NORMAL_MATRIX: break;
}
return INDEX_OUT_OF_RANGE;
}
int main(){
initAndLoad(cv::Mat::eye(4,4,CV_32F), cv::Mat::eye(4,4,CV_32F), &vidRecord, &wcSkeletons, "map000000_aoto4_edit/video/", true);
zeroWorldCoordinateSkeletons(cv::Mat::eye(4,4,CV_32F), &vidRecord, &wcSkeletons);
setCameraMatrixTexture(KINECT::loadCameraParameters());
setCameraMatrixScene(KINECT::loadCameraParameters());
cylinderBody.Load("map000000-custCB/");
cv::namedWindow("rgb");
cv::namedWindow("3d", CV_GUI_NORMAL);
cv::setMouseCallback("3d", onMouse);
frame=0;
angle_y = 0;
angle_x = 0;
trans = cv::Mat::eye(4,4,CV_32F);
calculateSkeletonOffsetPoints(vidRecord, wcSkeletons, cylinderBody);
boundingBoxLerp = cv::Rect (0,0,WIDTH,HEIGHT);
lc = generateLerpCorners(boundingBoxLerp);
while(true){
cv::Mat tex_ = uncrop(vidRecord[frame].videoFrame);
cv::Mat tex(tex_.size(), CV_8UC4, cv::Scalar(255,255,255,255));
int from_to[] = { 0,0, 1,1, 2,2};
cv::mixChannels(&tex_,1,&tex,1,from_to,3);
cv::Mat _3d(HEIGHT, WIDTH, CV_8UC4, cv::Scalar(0,0,0,255));
cv::Vec3f sCenter = calcSkeleCenter(vidRecord[frame].kinectPoints);
trans = getTranslationMatrix(sCenter) * mat3_to_mat4(getRotationMatrix(cv::Vec3f(0,1,0), angle_y)) * mat3_to_mat4(getRotationMatrix(cv::Vec3f(1,0,0), angle_x)) * getTranslationMatrix(-sCenter);
ghostdraw_parallel(frame, cv::Mat::eye(4,4,CV_32F), vidRecord, wcSkeletons, cylinderBody, Limbrary(), tex, cv::Mat(), GD_CYL);
ghostdraw_parallel(frame, trans, vidRecord, wcSkeletons, cylinderBody, Limbrary(), _3d, cv::Mat(), GD_CYL);
cv::Scalar goodColor(50, 200, 250);
cv::Scalar badColor(20, 20, 250);
for(int joint=0;joint<NUMJOINTS;++joint)
{
cv::Scalar color;
if (wcSkeletons[frame].states[joint] < 1)
color = badColor;
else
color = goodColor;
{
cv::Vec3f jv = mat_to_vec3(wcSkeletons[frame].points.col(joint));
cv::Vec2f jv2 = mat4_to_vec2(getCameraMatrixScene() * vec3_to_mat4(jv));
cv::Point pj(jv2(0), jv2(1));
cv::circle(tex,pj,2,color,-1);
}
{
cv::Vec3f jv = mat_to_vec3(trans * wcSkeletons[frame].points.col(joint));
cv::Vec2f jv2 = mat4_to_vec2(getCameraMatrixScene() * vec3_to_mat4(jv));
cv::Point pj(jv2(0), jv2(1));
cv::circle(_3d,pj,3,color,-1);
}
}
std::stringstream frameSS;
frameSS << frame;
cv::putText(tex, frameSS.str(), cv::Point(30, 30), CV_FONT_HERSHEY_PLAIN, 2, cv::Scalar(0, 255, 0), 3);
cv::putText(_3d, frameSS.str(), cv::Point(30, 30), CV_FONT_HERSHEY_PLAIN, 2, cv::Scalar(255, 0, 255), 3);
cv::imshow("rgb", tex);
cv::imshow("3d", _3d);
char in = cv::waitKey(10);
switch(in){
case 'q':
return 0;
case 'z':
--frame;
while(vidRecord[frame].videoFrame.mat.empty()){
--frame;
}
if(frame < 0) frame = 0;
break;
case 'x':
++frame;
if (frame >= vidRecord.size()) frame = vidRecord.size() - 1;
while(vidRecord[frame].videoFrame.mat.empty()){
++frame;
if (frame >= vidRecord.size()) frame = vidRecord.size() - 1;
}
break;
case 'd':
if (lastjoint != -1){
if (wcSkeletons[frame].states[lastjoint] < 1){
wcSkeletons[frame].states[lastjoint] = 1;
}
else{
wcSkeletons[frame].states[lastjoint] = 0.5;
}
}
case 'r':
angle_y = 0;
angle_x = 0;
break;
case 'c':
alljoints = !alljoints;
break;
case 's':
for(int i=0;i<vidRecord.size();++i){
vidRecord[i].kinectPoints = wcSkeletons[i];
//if(!vidRecord[i].cam2World.empty())
//{
// vidRecord[i].kinectPoints.points = vidRecord[i].cam2World.inv() * vidRecord[i].kinectPoints.points;
//}
}
SaveVideo(&vidRecord, getCameraMatrixScene(), "map000000_aoto4_edit/video/");
break;
}
}
}
int main(){
std::ofstream of;
of.open("benchmark/score_out.txt");
std::cerr.rdbuf(of.rdbuf());
cv::Mat K2P = cv::Mat::eye(4,4,CV_32F);
#if PTAMM_STYLE == 1
K2P = getScaleMatrix(-1,-1,1);
#endif
initAndLoad(cv::Mat::eye(4,4,CV_32F),K2P,&vidRecord,&wcSkeletons,"map000000_whitesweater_edit/video/",false);
#if PTAMM_STYLE == 1
LoadWorldCoordinateSkeletons(wcSkeletons, "map000000_whitesweater/GhostGame.xml");
#else
zeroWorldCoordinateSkeletons(cv::Mat::eye(4,4,CV_32F),&vidRecord,&wcSkeletons);
#endif
cv::Vec3f skeleCenter = calcSkeleCenter(wcSkeletons[0]);
//cv::Mat shittyTransformMatrix = /*getTranslationMatrix(cv::Vec3f(0,0,1000)); */ getRotationMatrix4(cv::Vec3f(1,0,1), CV_PI);
//cv::FileStorage fs("ptammat.yml", cv::FileStorage::READ);
//fs["matCfW"] >> shittyTransformMatrix;
//for(int i=0;i<wcSkeletons.size();++i){
// wcSkeletons[i].points = shittyTransformMatrix * wcSkeletons[i].points;
//}
cylinderBody.Load("map000000-custCB/");
#if PTAMM_STYLE == 1
cylinderBody.radiusModifier = 0.658017;
#endif
calculateSkeletonOffsetPoints(vidRecord,wcSkeletons,cylinderBody);
//limbrary.Load("map000000_whitesweater-custCB-clust/");
limbrary.Load("map000000_whitesweater_edit-custCB-clean-test/");
//LoadStatusRecord("map000000_whitesweater/estim/", estimRecord);
//std::vector<Skeleton> wcSkeletonsEstim = wcSkeletons;
//interpolate(estimRecord, wcSkeletonsEstim);
cv::Vec3f translation(0,0,0);
cv::Vec3f rotation(0,CV_PI/2,0);
cv::Mat im(480, 640, CV_8UC4, cv::Scalar(255,255,255,0));
int frame = 100;
int r = 8;
int maxr = 16-4;
rotation(1) = CV_PI/2 - r * (CV_PI/16);
int p = 8;
rotation(0) = CV_PI/2 - (CV_PI/16) * p;
rotation(2) = 0;
//smoothKinectPoints(wcSkeletons, 0.5);
bool play = true;
while(true){
{
unsigned char options = GD_DRAW ;// | GD_NOWEIGHT | GD_NOLIMBRARY;
cv::Mat draw = im.clone();
cv::Mat zBuf;
cv::Mat transform = getTranslationMatrix(translation) * getTranslationMatrix(skeleCenter) *
mat3_to_mat4(getRotationMatrix(cv::Vec3f(1,0,0),rotation(0)) * getRotationMatrix(cv::Vec3f(0,1,0),rotation(1)) * getRotationMatrix(cv::Vec3f(0,0,1),rotation(2)))
* getTranslationMatrix(-skeleCenter);
#if PTAMM_STYLE == 1
cv::FileStorage fs("ptammat.yml", cv::FileStorage::READ);
fs["matCfW"] >> transform;
transform = getTranslationMatrix(translation) * getTranslationMatrix(skeleCenter) *
mat3_to_mat4(getRotationMatrix(cv::Vec3f(1,0,0),rotation(0)) * getRotationMatrix(cv::Vec3f(0,1,0),rotation(1)) * getRotationMatrix(cv::Vec3f(0,0,1),rotation(2)))
* getTranslationMatrix(-skeleCenter) * transform;
#endif
ScoreList scoreList[NUMLIMBS];
ghostdraw_parallel(frame, K2P.inv() * transform /* shittyTransformMatrix.inv()*/, vidRecord, wcSkeletons, cylinderBody, limbrary, draw, zBuf, options, scoreList);
/*
//hybrid
cv::Mat draw2 = im.clone();
options = GD_CYL ;
ghostdraw_parallel(frame, transform, vidRecord, wcSkeletons, cylinderBody, limbrary, draw2, options);
cv::Mat candidate = vidRecord[scoreList[0].front().first].videoFrame.mat;
cv::Mat cand_resize(HEIGHT, WIDTH, CV_8UC3, cv::Scalar(200,200,200));
int gap_y = (HEIGHT-candidate.rows)/2;
int gap_x = (HEIGHT-candidate.cols)/2;
//candidate.copyTo(cand_resize(cv::Range(gap_y,gap_y + candidate.rows), cv::Range(gap_x,gap_x+candidate.cols)));
candidate.copyTo(draw(cv::Range(0,candidate.rows), cv::Range(0,candidate.cols)));
cv::rectangle(draw,cv::Rect(0,0,candidate.cols,candidate.rows),cv::Scalar(200,150,100));
cv::Mat combine(HEIGHT*2,WIDTH,CV_8UC3);
draw2.copyTo(combine(cv::Range(0,draw2.rows),cv::Range(0,draw2.cols)));
draw.copyTo(combine(cv::Range(draw2.rows,draw.rows+draw2.rows),cv::Range(0,draw.cols)));
//cand_resize.copyTo(combine(cv::Range(draw.rows+draw2.rows,draw.rows+draw2.rows+cand_resize.rows),cv::Range(0,cand_resize.cols)));*/
//hybrid
//cv::Mat draw2 = im.clone();
//
//ghostdraw_parallel(frame, transform, vidRecord, wcSkeletonsEstim, cylinderBody, limbrary, draw2, zBuf, options);
//
//cv::Mat combine(HEIGHT*2,WIDTH,CV_8UC4);
//
//draw2.copyTo(combine(cv::Range(0,draw2.rows),cv::Range(0,draw2.cols)));
//draw.copyTo(combine(cv::Range(draw2.rows,draw.rows+draw2.rows),cv::Range(0,draw.cols)));
cv::imshow("pic", draw);
cv::Mat zBufNorm;
cv::normalize(zBuf, zBufNorm, 0, 255, cv::NORM_MINMAX);
cv::Mat zBuf8(zBuf.rows, zBuf.cols, CV_8U);
for(int i=0;i<zBuf.rows*zBuf.cols;++i){
zBuf8.ptr<unsigned char>()[i] = 255-zBufNorm.ptr<unsigned short>()[i];
}
cv::imshow("depth", zBuf8);
std::string dir = "out_ws_interpcmp/";
CreateDirectoryA(dir.c_str(), NULL);
std::stringstream ss;
ss << dir <<
"frame" << std::setfill('0') << std::setw(3) << frame <<
"r" << std::setw(2) << r <<
"p" << p << ".png";
//cv::imwrite(ss.str(), draw);
if(play)
{
if(++frame >= vidRecord.size()){
frame = 100;
++r;
rotation(1) = CV_PI/2 - r * (CV_PI/16);
if(r > maxr)
{
return 0;
}
}
}
}
char q = cv::waitKey(10);
if(q=='q') return 0;
else if(q=='w'){
translation(2) -= 1;
}else if(q=='s'){
translation(2) += 1;
}else if(q=='a'){
rotation(1) += CV_PI/16;
}else if(q=='d'){
rotation(1) -= CV_PI/16;
}else if(q=='z'){
rotation (2) += CV_PI/64;
}else if(q=='x'){
rotation (2) -= CV_PI/64;
}else if(q=='p'){
play = !play;
}
}
}
void ViewpointNode::getTranslationMatrix(float value[4][4])
{
SFMatrix mx;
getTranslationMatrix(&mx);
mx.getValue(value);
}
/**
*<Summary>
* Returns translated passed position in calling GameObject Coorinate system
*</Summary>
*/
Vector3D GameObject::getTranslatedPosition(Vector3D i_position)
{
return((getTranslationMatrix()* Matrix4x4(i_position)).getPositionFromMatrix4x4());
}
/**
*<summary>
* Check separation Axis test between passed GameObject(A) and calling GameObject(B)
* You may need to call it twice - for checking collision of B in A's coordinate system
*</Summary>
*/
bool GameObject::separationAxisTest(SharedPointer<GameObject> i_other, float& o_collisionTime, float & o_separationTime, myEngine::typedefs::Axis &o_collisionAxis) //Move to Collsion System to make it better - To-Do
{
bool isColliding = false;
float tCollisionInX = 0.0f;
float tSeparationInX = 0.0f;
float tCollisionInY = 0.0f;
float tSeparationInY = 0.0f;
float tCollisionInZ = 0.0f;
float tSeparationInZ = 0.0f;
//Position of other GameObject in this
Vector3D i_otherCenterPositionInThis = getTranslatedPosition(i_other->getPosition());
//Extent of other gameObject in this
Vector3D i_otherExtentInThis = getTransformedExtents(i_other);
//Transformed velocity of calling gamobject in its own coordiante system
Vector3D thisVelocityInThis = getTranslationMatrix() *physicsComponent->getCurrentVelocity();
//Velocity of other GameObject in calling gameObject coordinate system
Vector3D i_otherVelocityInThis = getTranslationMatrix() * i_other->physicsComponent->getCurrentVelocity();
//Relative velocity of other gamObject in freezed gameObject coordinate system i.e. in calling gameObject coordiante system
Vector3D i_otherChangedVelocityInThis = i_otherVelocityInThis - thisVelocityInThis;
//Expanding the extents of calling gameObject
float thisChangedXExtent = getCollider()->getExtendX() + i_otherExtentInThis.getX();
float thisChangedYExtent = getCollider()->getExtendY() + i_otherExtentInThis.getY();
float thisChangedZExtent = getCollider()->getExtendZ() + i_otherExtentInThis.getZ();
bool finalCollisionTimeInitialized = false;
//Swept collision for each axes
//Checking in X-Axis
switch (i_otherCenterPositionInThis.getX() >= 0 )
{
case true:
if (i_otherCenterPositionInThis.getX() <= thisChangedXExtent)
{
//To-Do - time of collision - Done but need testing
if (i_otherChangedVelocityInThis.getX() != 0)
{
tCollisionInX = abs(((0 + thisChangedXExtent) - i_otherCenterPositionInThis.getX()) / i_otherChangedVelocityInThis.getX());
tSeparationInX = abs((i_otherCenterPositionInThis.getX() - (0 - thisChangedXExtent)) / i_otherChangedVelocityInThis.getX());
o_collisionTime = tCollisionInX;
o_separationTime = tSeparationInX;
o_collisionAxis = myEngine::typedefs::XAxis;
finalCollisionTimeInitialized = true;
}
}
else return false;
break;
case false:
if (abs(i_otherCenterPositionInThis.getX()) <= thisChangedXExtent)
{
//To-Do - time of collision - Done but need testing
if (i_otherChangedVelocityInThis.getX() != 0)
{
tCollisionInX = abs((i_otherCenterPositionInThis.getX() - (0 - thisChangedXExtent)) / i_otherChangedVelocityInThis.getX());
tSeparationInX = abs(((0 + thisChangedXExtent) - i_otherCenterPositionInThis.getX()) / i_otherChangedVelocityInThis.getX());
o_collisionTime = tCollisionInX;
o_separationTime = tSeparationInX;
o_collisionAxis = myEngine::typedefs::XAxis;
finalCollisionTimeInitialized = true;
}
}
else return false;
break;
}
//Check in Y-axis in case X-Axis collision is true
switch (i_otherCenterPositionInThis.getY() >= 0)
{
case true:
if (i_otherCenterPositionInThis.getY() <= thisChangedYExtent)
{
//To-Do - time of collision -Done but need testing
if (i_otherChangedVelocityInThis.getY() != 0)
{
tCollisionInY = abs(((0 + thisChangedYExtent) - i_otherCenterPositionInThis.getY()) / i_otherChangedVelocityInThis.getY());
tSeparationInY = abs((i_otherCenterPositionInThis.getY() - (0 - thisChangedYExtent)) / i_otherChangedVelocityInThis.getY());
if (!finalCollisionTimeInitialized)
{
o_collisionTime = tCollisionInY;
o_separationTime = tSeparationInY;
o_collisionAxis = myEngine::typedefs::YAxis;
}
else
{
if (tCollisionInY < o_collisionTime)
{
o_collisionTime = tCollisionInY;
o_collisionAxis = myEngine::typedefs::YAxis;
}
if (tSeparationInY < o_separationTime)
o_separationTime = tSeparationInY;
}
}
}
else return false;
break;
case false:
if (abs(i_otherCenterPositionInThis.getY()) <= thisChangedYExtent)
{
//To-Do - time of collision - Done but need testing
if (i_otherChangedVelocityInThis.getY() != 0)
{
tCollisionInY = abs((i_otherCenterPositionInThis.getY() - (0 - thisChangedYExtent)) / i_otherChangedVelocityInThis.getY());
tSeparationInY = abs(((0 + thisChangedYExtent) - i_otherCenterPositionInThis.getY()) / i_otherChangedVelocityInThis.getY());
if (!finalCollisionTimeInitialized)
{
o_collisionTime = tCollisionInY;
o_separationTime = tSeparationInY;
o_collisionAxis = myEngine::typedefs::YAxis;
}
else
{
if (tCollisionInY < o_collisionTime)
{
o_collisionTime = tCollisionInY;
o_collisionAxis = myEngine::typedefs::YAxis;
}
if (tSeparationInY < o_separationTime)
o_separationTime = tSeparationInY;
}
}
}
else return false;
break;
}
//Check in Z-axis in case X-Axis and Y-Axis collision is true
switch (i_otherCenterPositionInThis.getZ() >= 0)
{
case true:
if (i_otherCenterPositionInThis.getZ() <= thisChangedZExtent)
{
//To-Do - time of collision - Done but need testing
if (i_otherChangedVelocityInThis.getZ() != 0)
{
tCollisionInZ = abs(((0 + thisChangedZExtent) - i_otherCenterPositionInThis.getZ()) / i_otherChangedVelocityInThis.getZ());
tSeparationInZ = abs((i_otherCenterPositionInThis.getZ() - (0 - thisChangedZExtent)) / i_otherChangedVelocityInThis.getZ());
if (!finalCollisionTimeInitialized)
{
o_collisionTime = tCollisionInZ;
o_separationTime = tSeparationInZ;
o_collisionAxis = myEngine::typedefs::ZAxis;
}
else
{
if (tCollisionInY < o_collisionTime)
{
o_collisionTime = tCollisionInZ;
o_collisionAxis = myEngine::typedefs::ZAxis;
}
if (tSeparationInY < o_separationTime)
o_separationTime = tSeparationInZ;
}
}
}
else return false;
break;
case false:
if (abs(i_otherCenterPositionInThis.getZ()) <= thisChangedZExtent)
{
//To-Do - time of collision - Done but need testing
if (i_otherChangedVelocityInThis.getZ() != 0)
{
tCollisionInZ = abs((i_otherCenterPositionInThis.getZ() - (0 - thisChangedZExtent)) / i_otherChangedVelocityInThis.getZ());
tSeparationInZ = abs(((0 + thisChangedZExtent) - i_otherCenterPositionInThis.getZ()) / i_otherChangedVelocityInThis.getZ());
if (!finalCollisionTimeInitialized)
{
o_collisionTime = tCollisionInZ;
o_separationTime = tSeparationInZ;
o_collisionAxis = myEngine::typedefs::ZAxis;
}
else
{
if (tCollisionInY < o_collisionTime)
{
o_collisionTime = tCollisionInZ;
o_collisionAxis = myEngine::typedefs::ZAxis;
}
if (tSeparationInY < o_separationTime)
o_separationTime = tSeparationInZ;
}
}
}
else return false;
break;
}
//returning if all if's are false i.e. collsion occured in this coordinate system
return true;
}
