void SmokeParticle::WorldUpdate(float currentTime)
{
currentTime;
//AZUL_UNUSED_FLOAT(currentTime);
// Goal: update the world matrix
Matrix Scale(MatrixScaleType::SCALE, this->scaleX, this->scaleY, 1.0f);
Matrix Rot(RotType::ROT_Z, this->angle);
Matrix Trans(MatrixTransType::TRANS, this->posX, this->posY, 0.0f);
*this->pWorld = Scale * Rot * Trans;
float timeInSec = Simulation::Instance()->getTimeStep();
this->lifeTime += timeInSec;
// add schrink
if (timeInSec > 0.0f)
{
this->scaleX *= 0.99f;
this->scaleY *= 0.99f;
}
// this->pendingDamage = this->damage;
// test with a small delta
if (this->lifeTime >= maxLifeTime)
{
this->deleteMe = true;
// Add to death list
GameObjectMan::AddToDeleteList(this);
}
}
/*! The matrix, when multiplied with a wrench applied at this contact will give the
resultant wrench applied on the other body in contact (thus computed relative
to that object's center of mass), expressed in world coordinates.
The matrix looks like this:
| R 0 |
|CR R |
Where R is the 3x3 rotation matrix between the coordinate systems and C also
contains the cross product matrix that depends on the translation between them.
*/
Matrix
Contact::localToWorldWrenchMatrix() const
{
Matrix Ro(Matrix::ZEROES<Matrix>(6,6));
transf contactTran = getContactFrame() * getBody1()->getTran();
mat3 R; contactTran.rotation().ToRotationMatrix(R);
Matrix Rot( Matrix::ROTATION(R) );
//the force transform is simple, just the matrix that changes coord. systems
Ro.copySubMatrix(0, 0, Rot);
Ro.copySubMatrix(3, 3, Rot);
//for torque we also multiply by a cross product matrix
vec3 worldLocation = contactTran.translation();
vec3 cog = getBody2()->getTran().translation();
mat3 C; C.setCrossProductMatrix(worldLocation - cog);
Matrix CR(3,3);
matrixMultiply(Matrix::ROTATION(C.transpose()), Rot, CR);
//also scale by object max radius so we get same units as force
//and optimization does not favor torque over force
double scale = 1.0;
if (getBody2()->isA("GraspableBody")) {
scale = scale / static_cast<GraspableBody*>(getBody2())->getMaxRadius();
}
CR.multiply(scale);
Ro.copySubMatrix(3, 0, CR);
return Ro;
}
void WorldProxy::ProtoDeSerializeProperties(const XML::Node& SelfRoot)
{
XML::Attribute CurrAttrib;
XML::Node PropertiesNode = SelfRoot.GetChild( WorldProxy::GetSerializableName() + "Properties" );
if( !PropertiesNode.Empty() ) {
if(PropertiesNode.GetAttribute("Version").AsInt() == 1) {
CurrAttrib = PropertiesNode.GetAttribute("ProxyID");
if( !CurrAttrib.Empty() )
this->ProxyID = static_cast<UInt32>( CurrAttrib.AsUint() );
XML::Node PositionNode = PropertiesNode.GetChild("Location").GetFirstChild();
if( !PositionNode.Empty() ) {
Vector3 Loc(PositionNode);
this->SetLocation(Loc);
}
XML::Node OrientationNode = PropertiesNode.GetChild("Orientation").GetFirstChild();
if( !PositionNode.Empty() ) {
Quaternion Rot(OrientationNode);
this->SetOrientation(Rot);
}
XML::Node ScaleNode = PropertiesNode.GetChild("Scale").GetFirstChild();
if( !PositionNode.Empty() ) {
Vector3 Scale(ScaleNode);
this->SetScale(Scale);
}
}else{
MEZZ_EXCEPTION(ExceptionBase::INVALID_VERSION_EXCEPTION,"Incompatible XML Version for " + (WorldProxy::GetSerializableName() + "Properties" ) + ": Not Version 1.");
}
}else{
MEZZ_EXCEPTION(ExceptionBase::II_IDENTITY_NOT_FOUND_EXCEPTION,WorldProxy::GetSerializableName() + "Properties" + " was not found in the provided XML node, which was expected.");
}
}
/* Generate ball*/
vector<Vector3r> BallPoints(Vector3r radii, int NumFacets,int seed){
vector<Vector3r> v;
if (NumFacets == 60) v = TruncIcosaHedPoints(radii);
if (NumFacets == 24) v = SnubCubePoints(radii);
else{
double inc = Mathr::PI * (3. - pow(5.,0.5));
double off = 2. / double(NumFacets);
double y,r,phi;
for(int k=0; k<NumFacets; k++){
y = double(k) * off - 1. + (off / 2.);
r = pow(1. - y*y,0.5);
phi = double(k) * inc;
v.push_back(Vector3r(cos(phi)*r*radii[0], y*radii[1], sin(phi)*r*radii[2]));
}
}
// randomly rotate
srand(seed);
Quaternionr Rot(double(rand())/RAND_MAX,double(rand())/RAND_MAX,double(rand())/RAND_MAX,double(rand())/RAND_MAX);
Rot.normalize();
for(int i=0; i< (int) v.size();i++) {
v[i] = Rot*(Vector3r(v[i][0],v[i][1],v[i][2]));
}
return v;
}
ElMatrix<double> OriMatrixConvertion(CamStenope * aCS)
{
ElMatrix<double> RotMM2CV(4,3,0.0); //External orientation matrix
ElMatrix<double> P(4,3,0.0); //Orientation (int & ext) matrix in the CV system
ElMatrix<double> F(3,3,0.0); //Internal orientation matrix
ElMatrix<double> Rot(3,3,0.0); //Rotation matrix between photogrammetry and CV systems
ElMatrix<double> Rotx(3,3,0.0); //180° rotation matrix along the x axis
Rotx(0,0)=1;
Rotx(1,1)=-1;
Rotx(2,2)=-1;
Rot=Rotx*aCS->Orient().Mat();
for(int i=0;i<3;i++)
{
RotMM2CV(i,i)=1;
}
RotMM2CV(3,0)=-aCS->VraiOpticalCenter().x;
RotMM2CV(3,1)=-aCS->VraiOpticalCenter().y;
RotMM2CV(3,2)=-aCS->VraiOpticalCenter().z;
RotMM2CV=Rot*RotMM2CV;
F(0,0)=-aCS->Focale();
F(1,1)=aCS->Focale();
Pt2dr PPOut=aCS->DistDirecte(aCS->PP());
F(2,0)=PPOut.x;
F(2,1)=PPOut.y;
F(2,2)=1;
//Computation of the orientation matrix (P=-F*RotMM2CV)
P.mul(F*RotMM2CV,-1);
return P;
}
void Solve( PiecesLeft pl, const PuzzleState &ps ) {
if(!Solvable(ps)) {
return;
}
if( pl.size() == 0 ) {
printf( "Solution:\n" );
PrintState( ps );
PrintState( ps, fopen( solutionfilename, "w" ) );
exit( 0 );
}
int i = *(pl.begin());
Piece piece = pieces[i];
//printf( "Attempting to fit in piece: %c\n", piece.id );
pl.erase( i );
for( int r = 0; r < 8; ++r ) {
Piece newPiece = Rot( piece, r );
//printf( "Trying this way:\n" );
//PrintPiece( newPiece );
for( int y = -2; y < HEIGHT; ++y ) {
for( int x = -2; x < WIDTH; ++x ) {
if( PieceFits( ps, newPiece, x, y ) ) {
PuzzleState newState = FillState( ps, newPiece, x, y );
Solve( pl, newState );
}
}
}
}
}
void Polyhedra::GenerateRandomGeometry(){
srand(seed);
vector<CGALpoint> nuclei;
nuclei.push_back(CGALpoint(5.,5.,5.));
CGALpoint trial;
int iter = 0;
bool isOK;
//fill box 5x5x5 with randomly located nuclei with restricted minimal mutual distance 0.75 which gives approximate mean mutual distance 1;
Real dist_min2 = 0.75*0.75;
while(iter<500){
isOK = true;
iter++;
trial = CGALpoint(Real(rand())/RAND_MAX*5.+2.5,Real(rand())/RAND_MAX*5.+2.5,Real(rand())/RAND_MAX*5.+2.5);
for(int i=0;i< (int) nuclei.size();i++) {
isOK = pow(nuclei[i].x()-trial.x(),2)+pow(nuclei[i].y()-trial.y(),2)+pow(nuclei[i].z()-trial.z(),2) > dist_min2;
if (!isOK) break;
}
if(isOK){
iter = 0;
nuclei.push_back(trial);
}
}
//perform Voronoi tessellation
nuclei.erase(nuclei.begin());
Triangulation dt(nuclei.begin(), nuclei.end());
Triangulation::Vertex_handle zero_point = dt.insert(CGALpoint(5.,5.,5.));
v.clear();
std::vector<Triangulation::Cell_handle> ch_cells;
dt.incident_cells(zero_point,std::back_inserter(ch_cells));
for(std::vector<Triangulation::Cell_handle>::iterator ci = ch_cells.begin(); ci !=ch_cells.end(); ++ci){
v.push_back(FromCGALPoint(dt.dual(*ci))-Vector3r(5.,5.,5.));
}
//resize and rotate the voronoi cell
Quaternionr Rot(Real(rand())/RAND_MAX,Real(rand())/RAND_MAX,Real(rand())/RAND_MAX,Real(rand())/RAND_MAX);
Rot.normalize();
for(int i=0; i< (int) v.size();i++) {
v[i] = Rot*(Vector3r(v[i][0]*size[0],v[i][1]*size[1],v[i][2]*size[2]));
}
//to avoid patological cases (that should not be present, but CGAL works somehow unpredicable)
if (v.size() < 8) {
cout << "wrong " << v.size() << endl;
v.clear();
seed = rand();
GenerateRandomGeometry();
}
}
void Marker::glGetModelViewMatrix(double modelview_matrix[16])
{
// check if paremeters are valid
bool invalid = false;
for (int i = 0; i < 3 && !invalid; i++)
{
if (Tvec.at<float>(i, 0) != -999999)
invalid |= false;
if (Rvec.at<float>(i, 0) != -999999)
invalid |= false;
}
if (invalid)
throw cv::Exception(9003, "extrinsic parameters are not set", "Marker::getModelViewMatrix", __FILE__,
__LINE__);
cv::Mat Rot(3, 3, CV_32FC1), Jacob;
cv::Rodrigues(Rvec, Rot, Jacob);
double para[3][4];
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
para[i][j] = Rot.at<float>(i, j);
// now, add the translation
para[0][3] = Tvec.at<float>(0, 0);
para[1][3] = Tvec.at<float>(1, 0);
para[2][3] = Tvec.at<float>(2, 0);
double scale = 1;
modelview_matrix[0 + 0 * 4] = para[0][0];
// R1C2
modelview_matrix[0 + 1 * 4] = para[0][1];
modelview_matrix[0 + 2 * 4] = para[0][2];
modelview_matrix[0 + 3 * 4] = para[0][3];
// R2
modelview_matrix[1 + 0 * 4] = para[1][0];
modelview_matrix[1 + 1 * 4] = para[1][1];
modelview_matrix[1 + 2 * 4] = para[1][2];
modelview_matrix[1 + 3 * 4] = para[1][3];
// R3
modelview_matrix[2 + 0 * 4] = -para[2][0];
modelview_matrix[2 + 1 * 4] = -para[2][1];
modelview_matrix[2 + 2 * 4] = -para[2][2];
modelview_matrix[2 + 3 * 4] = -para[2][3];
modelview_matrix[3 + 0 * 4] = 0.0;
modelview_matrix[3 + 1 * 4] = 0.0;
modelview_matrix[3 + 2 * 4] = 0.0;
modelview_matrix[3 + 3 * 4] = 1.0;
if (scale != 0.0)
{
modelview_matrix[12] *= scale;
modelview_matrix[13] *= scale;
modelview_matrix[14] *= scale;
}
}
void rotation2angles(Eigen::Matrix< Tp, 3, 3 > &Rot, Eigen::Matrix< Tp, 3, 1 > &angles, bool degrees = true)
{
Tp anglex = 0.0;
Tp angley = 0.0;
Tp anglez = 0.0;
anglex = atan2( Rot(1,2), Rot(2,2) );
angley = atan2( -Rot(0,2), sqrt( pow( Rot(0,0), 2.0 ) + pow( Rot(0,1), 2.0 ) ) );
Tp s1 = sin(anglex); Tp c1 = cos(anglex);
anglez = atan2( s1*Rot(2,0) - c1*Rot(1,0) , c1*Rot(1,1) - s1*Rot(2,1) );
// radians to degrees convertion
if(degrees)
angles = Eigen::Matrix< Tp, 3, 1 >( -anglex*180/CONSTANT_PI, -angley*180/CONSTANT_PI, -anglez*180/CONSTANT_PI );
else
angles = Eigen::Matrix< Tp, 3, 1 >( -anglex, -angley, -anglez );
return;
}
void StoneBall::WorldUpdate(float currentTime)
{
currentTime;
// Goal: update the world matrix
Matrix ScaleF(MatrixScaleType::SCALE, this->scaleFactor, this->scaleFactor, 1.0f);
Matrix Scale(MatrixScaleType::SCALE, this->scaleX, this->scaleY, 1.0f);
Matrix Rot(RotType::ROT_Z, this->angle);
Matrix Trans(MatrixTransType::TRANS, this->posX, this->posY, 0.0f);
*this->pWorld = ScaleF* Scale * Rot * Trans;
}
/*===================================*/
void CFragment::DiscreteInit( void )
{
NxVec3 Dimensions[CBreakWall::FragmentType] = { NxVec3( 0.60f , 0.30f , 1.15f )
, NxVec3( 0.25f , 0.15f , 1.65f )
, NxVec3( 0.60f , 0.30f , 1.65f ) };
NxBodyDesc bodyDesc;
bodyDesc.setToDefault();
NxActorDesc actorDesc;
actorDesc.setToDefault();
NxBoxShapeDesc boxDesc;
boxDesc.setToDefault();
boxDesc.skinWidth = 0.0f;
actorDesc.density = 1000000.0f;
actorDesc.body = &bodyDesc;
actorDesc.globalPose.t = NxVec3( m_vPos.x , m_vPos.y , m_vPos.z );
NxMat33 Mat;
CRandomize Rand;
D3DXVECTOR3 Rot( (float)Rand.GetRandomize( 0.0 , 359.0 ) , (float)Rand.GetRandomize( 0.0 , 359.0 ) , (float)Rand.GetRandomize( 0.0 , 359.0 ) );
CDPConverter::SetRot( &Mat , &Rot );
actorDesc.globalPose.M = Mat;
bodyDesc.angularDamping = 0.0f;
boxDesc.dimensions = Dimensions[m_Type-1]*m_Scale;
if( m_MaterialIndex == -1 )
{
// アクターのマテリアルを設定する
NxMaterialDesc materialDesc;
materialDesc.restitution = 0.4f;
materialDesc.dynamicFriction = 3.8f;
materialDesc.staticFriction = 3.5f;
m_MaterialIndex = m_pPhysX->GetScene()->createMaterial( materialDesc )->getMaterialIndex();
boxDesc.materialIndex = m_MaterialIndex;
}
else
{
boxDesc.materialIndex = m_MaterialIndex;
}
actorDesc.shapes.pushBack( &boxDesc );
m_Actor = m_pPhysX->GetScene()->createActor( actorDesc );
m_Actor->putToSleep();
m_Material = m_lpXFile->GetMaterial();
}
void Rotate(float angle, Vector3f axis)
{
const float RadAngle = (float) ToRadian( angle / 2 );
const float sinAngle = sinf( RadAngle );
const float cosAngle = cosf( RadAngle );
axis *= sinAngle;
// quaternion describing an 'angle' rotation around the axis
Quaternion Rot( axis, cosAngle );
Quaternion Res = Rot * (*this);
Res *= Rot.conjugate();
*this = Res.axis;
}
void StoneBlock::WorldUpdate(float currentTime)
{
AZUL_UNUSED_FLOAT(currentTime);
// Goal: update the world matrix
Matrix ScaleF(MatrixScaleType::SCALE, this->scaleFactor, this->scaleFactor, 1.0f);
Matrix Scale(MatrixScaleType::SCALE, this->scaleX, this->scaleY, 1.0f);
Matrix Rot(RotType::ROT_Z, this->angle);
Matrix Trans(MatrixTransType::TRANS, this->posX, this->posY, 0.0f);
*this->pWorld = ScaleF* Scale * Rot * Trans;
this->pBlockState->Execute(currentTime, this);
//ApplyDamage();
}
int main( int argc, char *argv[] ) {
printf( "have %i args\n", argc );
printf( "first arg: %s\n", argv[0] );
PuzzleState ps;
PiecesLeft pl;
if( argc == 2 ) {
LoadState( ps, pl, argv[1] );
sprintf( solutionfilename, "SOLVED_%s", argv[1] );
printf( "Solving\n");
PrintRemainingPieces( pl );
Solve( pl, ps );
printf( "Unsolvable?\n");
} else {
Piece e = pieces[4];
for( int ori = 0; ori < 8; ++ori ) {
printf( "Piece rotated %i\n", ori );
PrintPiece( Rot( e, ori ) );
printf( "\n" );
}
}
}
void pose_estimation_from_line_correspondence(Eigen::MatrixXf start_points,
Eigen::MatrixXf end_points,
Eigen::MatrixXf directions,
Eigen::MatrixXf points,
Eigen::MatrixXf &rot_cw,
Eigen::VectorXf &pos_cw)
{
int n = start_points.cols();
if(n != directions.cols())
{
return;
}
if(n<4)
{
return;
}
float condition_err_threshold = 1e-3;
Eigen::VectorXf cosAngleThreshold(3);
cosAngleThreshold << 1.1, 0.9659, 0.8660;
Eigen::MatrixXf optimumrot_cw(3,3);
Eigen::VectorXf optimumpos_cw(3);
std::vector<float> lineLenVec(n,1);
vfloat3 l1;
vfloat3 l2;
vfloat3 nc1;
vfloat3 Vw1;
vfloat3 Xm;
vfloat3 Ym;
vfloat3 Zm;
Eigen::MatrixXf Rot(3,3);
std::vector<vfloat3> nc_bar(n,vfloat3(0,0,0));
std::vector<vfloat3> Vw_bar(n,vfloat3(0,0,0));
std::vector<vfloat3> n_c(n,vfloat3(0,0,0));
Eigen::MatrixXf Rx(3,3);
int line_id;
for(int HowToChooseFixedTwoLines = 1 ; HowToChooseFixedTwoLines <=3 ; HowToChooseFixedTwoLines++)
{
if(HowToChooseFixedTwoLines==1)
{
#pragma omp parallel for
for(int i = 0; i < n ; i++ )
{
// to correct
float lineLen = 10;
lineLenVec[i] = lineLen;
}
std::vector<float>::iterator result;
result = std::max_element(lineLenVec.begin(), lineLenVec.end());
line_id = std::distance(lineLenVec.begin(), result);
vfloat3 temp;
temp = start_points.col(0);
start_points.col(0) = start_points.col(line_id);
start_points.col(line_id) = temp;
temp = end_points.col(0);
end_points.col(0) = end_points.col(line_id);
end_points.col(line_id) = temp;
temp = directions.col(line_id);
directions.col(0) = directions.col(line_id);
directions.col(line_id) = temp;
temp = points.col(0);
points.col(0) = points.col(line_id);
points.col(line_id) = temp;
lineLenVec[line_id] = lineLenVec[1];
lineLenVec[1] = 0;
l1 = start_points.col(0) - end_points.col(0);
l1 = l1/l1.norm();
}
for(int i = 1; i < n; i++)
{
std::vector<float>::iterator result;
result = std::max_element(lineLenVec.begin(), lineLenVec.end());
line_id = std::distance(lineLenVec.begin(), result);
l2 = start_points.col(line_id) - end_points.col(line_id);
l2 = l2/l2.norm();
lineLenVec[line_id] = 0;
MatrixXf cosAngle(3,3);
cosAngle = (l1.transpose()*l2).cwiseAbs();
if(cosAngle.maxCoeff() < cosAngleThreshold[HowToChooseFixedTwoLines])
{
break;
}
}
vfloat3 temp;
temp = start_points.col(1);
start_points.col(1) = start_points.col(line_id);
start_points.col(line_id) = temp;
temp = end_points.col(1);
end_points.col(1) = end_points.col(line_id);
end_points.col(line_id) = temp;
temp = directions.col(1);
directions.col(1) = directions.col(line_id);
directions.col(line_id) = temp;
temp = points.col(1);
points.col(1) = points.col(line_id);
points.col(line_id) = temp;
lineLenVec[line_id] = lineLenVec[1];
lineLenVec[1] = 0;
// The rotation matrix R_wc is decomposed in way which is slightly different from the description in the paper,
// but the framework is the same.
// R_wc = (Rot') * R * Rot = (Rot') * (Ry(theta) * Rz(phi) * Rx(psi)) * Rot
nc1 = x_cross(start_points.col(1),end_points.col(1));
nc1 = nc1/nc1.norm();
Vw1 = directions.col(1);
Vw1 = Vw1/Vw1.norm();
//the X axis of Model frame
Xm = x_cross(nc1,Vw1);
Xm = Xm/Xm.norm();
//the Y axis of Model frame
Ym = nc1;
//the Z axis of Model frame
Zm = x_cross(Xm,Zm);
Zm = Zm/Zm.norm();
//Rot * [Xm, Ym, Zm] = I.
Rot.col(0) = Xm;
Rot.col(1) = Ym;
Rot.col(2) = Zm;
Rot = Rot.transpose();
//rotate all the vector by Rot.
//nc_bar(:,i) = Rot * nc(:,i)
//Vw_bar(:,i) = Rot * Vw(:,i)
#pragma omp parallel for
for(int i = 0 ; i < n ; i++)
{
vfloat3 nc = x_cross(start_points.col(1),end_points.col(1));
nc = nc/nc.norm();
n_c[i] = nc;
nc_bar[i] = Rot * nc;
Vw_bar[i] = Rot * directions.col(i);
}
//Determine the angle psi, it is the angle between z axis and Vw_bar(:,1).
//The rotation matrix Rx(psi) rotates Vw_bar(:,1) to z axis
float cospsi = (Vw_bar[1])[2];//the angle between z axis and Vw_bar(:,1); cospsi=[0,0,1] * Vw_bar(:,1);.
float sinpsi= sqrt(1 - cospsi*cospsi);
Rx.row(0) = vfloat3(1,0,0);
Rx.row(1) = vfloat3(0,cospsi,-sinpsi);
Rx.row(2) = vfloat3(0,sinpsi,cospsi);
vfloat3 Zaxis = Rx * Vw_bar[1];
if(1-fabs(Zaxis[3]) > 1e-5)
{
Rx = Rx.transpose();
}
//estimate the rotation angle phi by least square residual.
//i.e the rotation matrix Rz(phi)
vfloat3 Vm2 = Rx * Vw_bar[1];
float A2 = Vm2[0];
float B2 = Vm2[1];
float C2 = Vm2[2];
float x2 = (nc_bar[1])[0];
float y2 = (nc_bar[1])[1];
float z2 = (nc_bar[1])[2];
Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
Eigen::VectorXf coeff(9);
std::vector<float> coef(9,0); //coefficients of equation (7)
Eigen::VectorXf polyDF = VectorXf::Zero(16); //%dF = ployDF(1) * t^15 + ployDF(2) * t^14 + ... + ployDF(15) * t + ployDF(16);
//construct the polynomial F'
#pragma omp parallel for
for(int i = 3 ; i < n ; i++)
{
vfloat3 Vm3 = Rx*Vw_bar[i];
float A3 = Vm3[0];
float B3 = Vm3[1];
float C3 = Vm3[2];
float x3 = (nc_bar[i])[0];
float y3 = (nc_bar[i])[1];
float z3 = (nc_bar[i])[2];
float u11 = -z2*A2*y3*B3 + y2*B2*z3*A3;
float u12 = -y2*A2*z3*B3 + z2*B2*y3*A3;
float u13 = -y2*B2*z3*B3 + z2*B2*y3*B3 + y2*A2*z3*A3 - z2*A2*y3*A3;
float u14 = -y2*B2*x3*C3 + x2*C2*y3*B3;
float u15 = x2*C2*y3*A3 - y2*A2*x3*C3;
float u21 = -x2*A2*y3*B3 + y2*B2*x3*A3;
float u22 = -y2*A2*x3*B3 + x2*B2*y3*A3;
float u23 = x2*B2*y3*B3 - y2*B2*x3*B3 - x2*A2*y3*A3 + y2*A2*x3*A3;
float u24 = y2*B2*z3*C3 - z2*C2*y3*B3;
float u25 = y2*A2*z3*C3 - z2*C2*y3*A3;
float u31 = -x2*A2*z3*A3 + z2*A2*x3*A3;
float u32 = -x2*B2*z3*B3 + z2*B2*x3*B3;
float u33 = x2*A2*z3*B3 - z2*A2*x3*B3 + x2*B2*z3*A3 - z2*B2*x3*A3;
float u34 = z2*A2*z3*C3 + x2*A2*x3*C3 - z2*C2*z3*A3 - x2*C2*x3*A3;
float u35 = -z2*B2*z3*C3 - x2*B2*x3*C3 + z2*C2*z3*B3 + x2*C2*x3*B3;
float u36 = -x2*C2*z3*C3 + z2*C2*x3*C3;
float a4 = u11*u11 + u12*u12 - u13*u13 - 2*u11*u12 + u21*u21 + u22*u22 - u23*u23
-2*u21*u22 - u31*u31 - u32*u32 + u33*u33 + 2*u31*u32;
float a3 =2*(u11*u14 - u13*u15 - u12*u14 + u21*u24 - u23*u25
- u22*u24 - u31*u34 + u33*u35 + u32*u34);
float a2 =-2*u12*u12 + u13*u13 + u14*u14 - u15*u15 + 2*u11*u12 - 2*u22*u22 + u23*u23
+ u24*u24 - u25*u25 +2*u21*u22+ 2*u32*u32 - u33*u33
- u34*u34 + u35*u35 -2*u31*u32- 2*u31*u36 + 2*u32*u36;
float a1 =2*(u12*u14 + u13*u15 + u22*u24 + u23*u25 - u32*u34 - u33*u35 - u34*u36);
float a0 = u12*u12 + u15*u15+ u22*u22 + u25*u25 - u32*u32 - u35*u35 - u36*u36 - 2*u32*u36;
float b3 =2*(u11*u13 - u12*u13 + u21*u23 - u22*u23 - u31*u33 + u32*u33);
float b2 =2*(u11*u15 - u12*u15 + u13*u14 + u21*u25 - u22*u25 + u23*u24 - u31*u35 + u32*u35 - u33*u34);
float b1 =2*(u12*u13 + u14*u15 + u22*u23 + u24*u25 - u32*u33 - u34*u35 - u33*u36);
float b0 =2*(u12*u15 + u22*u25 - u32*u35 - u35*u36);
float d0 = a0*a0 - b0*b0;
float d1 = 2*(a0*a1 - b0*b1);
float d2 = a1*a1 + 2*a0*a2 + b0*b0 - b1*b1 - 2*b0*b2;
float d3 = 2*(a0*a3 + a1*a2 + b0*b1 - b1*b2 - b0*b3);
float d4 = a2*a2 + 2*a0*a4 + 2*a1*a3 + b1*b1 + 2*b0*b2 - b2*b2 - 2*b1*b3;
float d5 = 2*(a1*a4 + a2*a3 + b1*b2 + b0*b3 - b2*b3);
float d6 = a3*a3 + 2*a2*a4 + b2*b2 - b3*b3 + 2*b1*b3;
float d7 = 2*(a3*a4 + b2*b3);
float d8 = a4*a4 + b3*b3;
std::vector<float> v { a4, a3, a2, a1, a0, b3, b2, b1, b0 };
Eigen::VectorXf vp;
vp << a4, a3, a2, a1, a0, b3, b2, b1, b0 ;
//coef = coef + v;
coeff = coeff + vp;
polyDF[0] = polyDF[0] + 8*d8*d8;
polyDF[1] = polyDF[1] + 15* d7*d8;
polyDF[2] = polyDF[2] + 14* d6*d8 + 7*d7*d7;
polyDF[3] = polyDF[3] + 13*(d5*d8 + d6*d7);
polyDF[4] = polyDF[4] + 12*(d4*d8 + d5*d7) + 6*d6*d6;
polyDF[5] = polyDF[5] + 11*(d3*d8 + d4*d7 + d5*d6);
polyDF[6] = polyDF[6] + 10*(d2*d8 + d3*d7 + d4*d6) + 5*d5*d5;
polyDF[7] = polyDF[7] + 9 *(d1*d8 + d2*d7 + d3*d6 + d4*d5);
polyDF[8] = polyDF[8] + 8 *(d1*d7 + d2*d6 + d3*d5) + 4*d4*d4 + 8*d0*d8;
polyDF[9] = polyDF[9] + 7 *(d1*d6 + d2*d5 + d3*d4) + 7*d0*d7;
polyDF[10] = polyDF[10] + 6 *(d1*d5 + d2*d4) + 3*d3*d3 + 6*d0*d6;
polyDF[11] = polyDF[11] + 5 *(d1*d4 + d2*d3)+ 5*d0*d5;
polyDF[12] = polyDF[12] + 4 * d1*d3 + 2*d2*d2 + 4*d0*d4;
polyDF[13] = polyDF[13] + 3 * d1*d2 + 3*d0*d3;
polyDF[14] = polyDF[14] + d1*d1 + 2*d0*d2;
polyDF[15] = polyDF[15] + d0*d1;
}
Eigen::VectorXd coefficientPoly = VectorXd::Zero(16);
for(int j =0; j < 16 ; j++)
{
coefficientPoly[j] = polyDF[15-j];
}
//solve polyDF
solver.compute(coefficientPoly);
const Eigen::PolynomialSolver<double, Eigen::Dynamic>::RootsType & r = solver.roots();
Eigen::VectorXd rs(r.rows());
Eigen::VectorXd is(r.rows());
std::vector<float> min_roots;
for(int j =0;j<r.rows();j++)
{
rs[j] = fabs(r[j].real());
is[j] = fabs(r[j].imag());
}
float maxreal = rs.maxCoeff();
for(int j = 0 ; j < rs.rows() ; j++ )
{
if(is[j]/maxreal <= 0.001)
{
min_roots.push_back(rs[j]);
}
}
std::vector<float> temp_v(15);
std::vector<float> poly(15);
for(int j = 0 ; j < 15 ; j++)
{
temp_v[j] = j+1;
}
for(int j = 0 ; j < 15 ; j++)
{
poly[j] = polyDF[j]*temp_v[j];
}
Eigen::Matrix<double,16,1> polynomial;
Eigen::VectorXd evaluation(min_roots.size());
for( int j = 0; j < min_roots.size(); j++ )
{
evaluation[j] = poly_eval( polynomial, min_roots[j] );
}
std::vector<float> minRoots;
for( int j = 0; j < min_roots.size(); j++ )
{
if(!evaluation[j]<=0)
{
minRoots.push_back(min_roots[j]);
}
}
if(minRoots.size()==0)
{
cout << "No solution" << endl;
return;
}
int numOfRoots = minRoots.size();
//for each minimum, we try to find a solution of the camera pose, then
//choose the one with the least reprojection residual as the optimum of the solution.
float minimalReprojectionError = 100;
// In general, there are two solutions which yields small re-projection error
// or condition error:"n_c * R_wc * V_w=0". One of the solution transforms the
// world scene behind the camera center, the other solution transforms the world
// scene in front of camera center. While only the latter one is correct.
// This can easily be checked by verifying their Z coordinates in the camera frame.
// P_c(Z) must be larger than 0 if it's in front of the camera.
for(int rootId = 0 ; rootId < numOfRoots ; rootId++)
{
float cosphi = minRoots[rootId];
float sign1 = sign_of_number(coeff[0] * pow(cosphi,4)
+ coeff[1] * pow(cosphi,3) + coeff[2] * pow(cosphi,2)
+ coeff[3] * cosphi + coeff[4]);
float sign2 = sign_of_number(coeff[5] * pow(cosphi,3)
+ coeff[6] * pow(cosphi,2) + coeff[7] * cosphi + coeff[8]);
float sinphi= -sign1*sign2*sqrt(abs(1-cosphi*cosphi));
Eigen::MatrixXf Rz(3,3);
Rz.row(0) = vfloat3(cosphi,-sinphi,0);
Rz.row(1) = vfloat3(sinphi,cosphi,0);
Rz.row(2) = vfloat3(0,0,1);
//now, according to Sec4.3, we estimate the rotation angle theta
//and the translation vector at a time.
Eigen::MatrixXf RzRxRot(3,3);
RzRxRot = Rz*Rx*Rot;
//According to the fact that n_i^C should be orthogonal to Pi^c and Vi^c, we
//have: scalarproduct(Vi^c, ni^c) = 0 and scalarproduct(Pi^c, ni^c) = 0.
//where Vi^c = Rwc * Vi^w, Pi^c = Rwc *(Pi^w - pos_cw) = Rwc * Pi^w - pos;
//Using the above two constraints to construct linear equation system Mat about
//[costheta, sintheta, tx, ty, tz, 1].
Eigen::MatrixXf Matrice(2*n-1,6);
#pragma omp parallel for
for(int i = 0 ; i < n ; i++)
{
float nxi = (nc_bar[i])[0];
float nyi = (nc_bar[i])[1];
float nzi = (nc_bar[i])[2];
Eigen::VectorXf Vm(3);
Vm = RzRxRot * directions.col(i);
float Vxi = Vm[0];
float Vyi = Vm[1];
float Vzi = Vm[2];
Eigen::VectorXf Pm(3);
Pm = RzRxRot * points.col(i);
float Pxi = Pm(1);
float Pyi = Pm(2);
float Pzi = Pm(3);
//apply the constraint scalarproduct(Vi^c, ni^c) = 0
//if i=1, then scalarproduct(Vi^c, ni^c) always be 0
if(i>1)
{
Matrice(2*i-3, 0) = nxi * Vxi + nzi * Vzi;
Matrice(2*i-3, 1) = nxi * Vzi - nzi * Vxi;
Matrice(2*i-3, 5) = nyi * Vyi;
}
//apply the constraint scalarproduct(Pi^c, ni^c) = 0
Matrice(2*i-2, 0) = nxi * Pxi + nzi * Pzi;
Matrice(2*i-2, 1) = nxi * Pzi - nzi * Pxi;
Matrice(2*i-2, 2) = -nxi;
Matrice(2*i-2, 3) = -nyi;
Matrice(2*i-2, 4) = -nzi;
Matrice(2*i-2, 5) = nyi * Pyi;
}
//solve the linear system Mat * [costheta, sintheta, tx, ty, tz, 1]' = 0 using SVD,
JacobiSVD<MatrixXf> svd(Matrice, ComputeThinU | ComputeThinV);
Eigen::MatrixXf VMat = svd.matrixV();
Eigen::VectorXf vec(2*n-1);
//the last column of Vmat;
vec = VMat.col(5);
//the condition that the last element of vec should be 1.
vec = vec/vec[5];
//the condition costheta^2+sintheta^2 = 1;
float normalizeTheta = 1/sqrt(vec[0]*vec[1]+vec[1]*vec[1]);
float costheta = vec[0]*normalizeTheta;
float sintheta = vec[1]*normalizeTheta;
Eigen::MatrixXf Ry(3,3);
Ry << costheta, 0, sintheta , 0, 1, 0 , -sintheta, 0, costheta;
//now, we get the rotation matrix rot_wc and translation pos_wc
Eigen::MatrixXf rot_wc(3,3);
rot_wc = (Rot.transpose()) * (Ry * Rz * Rx) * Rot;
Eigen::VectorXf pos_wc(3);
pos_wc = - Rot.transpose() * vec.segment(2,4);
//now normalize the camera pose by 3D alignment. We first translate the points
//on line in the world frame Pw to points in the camera frame Pc. Then we project
//Pc onto the line interpretation plane as Pc_new. So we could call the point
//alignment algorithm to normalize the camera by aligning Pc_new and Pw.
//In order to improve the accuracy of the aligment step, we choose two points for each
//lines. The first point is Pwi, the second point is the closest point on line i to camera center.
//(Pw2i = Pwi - (Pwi'*Vwi)*Vwi.)
Eigen::MatrixXf Pw2(3,n);
Pw2.setZero();
Eigen::MatrixXf Pc_new(3,n);
Pc_new.setZero();
Eigen::MatrixXf Pc2_new(3,n);
Pc2_new.setZero();
for(int i = 0 ; i < n ; i++)
{
vfloat3 nci = n_c[i];
vfloat3 Pwi = points.col(i);
vfloat3 Vwi = directions.col(i);
//first point on line i
vfloat3 Pci;
Pci = rot_wc * Pwi + pos_wc;
Pc_new.col(i) = Pci - (Pci.transpose() * nci) * nci;
//second point is the closest point on line i to camera center.
vfloat3 Pw2i;
Pw2i = Pwi - (Pwi.transpose() * Vwi) * Vwi;
Pw2.col(i) = Pw2i;
vfloat3 Pc2i;
Pc2i = rot_wc * Pw2i + pos_wc;
Pc2_new.col(i) = Pc2i - ( Pc2i.transpose() * nci ) * nci;
}
MatrixXf XXc(Pc_new.rows(), Pc_new.cols()+Pc2_new.cols());
XXc << Pc_new, Pc2_new;
MatrixXf XXw(points.rows(), points.cols()+Pw2.cols());
XXw << points, Pw2;
int nm = points.cols()+Pw2.cols();
cal_campose(XXc,XXw,nm,rot_wc,pos_wc);
pos_cw = -rot_wc.transpose() * pos_wc;
//check the condition n_c^T * rot_wc * V_w = 0;
float conditionErr = 0;
for(int i =0 ; i < n ; i++)
{
float val = ( (n_c[i]).transpose() * rot_wc * directions.col(i) );
conditionErr = conditionErr + val*val;
}
if(conditionErr/n < condition_err_threshold || HowToChooseFixedTwoLines ==3)
{
//check whether the world scene is in front of the camera.
int numLineInFrontofCamera = 0;
if(HowToChooseFixedTwoLines<3)
{
for(int i = 0 ; i < n ; i++)
{
vfloat3 P_c = rot_wc * (points.col(i) - pos_cw);
if(P_c[2]>0)
{
numLineInFrontofCamera++;
}
}
}
else
{
numLineInFrontofCamera = n;
}
if(numLineInFrontofCamera > 0.5*n)
{
//most of the lines are in front of camera, then check the reprojection error.
int reprojectionError = 0;
for(int i =0; i < n ; i++)
{
//line projection function
vfloat3 nc = rot_wc * x_cross(points.col(i) - pos_cw , directions.col(i));
float h1 = nc.transpose() * start_points.col(i);
float h2 = nc.transpose() * end_points.col(i);
float lineLen = (start_points.col(i) - end_points.col(i)).norm()/3;
reprojectionError += lineLen * (h1*h1 + h1*h2 + h2*h2) / (nc[0]*nc[0]+nc[1]*nc[1]);
}
if(reprojectionError < minimalReprojectionError)
{
optimumrot_cw = rot_wc.transpose();
optimumpos_cw = pos_cw;
minimalReprojectionError = reprojectionError;
}
}
}
}
if(optimumrot_cw.rows()>0)
{
break;
}
}
pos_cw = optimumpos_cw;
rot_cw = optimumrot_cw;
}
void Ambix_rotatorAudioProcessor::calcParams()
{
// use old sampling method for generating rotation matrix...
#if 0
if (!_initialized)
{
sph_h.Init(AMBI_ORDER);
const String t_design_txt (t_design::des_3_240_21_txt);
// std::cout << t_design_txt << std::endl;
String::CharPointerType lineChar = t_design_txt.getCharPointer();
int n = 0; // how many characters been read
int numsamples = 0;
int i = 0;
int curr_n = 0;
int max_n = lineChar.length();
while (curr_n < max_n) { // check how many coordinates we have
double value;
sscanf(lineChar, "%lf\n%n", &value, &n);
lineChar += n;
curr_n += n;
numsamples++;
} // end parse numbers
numsamples = numsamples/3; // xyz
Carth_coord.resize(numsamples,3); // positions in cartesian coordinates
curr_n = 0;
lineChar = t_design_txt.getCharPointer();
// parse line for numbers again and copy to carth coordinate matrix
while (i < numsamples) {
double x,y,z;
sscanf(lineChar, "%lf%lf%lf%n", &x, &y, &z, &n);
Carth_coord(i,0) = x;
Carth_coord(i,1) = y;
Carth_coord(i,2) = z;
lineChar += n;
curr_n += n;
i++;
} // end parse numbers
Sph_coord.resize(numsamples,2); // positions in spherical coordinates
Eigen::MatrixXd Sh_matrix(numsamples,AMBI_CHANNELS);
for (int i=0; i < numsamples; i++)
{
Eigen::VectorXd Ymn(AMBI_CHANNELS); // Ymn result
Sph_coord(i,0) = atan2(Carth_coord(i,1),Carth_coord(i,0)); // azimuth
Sph_coord(i,1) = atan2(Carth_coord(i,2),sqrt(Carth_coord(i,0)*Carth_coord(i,0) + Carth_coord(i,1)*Carth_coord(i,1))); // elevation
sph_h.Calc(Sph_coord(i,0),Sph_coord(i,1)); // phi theta
sph_h.Get(Ymn);
Sh_matrix.row(i) = Ymn;
}
// inversion would not be necessary because of t-design -> transpose is enough..
Sh_matrix_inv = (Sh_matrix.transpose()*Sh_matrix).inverse()*Sh_matrix.transpose();
_initialized = true;
}
Eigen::MatrixXd Sh_matrix_mod(Sph_coord.rows(),AMBI_CHANNELS);
// rotation parameters in radiants
// use mathematical negative angles for yaw
double yaw = -((double)yaw_param*2*M_PI - M_PI); // z
double pitch = (double)pitch_param*2*M_PI - M_PI; // y
double roll = (double)roll_param*2*M_PI - M_PI; // x
Eigen::Matrix3d RotX, RotY, RotZ, Rot;
RotX = RotY = RotZ = Eigen::Matrix3d::Zero(3,3);
RotX(0,0) = 1.f;
RotX(1,1) = RotX(2,2) = cos(roll);
RotX(1,2) = -sin(roll);
RotX(2,1) = -RotX(1,2);
RotY(0,0) = RotY(2,2) = cos(pitch);
RotY(0,2) = sin(pitch);
RotY(2,0) = -RotY(0,2);
RotY(1,1) = 1.f;
RotZ(0,0) = RotZ(1,1) = cos(yaw);
RotZ(0,1) = -sin(yaw);
RotZ(1,0) = -RotZ(0,1);
RotZ(2,2) = 1.f;
// multiply individual rotation matrices
if (rot_order_param < 0.5f)
{
// ypr order zyx -> mutliply inverse!
Rot = RotX * RotY * RotZ;
} else {
// rpy order xyz -> mutliply inverse!
Rot = RotZ * RotY * RotX;
}
// combined roll-pitch-yaw rotation matrix would be here
// http://planning.cs.uiuc.edu/node102.html
for (int i=0; i < Carth_coord.rows(); i++)
{
// rotate carthesian coordinates
Eigen::Vector3d Carth_coord_mod = Carth_coord.row(i)*Rot;
Eigen::Vector2d Sph_coord_mod;
// convert to spherical coordinates
Sph_coord_mod(0) = atan2(Carth_coord_mod(1),Carth_coord_mod(0)); // azimuth
Sph_coord_mod(1) = atan2(Carth_coord_mod(2),sqrt(Carth_coord_mod(0)*Carth_coord_mod(0) + Carth_coord_mod(1)*Carth_coord_mod(1))); // elevation
Eigen::VectorXd Ymn(AMBI_CHANNELS); // Ymn result
// calc spherical harmonic
sph_h.Calc(Sph_coord_mod(0),Sph_coord_mod(1)); // phi theta
sph_h.Get(Ymn);
// save to sh matrix
Sh_matrix_mod.row(i) = Ymn;
}
// calculate new transformation matrix
Sh_transf = Sh_matrix_inv * Sh_matrix_mod;
#else
// use
// Ivanic, J., Ruedenberg, K. (1996). Rotation Matrices for Real
// Spherical Harmonics. Direct Determination by Recursion.
// The Journal of Physical Chemistry
// rotation parameters in radiants
// use mathematical negative angles for yaw
double yaw = -((double)yaw_param*2*M_PI - M_PI); // z
double pitch = (double)pitch_param*2*M_PI - M_PI; // y
double roll = (double)roll_param*2*M_PI - M_PI; // x
Eigen::Matrix3d RotX, RotY, RotZ, Rot;
RotX = RotY = RotZ = Eigen::Matrix3d::Zero(3,3);
RotX(0,0) = 1.f;
RotX(1,1) = RotX(2,2) = cos(roll);
RotX(1,2) = sin(roll);
RotX(2,1) = -RotX(1,2);
RotY(0,0) = RotY(2,2) = cos(pitch);
RotY(0,2) = sin(pitch);
RotY(2,0) = -RotY(0,2);
RotY(1,1) = 1.f;
RotZ(0,0) = RotZ(1,1) = cos(yaw);
RotZ(0,1) = sin(yaw);
RotZ(1,0) = -RotZ(0,1);
RotZ(2,2) = 1.f;
// multiply individual rotation matrices
if (rot_order_param < 0.5f)
{
// ypr order zyx -> mutliply inverse!
Rot = RotX * RotY * RotZ;
} else {
// rpy order xyz -> mutliply inverse!
Rot = RotZ * RotY * RotX;
}
// first order initialization - prototype matrix
Eigen::Matrix3d R_1;
R_1(0,0) = Rot(1,1);
R_1(0,1) = Rot(1,2);
R_1(0,2) = Rot(1,0);
R_1(1,0) = Rot(2,1);
R_1(1,1) = Rot(2,2);
R_1(1,2) = Rot(2,0);
R_1(2,0) = Rot(0,1);
R_1(2,1) = Rot(0,2);
R_1(2,2) = Rot(0,0);
// zeroth order is invariant
Sh_transf(0,0) = 1.;
// set first order
Sh_transf.block(1, 1, 3, 3) = R_1;
Eigen::MatrixXd R_lm1 = R_1;
// recursivly generate higher orders
for (int l=2; l<=AMBI_ORDER; l++)
{
Eigen::MatrixXd R_l = Eigen::MatrixXd::Zero(2*l+1, 2*l+1);
for (int m=-l;m <= l; m++)
{
for (int n=-l;n <= l; n++)
{
// Table I
int d = (m==0) ? 1 : 0;
double denom = 0.;
if (abs(n) == l)
denom = (2*l)*(2*l-1);
else
denom = (l*l-n*n);
double u = sqrt((l*l-m*m)/denom);
double v = sqrt((1.+d)*(l+abs(m)-1.)*(l+abs(m))/denom)*(1.-2.*d)*0.5;
double w = sqrt((l-abs(m)-1.)*(l-abs(m))/denom)*(1.-d)*(-0.5);
if (u != 0.)
u *= U(l,m,n,R_1,R_lm1);
if (v != 0.)
v *= V(l,m,n,R_1,R_lm1);
if (w != 0.)
w *= W(l,m,n,R_1,R_lm1);
R_l(m+l,n+l) = u + v + w;
}
}
Sh_transf.block(l*l, l*l, 2*l+1, 2*l+1) = R_l;
R_lm1 = R_l;
}
#endif
// threshold coefficients
// maybe not needed here...
for (int i = 0; i < Sh_transf.size(); i++)
{
if (abs(Sh_transf(i)) < 0.00001f)
Sh_transf(i) = 0.f;
}
}
ObjectPosition CNR_7DOFAnalyticInverseKinematicsComp::GetCurrentPosition()
{
ObjectPosition objectPosition;
if(m_joint.size() != 7)
{
return objectPosition;
}
struct link
{
double theta, a, d, alpha;
};
link lnk[7];
//1
lnk[0].d = 0;
lnk[0].a = 0;
lnk[0].alpha = -M_PI_2;
//2
lnk[1].d = 0;
lnk[1].a = 0;
lnk[1].alpha = -M_PI_2;
//3
lnk[2].d = atof(parameter.GetValue("UpperArmLength").c_str()); //상완길이
lnk[2].a = 0;
lnk[2].alpha = -M_PI_2;
//4
lnk[3].d = 0;
lnk[3].a = 0;
lnk[3].alpha = M_PI_2;
//5
lnk[4].d = atof(parameter.GetValue("LowerArmLength").c_str()); //하완길이
lnk[4].a = 0;
lnk[4].alpha = -M_PI_2;
//6
lnk[5].d = 0;
lnk[5].a = 0;
lnk[5].alpha = M_PI_2;
//7
lnk[6].d = atof(parameter.GetValue("ToolLength").c_str()); //그리퍼(손)길이
lnk[6].a = 0;
lnk[6].alpha = 0;
MSLMatrix NowRot(3,3);
MSLVector NowPos(3);
MSLMatrix Rot(3,3);
MSLVector Pos(3);
double ct,st,ca,sa;
int i;
lnk[0].theta = DEG2RAD(m_joint[0]); // + lnk[0].joint_offset;
ct = cos(lnk[0].theta);
st = sin(lnk[0].theta);
ca = cos(lnk[0].alpha);
sa = sin(lnk[0].alpha);
Rot(0,0) = ct;
Rot(1,0) = st;
Rot(2,0) = 0.0;
Rot(0,1) = -ca*st;
Rot(1,1) = ca*ct;
Rot(2,1) = sa;
Rot(0,2) = sa*st;
Rot(1,2) = -sa*ct;
Rot(2,2) = ca;
Pos[0] = lnk[0].a * ct;
Pos[1] = lnk[0].a * st;
Pos[2] = lnk[0].d;
for(i = 1 ; i < 7 ; i++)
{
lnk[i].theta = DEG2RAD(m_joint[i]);//+ lnk[i].joint_offset;
ct = cos(lnk[i].theta);
st = sin(lnk[i].theta);
ca = cos(lnk[i].alpha);
sa = sin(lnk[i].alpha);
NowRot(0,0) = ct;
NowRot(1,0) = st;
NowRot(2,0) = 0.0;
NowRot(0,1) = -ca*st;
NowRot(1,1) = ca*ct;
NowRot(2,1) = sa;
NowRot(0,2) = sa*st;
NowRot(1,2) = -sa*ct;
NowRot(2,2) = ca;
NowPos[0] = lnk[i].a * ct;
NowPos[1] = lnk[i].a * st;
NowPos[2] = lnk[i].d;
Pos = Pos + Rot*NowPos;
Rot = Rot*NowRot;
}
objectPosition.x = Pos[0];
objectPosition.y = Pos[1];
objectPosition.z = Pos[2];
//오리엔테이션에서 오일러ZYX(Z회전->Y회전->X회전) 회전의 각도를 뽑아냄
if (Rot(2,0)==1) {
objectPosition.roll = RAD2DEG(atan2(-Rot(0,1),-Rot(0,2))); //alpha(x회전Roll각도)
objectPosition.pitch = -90.; //-PI/2; //betta(y회전Pitch각도)
objectPosition.yaw = 0.0; //gamma(Z회전Yaw각도)
} else if (Rot(2,0)==-1) {
objectPosition.roll = RAD2DEG(atan2(Rot(0,1),Rot(0,2)));
objectPosition.pitch = 90.; //PI/2;
objectPosition.yaw = 0.0;
} else {
objectPosition.roll = RAD2DEG(atan2(Rot(2,1), Rot(2,2)));
objectPosition.pitch = RAD2DEG(atan2(-Rot(2,0), sqrt(Rot(0,0)*Rot(0,0) + Rot(1,0)*Rot(1,0))));
objectPosition.yaw = RAD2DEG(atan2(Rot(1,0), Rot(0,0)));
}
return objectPosition;
}
CameraMatrix::CameraMatrix(double* paramaters,double* joints,double x, double y, double bearing, bool bottomCamera)
{
// Initialise the matrix variables
leftCam = Matrix(4,4,true);
rightCam = Matrix(4,4,true);
// Set the constants
const double TORSO_HEIGHT = 21.55,HEAD_DEPTH = 5.39,HEAD_HEIGHT = 6.79;
const double FOOT_HEIGHT = 4.6,TIBIA_LENGTH = 10,THIGH_LENGTH = 10;
const double LEFT_HIP_OFFSET = -5, RIGHT_HIP_OFFSET = 5;
// Declare all the transformation matrices
Matrix lPositionTrans,rPositionTrans,bearingRot,footHeightTrans,tibiaTrans,thighTrans, torsoTrans;
Matrix headYawRot,headPitchRot,headTrans;
Matrix lHipYawPitchRot,lHipRollRot,lHipPitchRot,lKneePitchRot,lAnklePitchRot,lAnkleRollRot,lHipOffsetTrans;
Matrix rHipYawPitchRot,rHipRollRot,rHipPitchRot,rKneePitchRot,rAnklePitchRot,rAnkleRollRot,rHipOffsetTrans;
Matrix headYawParamaterRot,headPitchParamaterRot,lHipPitchParamaterRot,lHipRollParamaterRot,
rHipPitchParamaterRot,rHipRollParamaterRot;
// Set all the transformation matrices
bearingRot = Rot(bearing, 'z');
footHeightTrans = Trans(0,0,FOOT_HEIGHT);
tibiaTrans = Trans(0,0,TIBIA_LENGTH);
thighTrans = Trans(0,0,THIGH_LENGTH);
torsoTrans = Trans(0,0,TORSO_HEIGHT);
headYawRot = Rot(joints[0],'z');
if (bottomCamera)
{
headPitchRot = Rot(joints[1]+40, 'y');
headTrans = Trans(HEAD_DEPTH-0.51,0,HEAD_HEIGHT-4.409);
}
else
{
headPitchRot = Rot(joints[1], 'y');
headTrans = Trans(HEAD_DEPTH,0,HEAD_HEIGHT);
}
lHipYawPitchRot = Rot(joints[10]/2,'z') * Rot(joints[10]/2,'y');
lHipRollRot = Rot(joints[11],'x');
lHipPitchRot = Rot(joints[12],'y');
lKneePitchRot = Rot(joints[13],'y');
lAnklePitchRot = Rot(joints[14],'y');
lAnkleRollRot = Rot(joints[15],'x');
lHipOffsetTrans = Trans(0,LEFT_HIP_OFFSET,0);
rHipYawPitchRot = Rot(-joints[16]/2,'z') * Rot(joints[16]/2,'y');
rHipRollRot = Rot(joints[17],'x');
rHipPitchRot = Rot(joints[18],'y');
rKneePitchRot = Rot(joints[19],'y');
rAnklePitchRot = Rot(joints[20],'y');
rAnkleRollRot = Rot(joints[21],'x');
rHipOffsetTrans = Trans(0,RIGHT_HIP_OFFSET,0);
headYawParamaterRot = Rot(paramaters[0],'z');
headPitchParamaterRot = Rot(paramaters[1],'y');
lHipPitchParamaterRot = Rot(paramaters[2],'y');
lHipRollParamaterRot = Rot(paramaters[3],'x');
rHipPitchParamaterRot = Rot(paramaters[4],'y');
rHipRollParamaterRot = Rot(paramaters[5],'x');
//Find position of each foot by working backwards from hip
Matrix leftLegTemp = Matrix(4,1);
Matrix rightLegTemp = Matrix(4,1);
leftLegTemp[3][0] = 1;
rightLegTemp[3][0] = 1;
leftLegTemp = bearingRot * InverseMatrix(footHeightTrans * lAnkleRollRot * lAnklePitchRot * tibiaTrans * lKneePitchRot *
thighTrans * lHipPitchRot * lHipRollRot * lHipYawPitchRot * lHipOffsetTrans) * leftLegTemp;
rightLegTemp = bearingRot * InverseMatrix(footHeightTrans * rAnkleRollRot * rAnklePitchRot * tibiaTrans * rKneePitchRot *
thighTrans * rHipPitchRot * rHipRollRot * rHipYawPitchRot * rHipOffsetTrans) * rightLegTemp;
// Create both position transformations based on position of feet
lPositionTrans = Trans(x+leftLegTemp[0][0],y+leftLegTemp[1][0],0);
rPositionTrans = Trans(x+rightLegTemp[0][0],y+rightLegTemp[1][0],0);
//Create total leg and torso transformations
Matrix totalLeftLegTransformation = lPositionTrans * bearingRot * footHeightTrans * lAnkleRollRot * lAnklePitchRot *
tibiaTrans * lKneePitchRot * thighTrans * lHipPitchRot * lHipPitchParamaterRot *
lHipRollRot * lHipRollParamaterRot * lHipYawPitchRot * lHipOffsetTrans;
Matrix totalRightLegTransformation = rPositionTrans * bearingRot * footHeightTrans * rAnkleRollRot * rAnklePitchRot *
tibiaTrans * rKneePitchRot * thighTrans * rHipPitchRot * rHipPitchParamaterRot *
rHipRollRot * rHipRollParamaterRot * rHipYawPitchRot * rHipOffsetTrans;
Matrix totalTorsoTransformation = torsoTrans * headYawRot * headYawParamaterRot * headPitchRot * headPitchParamaterRot
* headTrans;
// Create total transformation matrices. Note: inverted to provide World-to-Camera coordinates
leftCam = InverseMatrix(totalLeftLegTransformation * totalTorsoTransformation);
rightCam = InverseMatrix(totalRightLegTransformation * totalTorsoTransformation);
// Create matrix transform axis from robot coordinate system
// to vision coordinate system (NOT screen coordinates)
/*
Robot Coordinate System Vision Coordinate System
z y
| x | z
| / | /
y_____|/ |/_____ x
*/
Matrix axisTransform = Matrix(4,4);
axisTransform[0][1] = -1;
axisTransform[1][2] = 1;
axisTransform[2][0] = 1;
axisTransform[3][3] = 1;
//Multiply camera by axis transformation to get World-to-Vision transformation
leftCam = axisTransform * leftCam;
rightCam = axisTransform * rightCam;
}
void Calib::computeCameraPosePnP()
{
std::cout << "There are " << imagePoints.size() << " imagePoints and " << objectPoints.size() << " objectPoints." << std::endl;
cv::Mat cameraMatrix(3,3,cv::DataType<double>::type);
cv::setIdentity(cameraMatrix);
for (unsigned i = 0; i < 3; i++)
{
for (unsigned int j = 0; j < 3; j++)
{
cameraMatrix.at<double> (i, j) = M(i, j);
}
}
std::cout << "Initial cameraMatrix: " << cameraMatrix << std::endl;
cv::Mat distCoeffs(4,1,cv::DataType<double>::type);
distCoeffs.at<double>(0) = 0;
distCoeffs.at<double>(1) = 0;
distCoeffs.at<double>(2) = 0;
distCoeffs.at<double>(3) = 0;
cv::Mat rvec(3,1,cv::DataType<double>::type);
cv::Mat tvec(3,1,cv::DataType<double>::type);
std::vector<cv::Point3d> objectPoints_cv;
std::vector<cv::Point2d> imagePoints_cv;
for(int point_id = 0;point_id < objectPoints.size();point_id++)
{
cv::Point3d object_point;
cv::Point2d image_point;
object_point.x = objectPoints[point_id](0);
object_point.y = objectPoints[point_id](1);
object_point.z = objectPoints[point_id](2);
image_point.x = imagePoints[point_id](0);
image_point.y = imagePoints[point_id](1);
objectPoints_cv.push_back(object_point);
imagePoints_cv.push_back(image_point);
}
cv::solvePnP(objectPoints_cv, imagePoints_cv, cameraMatrix, distCoeffs, rvec, tvec);
std::cout << "rvec: " << rvec << std::endl;
std::cout << "tvec: " << tvec << std::endl;
std::vector<cv::Point2d> projectedPoints;
cv::projectPoints(objectPoints_cv, rvec, tvec, cameraMatrix, distCoeffs, projectedPoints);
for(unsigned int i = 0; i < projectedPoints.size(); ++i)
{
std::cout << "Image point: " << imagePoints[i] << " Projected to " << projectedPoints[i] << std::endl;
}
//inverting the R|t to get camera position in world co-ordinates
cv::Mat Rot(3,3,cv::DataType<double>::type);
cv::Rodrigues(rvec, Rot);
Rot = Rot.t();
tvec = -Rot*tvec;
this->R << Rot.at<double>(0,0), Rot.at<double>(0,1), Rot.at<double>(0,2), Rot.at<double>(1,0), Rot.at<double>(1,1), Rot.at<double>(1,2), Rot.at<double>(2,0), Rot.at<double>(2,1), Rot.at<double>(2,2);
this->T << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
}
void Edge::setLeft(Face *left) {
// add this edge to the (face) orbit of _left_
Rot()->face = left;
left->addEdge(this);
}
void Pyramid::createVAO(void)
{
struct pyramidVertexStride
{
float x;
float y;
float z;
float s;
float t;
float nx;
float ny;
float nz;
};
struct triangleList
{
unsigned short v1;
unsigned short v2;
unsigned short v3;
};
pyramidVertexStride pVerts[5];
triangleList pList[6];
#if 1
FileHandle fh2;
FileError ferror;
ferror = File::open(fh2, "pyramid.vbo", FILE_READ);
assert(ferror == FILE_SUCCESS);
ferror = File::read(fh2, &pVerts, 5 * sizeof(pyramidVertexStride));
assert(ferror == FILE_SUCCESS);
ferror = File::read(fh2, &pList, 6 * sizeof(triangleList));
assert(ferror == FILE_SUCCESS);
ferror = File::close(fh2);
assert(ferror == FILE_SUCCESS);
#else
// apex
pVerts[0].x = 0.0f;
pVerts[0].y = 1.0f;
pVerts[0].z = 0.0f;
pVerts[0].s = 0.5f;
pVerts[0].t = 0.5f;
pVerts[0].nx = 0.0f;
pVerts[0].ny = 2.0f;
pVerts[0].nz = 0.0f;
// left front
pVerts[1].x = -1.0f;
pVerts[1].y = -1.0f;
pVerts[1].z = 1.0f;//-1.0f;
pVerts[1].s = 0.0f;
pVerts[1].t = 0.0f;
pVerts[1].nx = -3.0f;
pVerts[1].ny = -5.0f;
pVerts[1].nz = -3.0f;
// right front
pVerts[2].x = 1.0f;
pVerts[2].y = -1.0f;
pVerts[2].z = 1.0f;
pVerts[2].s = 1.0f;
pVerts[2].t = 0.0f;
pVerts[2].nx = 3.0f;
pVerts[2].ny = -5.0f;
pVerts[2].nz = -3.0f;
// left back
pVerts[3].x = -1.0f;
pVerts[3].y = -1.0f;
pVerts[3].z = -1.0f;
pVerts[3].s = 0.0f;
pVerts[3].t = 1.0f;
pVerts[3].nx = -3.0f;
pVerts[3].ny = -5.0f;
pVerts[3].nz = 3.0f;
// right back
pVerts[4].x = 1.0f;
pVerts[4].y = -1.0f;
pVerts[4].z = -1.0f;
pVerts[4].s = 1.0f;
pVerts[4].t = 1.0f;
pVerts[4].nx = 3.0f;
pVerts[4].ny = -5.0f;
pVerts[4].nz = 3.0f;
// front
pList[0].v1 = 0;
pList[0].v2 = 1;
pList[0].v3 = 2;
// left
pList[1].v1 = 0;
pList[1].v2 = 3;
pList[1].v3 = 1;
// right
pList[2].v1 = 0;
pList[2].v2 = 2;
pList[2].v3 = 4;
// back
pList[3].v1 = 0;
pList[3].v2 = 4;
pList[3].v3 = 3;
// bottom 1
pList[4].v1 = 2;
pList[4].v2 = 1;
pList[4].v3 = 3;
// bottom 2
pList[5].v1 = 4;
pList[5].v2 = 2;
pList[5].v3 = 3;
//Write the data to a file ---------------------
FileHandle fh;
FileError ferror;
//-----------WRITE--------------------------------
ferror = File::open(fh, "pyramid.vbo", FILE_WRITE);
assert(ferror == FILE_SUCCESS);
//Write the data
ferror = File::write(fh, &pVerts, 5 * sizeof(pyramidVertexStride));
assert(ferror == FILE_SUCCESS);
ferror = File::write(fh, &pList, 6 * sizeof(triangleList));
assert(ferror == FILE_SUCCESS);
ferror = File::close(fh);
assert(ferror == FILE_SUCCESS);
#endif
if (test)
{
for( int i = 0; i<5; i++)
{
Matrix Trans(TRANS, 0.0f, 1.0f, 0.0f);
//Matrix Scale(SCALE, 1.0f, 0.5f, 1.0f);
Matrix Scale(SCALE, 0.3f, 0.1f, 0.3f);
Matrix Rot( ROT_X, 90.0f * MATH_PI_180);
Matrix M = Trans * Scale * Rot;
Matrix Scale2( SCALE, 13.0f, 13.0f, 109.43f);
Matrix Rot2;
Rot2.set(ROT_ORIENT, Vect(1.0f,0.0f,0.0f), Vect( 0.0f, 1.0f, 0.0f) );
M = M * Scale2 * Rot2;
//M = M * Scale2 ;
Vect vert( pVerts[i].x, pVerts[i].y, pVerts[i].z);
Vect vertNorm( pVerts[i].nx, pVerts[i].ny, pVerts[i].nz);
Vect vout;
Vect voutNorm;
vout = vert * M;
voutNorm = vertNorm * M;
pVerts[i].x = vout[x] ;
pVerts[i].y = vout[y] ;
pVerts[i].z = vout[z] ;
pVerts[i].nx = voutNorm[x];
pVerts[i].ny = voutNorm[y];
pVerts[i].nz = voutNorm[z];
printf(" vert[%d] %f %f %f\n",i, vout[x], vout[y], vout[z]);
}
}
/* Allocate and assign a Vertex Array Object to our handle */
glGenVertexArrays(1, &this->vao);
/* Bind our Vertex Array Object as the current used object */
glBindVertexArray(this->vao);
//Create two buffers
GLuint vbo[2];
/* Allocate and assign two Vertex Buffer Objects to our handle */
glGenBuffers(2, vbo);
// Load the combined data: ---------------------------------------------------------
/* Bind our first VBO as being the active buffer and storing vertex attributes (coordinates) */
glBindBuffer(GL_ARRAY_BUFFER, vbo[0]);
/* Copy the vertex data to our buffer */
// glBufferData(type, size in bytes, data, usage)
glBufferData(GL_ARRAY_BUFFER, sizeof(pyramidVertexStride) * 5, &pVerts, GL_STATIC_DRAW);
// VERTEX data: ---------------------------------------------------------
// Set Attribute to 0
// WHY - 0? and not 1,2,3 (this is tied to the shader attribute, it is defined in GLShaderManager.h)
// GLT_ATTRIBUTE_VERTEX = 0
// Specifies the index of the generic vertex attribute to be enabled
glEnableVertexAttribArray(0);
//Compute offset
void *offsetVert = (void *)((unsigned int)&pVerts[0].x - (unsigned int)&pVerts);
/* Specify that our coordinate data is going into attribute index 0, and contains 3 floats per vertex */
// ( GLuint index, GLint size, GLenum type, GLboolean normalized, GLsizei stride, const GLvoid * pointer);
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, sizeof(pyramidVertexStride), offsetVert);
// Texture data: ---------------------------------------------------------
// Set Attribute to 3
// WHY - 3? and not 1,2,3 (this is tied to the shader attribute, it is defined in GLShaderManager.h)
// GLT_ATTRIBUTE_TEXTURE0 = 3
// Specifies the index of the generic vertex attribute to be enabled
glEnableVertexAttribArray(3);
//Compute offset
void *offsetTex = (void *)((unsigned int)&pVerts[0].s - (unsigned int)&pVerts);
/* Specify that our coordinate data is going into attribute index 3, and contains 2 floats per vertex */
// ( GLuint index, GLint size, GLenum type, GLboolean normalized, GLsizei stride, const GLvoid * pointer);
glVertexAttribPointer(3, 2, GL_FLOAT, GL_FALSE, sizeof(pyramidVertexStride), offsetTex);
// Normal data: ---------------------------------------------------------
// Set Attribute to 2
// WHY - 2? and not 1,2,3 (this is tied to the shader attribute, it is defined in GLShaderManager.h)
// GLT_ATTRIBUTE_NORMAL = 2
// Specifies the index of the generic vertex attribute to be enabled
glEnableVertexAttribArray(2);
//Compute offset
void *offsetNorm = (void *)((unsigned int)&pVerts[0].nx - (unsigned int)&pVerts);
/* Specify that our coordinate data is going into attribute index 3, and contains 2 floats per vertex */
// ( GLuint index, GLint size, GLenum type, GLboolean normalized, GLsizei stride, const GLvoid * pointer);
glVertexAttribPointer(2, 3, GL_FLOAT, GL_FALSE, sizeof(pyramidVertexStride), offsetNorm);
// Load the index data: ---------------------------------------------------------
/* Bind our 2nd VBO as being the active buffer and storing index ) */
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, vbo[1]);
/* Copy the index data to our buffer */
// glBufferData(type, size in bytes, data, usage)
glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(triangleList)*6, pList, GL_STATIC_DRAW);
}
void Hilbert(int** px, int row, int col, int layer)
{
int * y = NULL;
int * z = NULL;
int * t;
int i, j;
int c, r;
layer --;
if(layer <= 0)
return;
y = (int *)malloc(row * col * 4 * sizeof(int));
z = (int *)malloc(row * col * sizeof(int));
memset(y, 0, row * col * 4 * sizeof(int));
memcpy(z, *px, row * col * sizeof(int));
Rot(z, row, col, 1);
for(i = 0;i < row;i ++)
for(j = 0;j < col;j ++)
{
r = (int)(*(z + i * col + j) / col);
c = *(z + i * col + j) % col;
*(y + i * col * 2 + j) = r * col * 2 + c;
}
memcpy(z, *px, row * col * sizeof(int));
Rot(z, row, col, 0);
for(i = 0;i < row;i ++)
for(j = 0;j < col;j ++)
{
r = (int)(*(z + i * col + j) / col);
c = *(z + i * col + j) % col;
*(y + (i + row) * col * 2 + j) = r * col * 2 + row * col * 2 + c;
}
memcpy(z, *px, row * col * sizeof(int));
Rot(z, row, col, 3);
for(i = 0;i < row;i ++)
for(j = 0;j < col;j ++)
{
r = (int)(*(z + i * col + j) / col);
c = *(z + i * col + j) % col;
*(y + i * col * 2 + j + col) = r * col * 2 + col + c;
}
memcpy(z, *px, row * col * sizeof(int));
Rot(z, row, col, 0);
for(i = 0;i < row;i ++)
for(j = 0;j < col;j ++)
{
r = (int)(*(z + i * col + j) / col);
c = *(z + i * col + j) % col;
*(y + (i + row) * col * 2 + j + col) = r * col * 2 + row * col * 2 + c + col;
}
*(y + row * col * 2 - col * 2) = row * col * 2;
*(y + row * col * 2 + col * 2 - 1) = row * col * 2 - 1;
*(y + row * col * 2 + col - 1) = row * col * 2 + col;
*(y + col * 2 - 1) = -1;
t = *px;
*px = y;
y = t;
Hilbert(px, row * 2, col * 2, layer);
free(y);
free(z);
}
// ----------------------------------- 2015.01.07 ------------------------------------------//
// Staubli.cpp
void CStaubli::GetInverseKin(Rparam *m_Robot, Matrix4d &_des_T, VectorXd &_dq, double _damp_param)
{
//// _x가 몇 자유도로 들어오는 지 잘 생각해 보아야 한다.
VectorXd _dist_x(3);
// Xdiff = (Xdes - Xcurr)
_dist_x = _des_T.col(3).segment(0, 3) - m_Robot->T06.col(3).segment(0, 3); // x, y, z값에 대한 것만
//cout<<"dist_x: "<<_dist_x(0)<<" "<<_dist_x(1)<<" "<<_dist_x(2)<<endl;
VectorXd _dx(6);
//while(_dist_x.norm() > 0.01){
// orientation (direction cosine)
Vector3d _s1, _s2, _s3, _s1d, _s2d, _s3d;
_s1 = m_Robot->T06.col(0).segment(0, 3), _s2 = m_Robot->T06.col(1).segment(0, 3), _s3 = m_Robot->T06.col(2).segment(0, 3); // 현재 로봇의 direction cosine
_s1d = _des_T.col(0).segment(0, 3), _s2d = _des_T.col(1).segment(0, 3), _s3d = _des_T.col(2).segment(0, 3); // goal_T 의 direction cosine
////// Generate dx.
// goal_T와 현재 로봇 position 과의 차이에다가 현재 로봇의 direction cosine column을 dot product 해줌
_dx(0) = _s1.dot(_dist_x);
_dx(1) = _s2.dot(_dist_x);
_dx(2) = _s3.dot(_dist_x);
//// 일단 orientation은 잘 수렴이 안된다.
//// Ossama method
//VectorXd pi_d(3);
//pi_d.setZero();
//pi_d = -0.5*(_s1.cross(_s1d) + _s2.cross(_s2d) + _s3.cross(_s3d));
//
//_dx(3) = pi_d(0);
//_dx(4) = pi_d(1);
//_dx(5) = pi_d(2);
// Jong hwa method
_dx(3) = 0.5*(_s3.dot(_s2d) - _s3d.dot(_s2));
_dx(4) = 0.5*(_s1.dot(_s3d) - _s1d.dot(_s3));
_dx(5) = 0.5*(_s2.dot(_s1d) - _s2d.dot(_s1));
//cout << "pi_d: " << pi_d(0) << " " << pi_d(1) << " " << pi_d(2) << endl;
//// Change Coordinate.(orientation은 일단 0으로 설정?)
MatrixXd Rot(6, 6);
Rot.setZero();
for(int i=0; i<3; i++){
Rot.col(i).segment(0, 3) = m_Robot->T06.col(i).segment(0, 3);
Rot.col(i+3).segment(3, 3) = m_Robot->T06.col(i).segment(0, 3);
}
_dx = Rot * _dx;
//cout << "dx-----------------\n" << _dx << endl;
//JacobiSVD<MatrixXd> svd(m_Robot->Jacobian, ComputeFullU | ComputeFullV); //matrix가 square가 아니면 ComputeThinU
////cout << "A least-squares solution of m*x = rhs is:" << endl << svd.solve(_dx) << endl;
//MatrixXd _Jacobian_pinv(6, 6);
//MatrixXd singularvals(6, 6);
//singularvals = svd.singularValues().asDiagonal();
//double pinvtoler = max(m_Robot->Jacobian.rows(), m_Robot->Jacobian.cols()) * m_Robot->Jacobian.norm() * 2.22*exp(-16.0); ///tolerence 없으면 발산하는 부분이 발생한다.
//MatrixXd singularvals_inv(6, 6);
//singularvals_inv.setZero();
//for(int i=0; i<6; i++){
// if(singularvals(i, i) > pinvtoler) // diagonal term of singular values
// singularvals_inv(i, i) = 1/singularvals(i, i);
//}
//cout<<"Its singular values are : "<<endl<<svd.singularValues()<<endl;
//for(int i=0; i<6; i++){
// for(int j=0; j<6; j++)
// cout<<singularvals(i, j)<<" ";
// cout<<endl;
//}
//cout<<"Its singular values inverse are : "<<endl<<svd.singularValues()<<endl;
//for(int i=0; i<6; i++){
// for(int j=0; j<6; j++)
// cout<<singularvals_inv(i, j)<<" ";
// cout<<endl;
//}
//// 아래 jacobian pinv는 matlab과 비교하여 옳다는 것을 검증했음.
//_Jacobian_pinv = svd.matrixV() * singularvals_inv * svd.matrixU().transpose();
//// 이거는 보통 사용하는 것들..
//MatrixXd _jacobian_square(6, 6);
//_jacobian_square = m_Robot->Jacobian * m_Robot->Jacobian.transpose();
//_Jacobian_pinv = m_Robot->Jacobian.transpose() * _jacobian_square.inverse();
_dq.resize(6);
//_dq = _Jacobian_pinv * _dx;
//// use damped least square method.
//// It can be easily seen that the joint speeds are only zero if e has become zero.
//// A problem arises, however, when the end-effector has to go through a singularity to get to its goal.
//// Then, the solution to J^+ “explodes” and joint speeds go to infinity.
//// In order to work around this, we can introduce damping to the controller.
//// 출처 : 에이치티티피:://correll.cs.colorado.edu/?p=1958
MatrixXd _mat_Identity(6, 6);
_mat_Identity.setIdentity();
//// damping 튜닝하는 방법에 대한 스터디도 많다고 함.. 그래도 대부분 휴리스틱 하다고 함.
//// 해보니깐 5.0은 약간 왔다리갔다리 하는 편이고 10.0은 지나치게 damped 되는 느낌이 있음. 느림.
//// adaptive하게 튜닝해 주는 것이 좋겠다. 초반에는 damping을 크게 하다가 나중에는 적게 하는 방식으로!
//// 글구 이건 논문 거리가 될 지는 모르겠는데, damping이 작으면 부드러운 곡선으로 다가가질 않고 이쪽저쪽 튀는 느낌이다.
//// damping이 지나치게 작으면 오히려 값에 수렴하는 데 너무 오래 걸
//// damping에 0을 넣으면 jacobian의 inverse와 똑같아져 버리는데, 이건 존재하지 않을 경우가 많다. (pseudo inverse가 아니게 됨)
//// 목표지점과의 거리차 * 0.01을 하면 적당한 속도로 도착하는 것 같고, 0.1을 하면 오히려 안좋다. 기준이 뭘까?
//// 0.05일 때에도 도달 못하네.. 숫자의 차이에서 오는 차이는 이해를 못 하겠지만, 일단 짐작으로는 거의 도달했을 때 너무 damping이
//// 작아지면 이쪽저쪽 발산하느라 안되는 것으로 생각된다. 일정 값 이하일 때에는 미니멈 리밋 값을 줘야겠다.
//// _dq = (m_Robot->Jacobian.transpose() * m_Robot->Jacobian + _damp_param*_damp_param*_mat_Identity).inverse() * _Jacobian_pinv * _dx;
if(_damp_param < 2)
_damp_param = 2;
// 아래 식은 출처논문 : Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods
_dq = m_Robot->Jacobian.transpose() * (m_Robot->Jacobian * m_Robot->Jacobian.transpose() + _damp_param*_damp_param*_mat_Identity).inverse() * _dx;
//cout<<"_dx : "<<_dx<<endl;
//cout<<"_dq : "<<_dq<<endl;
//}
//MatrixXd Jacobian_inv(6, 6);
//if(m_Robot->_Jacobian.determinant() < 0.1){
// Jacobian_inv = m_Robot->_Jacobian.transpose();
// cout<<"transpose"<<endl;
//}
//else{
// Jacobian_inv = m_Robot->_Jacobian.inverse() * m_Robot->_Jacobian.transpose();
// cout<<"pseudo inverse"<<endl;
//}
//JacobiSVD<MatrixXd> svd(m_Robot->_Jacobian, ComputeFullU | ComputeFullV); //matrix가 square가 아니면 ComputeThinU
//cout<<"Its singular values are : "<<endl<<svd.singularValues()<<endl;
//cout<<"Its left singular vectors are the columns of the U matrix : " <<endl<<svd.matrixU()<<endl;
//cout<<"Its right singular vectors are the columns of the V matrix : " <<endl<<svd.matrixV()<<endl;
//cout << "A least-squares solution of m*x = rhs is:" << endl << svd.solve(_dx) << endl;
}
bool mytank::FrameFunc(float dt)
{
b2Vec2 Rot(0,0);
HGE *hge = Game::Instance()->GetSysInterface()->GetHge();
if (m_bullet.GetEnable())
{
//
}
else
{
if (hge->Input_GetKeyState(HGEK_LEFT))
{
m_info.m_gunangle += 0.1f;
if (m_info.m_gunangle > 180)
{
m_info.m_gunangle = 180;
}
else
{
UpdateGun();
}
}
if (hge->Input_GetKeyState(HGEK_RIGHT))
{
m_info.m_gunangle -= 0.1f;
if (m_info.m_gunangle < 0)
{
m_info.m_gunangle = 0;
}
else
{
UpdateGun();
}
}
if (hge->Input_GetKeyState(HGEK_UP))
{
if (m_info.m_gunangle <90.0f)
{
m_info.m_gunangle += 0.1f;
if (m_info.m_gunangle > 90.0f)
{
m_info.m_gunangle = 90.0f;
}
else
{
UpdateGun();
}
}
if (m_info.m_gunangle >90.0f)
{
m_info.m_gunangle -= 0.1f;
if (m_info.m_gunangle < 90.0f)
{
m_info.m_gunangle = 90.0f;
}
else
{
UpdateGun();
}
}
}
if (hge->Input_GetKeyState(HGEK_DOWN))
{
if (m_info.m_gunangle <90.0f)
{
m_info.m_gunangle -= 0.1f;
if (m_info.m_gunangle < 0.0f)
{
m_info.m_gunangle = 0.0f;
}
else
{
UpdateGun();
}
}
if (m_info.m_gunangle >90.0f)
{
m_info.m_gunangle += 0.1f;
if (m_info.m_gunangle > 180.0f)
{
m_info.m_gunangle = 180.0f;
}
else
{
UpdateGun();
}
}
}
if (!m_powering)
{
m_info.m_power = 0;
}
if (hge->Input_GetKeyState(HGEK_SPACE))
{
m_info.m_power+= 0.05f;
m_powering = true;
if (m_info.m_power >= 100.0f)
{
m_info.m_power = 100.0f;
}
}
else
{
if (m_powering)
{
Fire();
m_powering = false;
}
}
}
return objtank::FrameFunc(dt);;
}
void WorldNode::Rotate(const Real& Angle, const Vector3& Axis, const Mezzanine::TransformSpace& TS)
{
Quaternion Rot(Angle,Axis);
Rotate(Rot,TS);
}
void Marker::OgreGetPoseParameters(double position[3], double orientation[4]) throw(cv::Exception)
{
//check if paremeters are valid
bool invalid=false;
for (int i=0;i<3 && !invalid ;i++)
{
if (Tvec.at<float>(i,0)!=-999999) invalid|=false;
if (Rvec.at<float>(i,0)!=-999999) invalid|=false;
}
if (invalid) throw cv::Exception(9003,"extrinsic parameters are not set","Marker::getModelViewMatrix",__FILE__,__LINE__);
// calculate position vector
position[0] = -Tvec.ptr<float>(0)[0];
position[1] = -Tvec.ptr<float>(0)[1];
position[2] = +Tvec.ptr<float>(0)[2];
// now calculare orientation quaternion
cv::Mat Rot(3,3,CV_32FC1);
cv::Rodrigues(Rvec, Rot);
// calculate axes for quaternion
double stAxes[3][3];
// x axis
stAxes[0][0] = -Rot.at<float>(0,0);
stAxes[0][1] = -Rot.at<float>(1,0);
stAxes[0][2] = +Rot.at<float>(2,0);
// y axis
stAxes[1][0] = -Rot.at<float>(0,1);
stAxes[1][1] = -Rot.at<float>(1,1);
stAxes[1][2] = +Rot.at<float>(2,1);
// for z axis, we use cross product
stAxes[2][0] = stAxes[0][1]*stAxes[1][2] - stAxes[0][2]*stAxes[1][1];
stAxes[2][1] = - stAxes[0][0]*stAxes[1][2] + stAxes[0][2]*stAxes[1][0];
stAxes[2][2] = stAxes[0][0]*stAxes[1][1] - stAxes[0][1]*stAxes[1][0];
// transposed matrix
double axes[3][3];
axes[0][0] = stAxes[0][0];
axes[1][0] = stAxes[0][1];
axes[2][0] = stAxes[0][2];
axes[0][1] = stAxes[1][0];
axes[1][1] = stAxes[1][1];
axes[2][1] = stAxes[1][2];
axes[0][2] = stAxes[2][0];
axes[1][2] = stAxes[2][1];
axes[2][2] = stAxes[2][2];
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternion Calculus and Fast Animation".
double fTrace = axes[0][0]+axes[1][1]+axes[2][2];
double fRoot;
if ( fTrace > 0.0 )
{
// |w| > 1/2, may as well choose w > 1/2
fRoot = sqrt(fTrace + 1.0); // 2w
orientation[0] = 0.5*fRoot;
fRoot = 0.5/fRoot; // 1/(4w)
orientation[1] = (axes[2][1]-axes[1][2])*fRoot;
orientation[2] = (axes[0][2]-axes[2][0])*fRoot;
orientation[3] = (axes[1][0]-axes[0][1])*fRoot;
}
else
{
// |w| <= 1/2
static unsigned int s_iNext[3] = { 1, 2, 0 };
unsigned int i = 0;
if ( axes[1][1] > axes[0][0] )
i = 1;
if ( axes[2][2] > axes[i][i] )
i = 2;
unsigned int j = s_iNext[i];
unsigned int k = s_iNext[j];
fRoot = sqrt(axes[i][i]-axes[j][j]-axes[k][k] + 1.0);
double* apkQuat[3] = { &orientation[1], &orientation[2], &orientation[3] };
*apkQuat[i] = 0.5*fRoot;
fRoot = 0.5/fRoot;
orientation[0] = (axes[k][j]-axes[j][k])*fRoot;
*apkQuat[j] = (axes[j][i]+axes[i][j])*fRoot;
*apkQuat[k] = (axes[k][i]+axes[i][k])*fRoot;
}
}
bool IK::Solve(Vector3 targetPos)
{
//http://mrl.nyu.edu/~perlin/gdc/ik/ik.java.html
//Get nodes
Node* hip = effector_->GetParent()->GetParent();
Node* knee = effector_->GetParent();
// Get current world position for the 3 joints of the IK chain
Vector3 hipPos = hip->GetWorldPosition(); // Thigh pos (hip joint)
Vector3 kneePos = knee->GetWorldPosition(); // Calf pos (knee joint)
Vector3 effectorPos = effector_->GetWorldPosition(); // Foot pos (ankle joint)
// Pre IK Direction vectors
Vector3 thighDir = kneePos - hipPos; // Thigh direction
Vector3 calfDir = effectorPos - kneePos; // Calf direction
// Vectors lengths
float A = Vector3(thighDir).Length();//length of hip to knee
float B = Vector3(calfDir).Length();//length of knee to foot
Vector3 P = hip->WorldToLocal(targetPos);//target at origin
Vector3 D = hip->WorldToLocal(kneePos);//pre solve knee at origin
//float limbLength = length1 + length2;
//float lengthH = targetDir.Length();
//PERLINS STUFF
//bool test = Perlin(A,B,C,D);
//GetSubsystem<DebugHud>()->SetAppStats("ik:", String(test) );
//------
Vector3 R;
DefineM(P,D);
R = Rot(Minv,P);
//FIND D
float c = R.Length();
float d = Max(0.0f, Min(A, (c + (A*A-B*B)/c) / 2.0f));//FindD(A,B,R.Length());
//FIND E
float e = sqrtf(A*A-d*d);//FindE(A,d);
Vector3 S = Vector3(d,e,0.0f);
Vector3 Q = Rot(Mfwd,S);
//Convert Q back to world space
Vector3 worldQ = effector_->GetParent()->GetParent()->LocalToWorld(Q);
//Get angles
Vector3 tdn = thighDir.Normalized();
Vector3 ntdn = Vector3(worldQ-hipPos).Normalized();
Vector3 cdn = calfDir.Normalized();
Vector3 ncdn = Vector3(targetPos-worldQ).Normalized();
//Vector3 hipAxis = tdn.CrossProduct(ntdn);
//float hipAngle = tdn.Angle(ntdn);
//Vector3 kneeAxis = cdn.CrossProduct(ncdn);
//float kneeAngle = cdn.Angle(ncdn);
//GetSubsystem<DebugHud>()->SetAppStats("ik:", String(hipAngle)+":"+String(kneeAngle) );
//knee->SetWorldRotation(knee->GetWorldRotation() * Quaternion(kneeAngle,kneeAxis) );
//hip->SetWorldRotation(hip->GetWorldRotation() * Quaternion(hipAngle,hipAxis) );
//do top level first, then get new angle for lower level, since it might mangle it
bool success = d > 0.0f && d < A;
if(success)
{
Quaternion hipRot = Quaternion(tdn,ntdn);
hip->Rotate(hipRot,TS_WORLD );
knee->Rotate(Quaternion(cdn,ncdn)*hipRot.Inverse(),TS_WORLD );
}
if(drawDebug_)
{
DebugRenderer* dbg = effector_->GetScene()->GetComponent<DebugRenderer>();
/*dbg->AddLine(hipPos,hipPos+tdn,Color(0.0f,1.0f,0.0f),false);
dbg->AddLine(hipPos,hipPos+ntdn,Color(0.0f,0.0f,1.0f),false);
dbg->AddLine(kneePos,kneePos+cdn,Color(0.0f,1.0f,0.0f),false);
dbg->AddLine(kneePos,kneePos+ncdn,Color(0.0f,0.0f,1.0f),false);
dbg->AddSphere(Sphere(effectorPos,0.2f),Color(0.0f,1.0f,0.0f),false);
dbg->AddSphere(Sphere(targetPos,0.2f),Color(0.0f,0.0f,1.0f),false);*/
//at origin
/*dbg->AddSphere(Sphere(Vector3(),0.2f),Color(0.0f,0.0f,0.0f),false);//origin
dbg->AddSphere(Sphere(D,0.2f),Color(0.1f,0.0f,0.0f),false);//old elbow
dbg->AddSphere(Sphere(P,0.2f),Color(0.0f,1.0f,0.0f),false);//target
dbg->AddLine(Vector3(),P,Color(0.1f,0.1f,0.1f),false);
//show solve at origin
dbg->AddSphere(Sphere(Q,0.2f),Color(1.0f,0.0f,0.0f),false);
dbg->AddLine(Vector3(),Q,Color(1.0f,0.0f,0.0f),false);
dbg->AddLine(Q,P,Color(1.0f,0.0f,0.0f),false);*/
//show solve at position
dbg->AddSphere(Sphere(worldQ,0.2f),Color(1.0f,0.0f,0.0f),false);
dbg->AddSphere(Sphere(targetPos,0.2f),Color(0.0f,1.0f,0.0f),false);
dbg->AddLine(hipPos,worldQ,Color(1.0f,0.0f,0.0f),false);
dbg->AddLine(worldQ,targetPos,Color(1.0f,0.0f,0.0f),false);
}
return success;
}
