/*!
Build a vpMbtDistanceLine thanks to two points corresponding to the extremities.
\param _p1 : The first extremity.
\param _p2 : The second extremity.
*/
void
vpMbtDistanceLine::buildFrom(vpPoint &_p1, vpPoint &_p2)
{
if (line == NULL) {
line = new vpLine ;
}
poly.setNbPoint(2);
poly.addPoint(0, _p1);
poly.addPoint(1, _p2);
p1 = &poly.p[0];
p2 = &poly.p[1];
vpColVector V1(3);
vpColVector V2(3);
vpColVector V3(3);
vpColVector V4(3);
V1[0] = p1->get_oX();
V1[1] = p1->get_oY();
V1[2] = p1->get_oZ();
V2[0] = p2->get_oX();
V2[1] = p2->get_oY();
V2[2] = p2->get_oZ();
//if((V1-V2).sumSquare()!=0)
if(std::fabs((V1-V2).sumSquare()) > std::numeric_limits<double>::epsilon())
{
{
V3[0]=double(rand()%1000)/100;
V3[1]=double(rand()%1000)/100;
V3[2]=double(rand()%1000)/100;
vpColVector v_tmp1,v_tmp2;
v_tmp1 = V2-V1;
v_tmp2 = V3-V1;
V4=vpColVector::cross(v_tmp1,v_tmp2);
}
vpPoint P3(V3[0],V3[1],V3[2]);
vpPoint P4(V4[0],V4[1],V4[2]);
buildLine(*p1,*p2, P3,P4, *line) ;
}
else
{
vpPoint P3(V1[0],V1[1],V1[2]);
vpPoint P4(V2[0],V2[1],V2[2]);
buildLine(*p1,*p2,P3,P4,*line) ;
}
}
Slots(const P1& plugin1 = P1(),
const P2& plugin2 = P2(),
const P3& plugin3 = P3(),
const P4& plugin4 = P4(),
const P5& plugin5 = P5())
: BaseT( plugin1, BaseSlots(plugin2, plugin3, plugin4, plugin5, NullT()) )
{ }
QPolygon KDChart::TextLayoutItem::rotatedCorners() const
{
// the angle in rad
const qreal angle = mAttributes.rotation() * PI / 180.0;
QSize size = unrotatedSizeHint();
// my P1 - P4 (the four points of the rotated area)
QPointF P1( size.height() * sin( angle ), 0 );
QPointF P2( size.height() * sin( angle ) + size.width() * cos( angle ), size.width() * sin( angle ) );
QPointF P3( size.width() * cos( angle ), size.width() * sin( angle ) + size.height() * cos( angle ) );
QPointF P4( 0, size.height() * cos( angle ) );
QPolygon result;
result << P1.toPoint() << P2.toPoint() << P3.toPoint() << P4.toPoint();
return result;
}
void ProcessController::MakeRaft(float &z)
{
vector<InFillHit> HitsBuffer;
uint LayerNr = 0;
float step;
float size = RaftSize;
Vector2f raftMin = Vector2f(Min.x - size+printOffset.x, Min.y - size+printOffset.y);
Vector2f raftMax = Vector2f(Max.x + size+printOffset.x, Max.y + size+printOffset.y);
Vector2f Center = (Vector2f(Max.x + size, Max.y + size)-Vector2f(Min.x + size, Min.y + size))/2+Vector2f(printOffset.x, printOffset.y);
float Length = sqrtf(2)*( ((raftMax.x)>(raftMax.y)? (raftMax.x):(raftMax.y)) - ((raftMin.x)<(raftMin.y)? (raftMin.x):(raftMin.y)) )/2.0f; // bbox of object
float E = 0.0f;
float rot = RaftRotation/180.0f*M_PI;
while(LayerNr < RaftBaseLayerCount+RaftInterfaceLayerCount)
{
rot = (RaftRotation+(float)LayerNr*RaftRotationPrLayer)/180.0f*M_PI;
Vector2f InfillDirX(cosf(rot), sinf(rot));
Vector2f InfillDirY(-InfillDirX.y, InfillDirX.x);
Vector3f LastPosition;
bool reverseLines = false;
if(LayerNr < RaftBaseLayerCount)
step = RaftBaseDistance;
else
step = RaftInterfaceDistance;
Vector2f P1, P2;
for(float x = -Length ; x < Length ; x+=step)
{
P1 = (InfillDirX * Length)+(InfillDirY*x)+ Center;
P2 = (InfillDirX * -Length)+(InfillDirY*x)+ Center;
if(reverseLines)
{
Vector2f tmp = P1;
P1 = P2;
P2 = tmp;
}
// glBegin(GL_LINES);
// glVertex2fv(&P1.x);
// glVertex2fv(&P2.x);
// Crop lines to bbox*size
Vector3f point;
InFillHit hit;
HitsBuffer.clear();
Vector2f P3(raftMin.x, raftMin.y);
Vector2f P4(raftMin.x, raftMax.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit)) //Intersect edges of bbox
HitsBuffer.push_back(hit);
P3 = Vector2f(raftMax.x,raftMax.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit))
HitsBuffer.push_back(hit);
P4 = Vector2f(raftMax.x,raftMin.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit))
HitsBuffer.push_back(hit);
P3 = Vector2f(raftMin.x,raftMin.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit))
HitsBuffer.push_back(hit);
// glEnd();
if(HitsBuffer.size() == 0) // it can only be 2 or zero
continue;
if(HitsBuffer.size() != 2)
continue;
std::sort(HitsBuffer.begin(), HitsBuffer.end(), InFillHitCompareFunc);
P1 = HitsBuffer[0].p;
P2 = HitsBuffer[1].p;
float materialRatio;
if(LayerNr < RaftBaseLayerCount)
materialRatio = RaftMaterialPrDistanceRatio; // move or extrude?
else
materialRatio = RaftInterfaceMaterialPrDistanceRatio; // move or extrude?
MakeAcceleratedGCodeLine(Vector3f(P1.x,P1.y,z), Vector3f(P2.x,P2.y,z), DistanceToReachFullSpeed, materialRatio, gcode, z, MinPrintSpeedXY, MaxPrintSpeedXY, MinPrintSpeedZ, MaxPrintSpeedZ, UseIncrementalEcode, Use3DGcode, E, EnableAcceleration);
reverseLines = !reverseLines;
}
// Set startspeed for Z-move
Command g;
g.Code = SETSPEED;
g.where = Vector3f(P2.x, P2.y, z);
g.f=MinPrintSpeedZ;
g.comment = "Move Z";
g.e = E;
gcode.commands.push_back(g);
if(LayerNr < RaftBaseLayerCount)
z+=RaftBaseThickness*LayerThickness;
else
z+=RaftInterfaceThickness*LayerThickness;
// Move Z
g.Code = ZMOVE;
g.where = Vector3f(P2.x, P2.y, z);
g.f=MinPrintSpeedZ;
g.comment = "Move Z";
g.e = E;
gcode.commands.push_back(g);
LayerNr++;
}
}
void Model::MakeRaft(float &z)
{
vector<InFillHit> HitsBuffer;
uint LayerNr = 0;
float size = settings.Raft.Size;
Vector2f raftMin = Vector2f(Min.x - size + printOffset.x, Min.y - size + printOffset.y);
Vector2f raftMax = Vector2f(Max.x + size + printOffset.x, Max.y + size + printOffset.y);
Vector2f Center = (Vector2f(Max.x + size, Max.y + size)-Vector2f(Min.x + size, Min.y + size))/2+Vector2f(printOffset.x, printOffset.y);
float Length = sqrtf(2)*( ((raftMax.x)>(raftMax.y)? (raftMax.x):(raftMax.y)) - ((raftMin.x)<(raftMin.y)? (raftMin.x):(raftMin.y)) )/2.0f; // bbox of object
float E = 0.0f;
float rot;
while(LayerNr < settings.Raft.Phase[0].LayerCount + settings.Raft.Phase[1].LayerCount)
{
Settings::RaftSettings::PhasePropertiesType *props;
props = LayerNr < settings.Raft.Phase[0].LayerCount ?
&settings.Raft.Phase[0] : &settings.Raft.Phase[1];
rot = (props->Rotation+(float)LayerNr * props->RotationPrLayer)/180.0f*M_PI;
Vector2f InfillDirX(cosf(rot), sinf(rot));
Vector2f InfillDirY(-InfillDirX.y, InfillDirX.x);
Vector3f LastPosition;
bool reverseLines = false;
Vector2f P1, P2;
for(float x = -Length ; x < Length ; x+=props->Distance)
{
P1 = (InfillDirX * Length)+(InfillDirY*x) + Center;
P2 = (InfillDirX * -Length)+(InfillDirY*x) + Center;
if(reverseLines)
{
Vector2f tmp = P1;
P1 = P2;
P2 = tmp;
}
// glBegin(GL_LINES);
// glVertex2fv(&P1.x);
// glVertex2fv(&P2.x);
// Crop lines to bbox*size
Vector3f point;
InFillHit hit;
HitsBuffer.clear();
Vector2f P3(raftMin.x, raftMin.y);
Vector2f P4(raftMin.x, raftMax.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit)) //Intersect edges of bbox
HitsBuffer.push_back(hit);
P3 = Vector2f(raftMax.x,raftMax.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit))
HitsBuffer.push_back(hit);
P4 = Vector2f(raftMax.x,raftMin.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit))
HitsBuffer.push_back(hit);
P3 = Vector2f(raftMin.x,raftMin.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit))
HitsBuffer.push_back(hit);
// glEnd();
if(HitsBuffer.size() == 0) // it can only be 2 or zero
continue;
if(HitsBuffer.size() != 2)
continue;
std::sort(HitsBuffer.begin(), HitsBuffer.end(), InFillHitCompareFunc);
P1 = HitsBuffer[0].p;
P2 = HitsBuffer[1].p;
MakeAcceleratedGCodeLine (Vector3f(P1.x,P1.y,z), Vector3f(P2.x,P2.y,z),
props->MaterialDistanceRatio, gcode,
E, z, settings.Slicing, settings.Hardware);
reverseLines = !reverseLines;
}
// Set startspeed for Z-move
Command g;
g.Code = SETSPEED;
g.where = Vector3f(P2.x, P2.y, z);
g.f=settings.Hardware.MinPrintSpeedZ;
g.comment = "Move Z";
g.e = E;
gcode.commands.push_back(g);
z += props->Thickness * settings.Hardware.LayerThickness;
// Move Z
g.Code = ZMOVE;
g.where = Vector3f(P2.x, P2.y, z);
g.f = settings.Hardware.MinPrintSpeedZ;
g.comment = "Move Z";
g.e = E;
gcode.commands.push_back(g);
LayerNr++;
}
// restore the E state
Command gotoE;
gotoE.Code = GOTO;
gotoE.e = 0;
gotoE.comment = "Reset E for the remaining print";
gcode.commands.push_back(gotoE);
}
/***********************************************************
when update npc position
***********************************************************/
void ExternalActor::NpcChangedUpdate(double updatetime,
float CurrPosX, float CurrPosY, float CurrPosZ,
float CurrRotation, const std::string &CurrAnimation,
bool ResetPosition, bool ResetRotation,
const LbaNet::PlayingSoundSequence &Sounds,
LbaNet::NpcUpdateBasePtr Update,
ScriptEnvironmentBase* scripthandler)
{
// reset free move
_freemove = false;
// reset projectiles
if(_character)
_character->ClearActionsOnAnimation();
// update only newest info
if(updatetime < _last_update)
return;
_last_update = updatetime;
if(_playingscript)
return;
boost::shared_ptr<PhysicalObjectHandlerBase> physo = _character->GetPhysicalObject();
float posX, posY, posZ;
physo->GetPosition(posX, posY, posZ);
float rotation = physo->GetRotationYAxis();
// update position and rotation
float diffpos = (CurrPosX-posX)*(CurrPosX-posX) +
(CurrPosY-posY)*(CurrPosY-posY) +
(CurrPosZ-posZ)*(CurrPosZ-posZ);
float diffrot = abs(CurrRotation-rotation);
// reset actor position
if(ResetPosition || diffpos > 64 || _shouldreset)
{
physo->SetPosition(CurrPosX, CurrPosY, CurrPosZ);
posX = CurrPosX;
posY = CurrPosY;
posZ = CurrPosZ;
}
// reset actor rotation
if(ResetRotation || diffrot > 20 || _shouldreset)
{
LbaQuaternion Q(CurrRotation, LbaVec3(0,1,0));
physo->SetRotation(Q);
rotation = CurrRotation;
}
_differencePosX = CurrPosX-posX;
_differencePosY = CurrPosY-posY;
_differencePosZ = CurrPosZ-posZ;
_differenceRotation = CurrRotation-rotation;
//update animation
if(CurrAnimation != "")
_character->GetDisplayObject()->Update(new LbaNet::AnimationStringUpdate(CurrAnimation), _playingscript);
_shouldreset = false;
// update sounds
boost::shared_ptr<SoundObjectHandlerBase> soundo = _character->GetSoundObject();
if(soundo)
{
soundo->SetSoundVector(Sounds, false);
_character->UpdateSoundPosition();
}
// update the script part
if(!Update)
{
_currentScripts = boost::shared_ptr<ScriptPartBase>();
return;
}
LbaNet::NpcUpdateBase & obj = *Update;
const std::type_info& info = typeid(obj);
// StraightWalkToNpcUpd
if(info == typeid(LbaNet::StraightWalkToNpcUpd))
{
LbaNet::StraightWalkToNpcUpd * castedptr =
dynamic_cast<LbaNet::StraightWalkToNpcUpd *>(&obj);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
StraightWalkToScriptPart(0, false, castedptr->PosX, castedptr->PosY, castedptr->PosZ,
_character));
return;
}
if(info == typeid(LbaNet::WalkToPointNpcUpd))
{
LbaNet::WalkToPointNpcUpd * castedptr =
dynamic_cast<LbaNet::WalkToPointNpcUpd *>(&obj);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
WalkToPointScriptPart(0, false, castedptr->PosX, castedptr->PosY, castedptr->PosZ, castedptr->RotationSpeedPerSec, castedptr->moveForward));
return;
}
// GoToNpcUpd
if(info == typeid(LbaNet::GoToNpcUpd))
{
LbaNet::GoToNpcUpd * castedptr =
dynamic_cast<LbaNet::GoToNpcUpd *>(&obj);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
GoToScriptPart(0, false, castedptr->PosX, castedptr->PosY, castedptr->PosZ, castedptr->Speed,
_character));
return;
}
// RotateNpcUpd
if(info == typeid(LbaNet::RotateNpcUpd))
{
LbaNet::RotateNpcUpd * castedptr =
dynamic_cast<LbaNet::RotateNpcUpd *>(&obj);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
RotateScriptPart(0, false, castedptr->Angle, castedptr->RotationSpeedPerSec,
castedptr->ManageAnimation));
return;
}
// AnimateNpcUpd
if(info == typeid(LbaNet::AnimateNpcUpd))
{
LbaNet::AnimateNpcUpd * castedptr =
dynamic_cast<LbaNet::AnimateNpcUpd *>(&obj);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
PlayAnimationScriptPart(0, false, castedptr->AnimationMove, castedptr->NbAnimation));
return;
}
// RotateFromPointNpcUpd
if(info == typeid(LbaNet::RotateFromPointNpcUpd))
{
LbaNet::RotateFromPointNpcUpd * castedptr =
dynamic_cast<LbaNet::RotateFromPointNpcUpd *>(&obj);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
RotateFromPointScriptPart(0, false, castedptr->Angle,
castedptr->PosX, castedptr->PosY, castedptr->PosZ, castedptr->Speed, _character));
return;
}
// FollowWaypointNpcUpd
if(info == typeid(LbaNet::FollowWaypointNpcUpd))
{
LbaNet::FollowWaypointNpcUpd * castedptr =
dynamic_cast<LbaNet::FollowWaypointNpcUpd *>(&obj);
LbaVec3 Pm1X(castedptr->Pm1X, castedptr->Pm1Y, castedptr->Pm1Z);
LbaVec3 P0(castedptr->P0X, castedptr->P0Y, castedptr->P0Z);
LbaVec3 P1(castedptr->P1X, castedptr->P1Y, castedptr->P1Z);
LbaVec3 P2(castedptr->P2X, castedptr->P2Y, castedptr->P2Z);
LbaVec3 P3(castedptr->P3X, castedptr->P3Y, castedptr->P3Z);
LbaVec3 P4(castedptr->P4X, castedptr->P4Y, castedptr->P4Z);
_currentScripts = boost::shared_ptr<ScriptPartBase>(new
FollowWaypointScriptPart(0, false, Pm1X, P0, P1, P2, P3, P4));
return;
}
}
void PPP_MD5_block
(
uint_32_ptr context,
/* [IN/OUT] - the incremental hash */
register uint_32_ptr block,
/* [IN] - the 16 word block */
register const uint_32 _PTR_ ctab
/* [IN] - the sine function */
)
{ /* Body */
#define F(x,y,z) ((y&x)|(z&~x))
#define G(x,y,z) ((x&z)|(y&~z))
#define H(x,y,z) (x^y^z)
#define I(x,y,z) (y^(x|~z))
#define P1(f,k,s) t=a+f(b,c,d)+block[k]+*ctab++; a=rotl(t,s)+b
#define P2(f,k,s) t=d+f(a,b,c)+block[k]+*ctab++; d=rotl(t,s)+a
#define P3(f,k,s) t=c+f(d,a,b)+block[k]+*ctab++; c=rotl(t,s)+d
#define P4(f,k,s) t=b+f(c,d,a)+block[k]+*ctab++; b=rotl(t,s)+c
register uint_32 a = context[0];
register uint_32 b = context[1];
register uint_32 c = context[2];
register uint_32 d = context[3];
register uint_32 t;
P1(F, 0, 7); P2(F, 1, 12); P3(F, 2, 17); P4(F, 3, 22);
P1(F, 4, 7); P2(F, 5, 12); P3(F, 6, 17); P4(F, 7, 22);
P1(F, 8, 7); P2(F, 9, 12); P3(F, 10, 17); P4(F, 11, 22);
P1(F, 12, 7); P2(F, 13, 12); P3(F, 14, 17); P4(F, 15, 22);
P1(G, 1, 5); P2(G, 6, 9); P3(G, 11, 14); P4(G, 0, 20);
P1(G, 5, 5); P2(G, 10, 9); P3(G, 15, 14); P4(G, 4, 20);
P1(G, 9, 5); P2(G, 14, 9); P3(G, 3, 14); P4(G, 8, 20);
P1(G, 13, 5); P2(G, 2, 9); P3(G, 7, 14); P4(G, 12, 20);
P1(H, 5, 4); P2(H, 8, 11); P3(H, 11, 16); P4(H, 14, 23);
P1(H, 1, 4); P2(H, 4, 11); P3(H, 7, 16); P4(H, 10, 23);
P1(H, 13, 4); P2(H, 0, 11); P3(H, 3, 16); P4(H, 6, 23);
P1(H, 9, 4); P2(H, 12, 11); P3(H, 15, 16); P4(H, 2, 23);
P1(I, 0, 6); P2(I, 7, 10); P3(I, 14, 15); P4(I, 5, 21);
P1(I, 12, 6); P2(I, 3, 10); P3(I, 10, 15); P4(I, 1, 21);
P1(I, 8, 6); P2(I, 15, 10); P3(I, 6, 15); P4(I, 13, 21);
P1(I, 4, 6); P2(I, 11, 10); P3(I, 2, 15); P4(I, 9, 21);
context[0] += a;
context[1] += b;
context[2] += c;
context[3] += d;
PPP_memzero(block, sizeof(uint_32[16]));
} /* Endbody */
int main() {
Point center(41,29);
// The numbering of the nodes corresponds to gslib's output order
Node P5(38,28,0.5740);
Node P8(45,29,1.2110);
Node P4(41,26,2.1270);
Node P3(39,31,8.3400);
Node P6(38,31,18.6420);
Node P2(39,30,7.9380);
Node P7(39,32,2.2840);
Node P1(40,31,2.5090);
Node P9(37,20,0.5740);
Node P10(25,28,1.2110);
Node P11(39,26,2.1270);
Node P12(32,31,8.3400);
Node P13(30,34,18.6420);
Node P14(33,35,7.9380);
Node P15(42,32,2.2840);
Node P16(31,23,2.5090);
neighborhood voisin;
voisin.add_node(P1);
voisin.add_node(P2);
voisin.add_node(P3);
voisin.add_node(P4);
voisin.add_node(P5);
voisin.add_node(P6);
voisin.add_node(P7);
voisin.add_node(P8);
voisin.add_node(P9);
voisin.add_node(P10);
voisin.add_node(P11);
voisin.add_node(P12);
voisin.add_node(P13);
voisin.add_node(P14);
voisin.add_node(P15);
voisin.add_node(P16);
typedef matrix_lib_traits<TNT_lib<double> >::Vector TNTvector;
covariance covar;
//______________________________
// Simple Kriging
//______________________________
std::cout << std::endl <<std::endl;
std::cout << "____________________________________________"
<< std::endl
<< "Simple kriging"
<< std::endl << std::endl;
TNTvector SK_weights;
SK_constraints SK;
OK_constraints OK;
double sk_variance;
TNT::stopwatch chrono;
chrono.start();
for(int count = 0 ; count < 50000 ; count ++) {
int change=1;
if(drand48() <0.5) change = -1;
(voisin[0].location())[0] += change;
kriging_weights<GSTL_TNT_lib>(SK_weights,
center, voisin,
covar, OK );
}
chrono.stop();
std::cout << "time elapsed using Gauss: " << chrono.read() << std::endl;
chrono.reset();
chrono.start();
for(int count = 0 ; count < 50000 ; count ++) {
int change=1;
if(drand48() <0.5) change = -1;
voisin[0].property_value() = 0.5*count;
kriging_weights(SK_weights,
center, voisin,
covar, OK );
}
chrono.stop();
std::cout << "time elapsed using LU: " << chrono.read() << std::endl;
/*
//______________________________
// Ordinary Kriging
//______________________________
std::cout << std::endl <<std::endl;
std::cout << "____________________________________________"
<< std::endl
<< "Ordinary kriging"
<< std::endl << std::endl;
TNTvector OK_weights;
OK_constraints OK;
double ok_variance;
status = kriging_weights(OK_weights, ok_variance,
center, voisin,
covar, OK);
std::cout << "Here are the weights:" << std::endl
<< OK_weights << std::endl;
//______________________________
// Kriging with Trend
//______________________________
std::cout << std::endl <<std::endl;
std::cout << "____________________________________________"
<< std::endl
<< "Kriging with Trend"
<< std::endl << std::endl;
TNTvector KT_weights;
KT_constraints<functIter> KT(functArray.begin(),functArray.end());
double kt_variance;
status = kriging_weights(KT_weights, kt_variance,
center, voisin,
covar, KT);
std::cout << "Here are the weights:" << std::endl
<< KT_weights << std::endl;
*/
return 0;
}
int recoverPose( InputArray E, InputArray _points1, InputArray _points2, InputArray _cameraMatrix,
OutputArray _R, OutputArray _t, InputOutputArray _mask)
{
Mat points1, points2, cameraMatrix;
_points1.getMat().convertTo(points1, CV_64F);
_points2.getMat().convertTo(points2, CV_64F);
_cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F);
int npoints = points1.checkVector(2);
CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
points1.type() == points2.type());
CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1);
if (points1.channels() > 1)
{
points1 = points1.reshape(1, npoints);
points2 = points2.reshape(1, npoints);
}
double fx = cameraMatrix.at<double>(0,0);
double fy = cameraMatrix.at<double>(1,1);
double cx = cameraMatrix.at<double>(0,2);
double cy = cameraMatrix.at<double>(1,2);
points1.col(0) = (points1.col(0) - cx) / fx;
points2.col(0) = (points2.col(0) - cx) / fx;
points1.col(1) = (points1.col(1) - cy) / fy;
points2.col(1) = (points2.col(1) - cy) / fy;
points1 = points1.t();
points2 = points2.t();
Mat R1, R2, t;
decomposeEssentialMat(E, R1, R2, t);
Mat P0 = Mat::eye(3, 4, R1.type());
Mat P1(3, 4, R1.type()), P2(3, 4, R1.type()), P3(3, 4, R1.type()), P4(3, 4, R1.type());
P1(Range::all(), Range(0, 3)) = R1 * 1.0; P1.col(3) = t * 1.0;
P2(Range::all(), Range(0, 3)) = R2 * 1.0; P2.col(3) = t * 1.0;
P3(Range::all(), Range(0, 3)) = R1 * 1.0; P3.col(3) = -t * 1.0;
P4(Range::all(), Range(0, 3)) = R2 * 1.0; P4.col(3) = -t * 1.0;
// Do the cheirality check.
// Notice here a threshold dist is used to filter
// out far away points (i.e. infinite points) since
// there depth may vary between postive and negtive.
double dist = 50.0;
Mat Q;
triangulatePoints(P0, P1, points1, points2, Q);
Mat mask1 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask1 = (Q.row(2) < dist) & mask1;
Q = P1 * Q;
mask1 = (Q.row(2) > 0) & mask1;
mask1 = (Q.row(2) < dist) & mask1;
triangulatePoints(P0, P2, points1, points2, Q);
Mat mask2 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask2 = (Q.row(2) < dist) & mask2;
Q = P2 * Q;
mask2 = (Q.row(2) > 0) & mask2;
mask2 = (Q.row(2) < dist) & mask2;
triangulatePoints(P0, P3, points1, points2, Q);
Mat mask3 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask3 = (Q.row(2) < dist) & mask3;
Q = P3 * Q;
mask3 = (Q.row(2) > 0) & mask3;
mask3 = (Q.row(2) < dist) & mask3;
triangulatePoints(P0, P4, points1, points2, Q);
Mat mask4 = Q.row(2).mul(Q.row(3)) > 0;
Q.row(0) /= Q.row(3);
Q.row(1) /= Q.row(3);
Q.row(2) /= Q.row(3);
Q.row(3) /= Q.row(3);
mask4 = (Q.row(2) < dist) & mask4;
Q = P4 * Q;
mask4 = (Q.row(2) > 0) & mask4;
mask4 = (Q.row(2) < dist) & mask4;
mask1 = mask1.t();
mask2 = mask2.t();
mask3 = mask3.t();
mask4 = mask4.t();
// If _mask is given, then use it to filter outliers.
if (!_mask.empty())
{
Mat mask = _mask.getMat();
CV_Assert(mask.size() == mask1.size());
bitwise_and(mask, mask1, mask1);
bitwise_and(mask, mask2, mask2);
bitwise_and(mask, mask3, mask3);
bitwise_and(mask, mask4, mask4);
}
if (_mask.empty() && _mask.needed())
{
_mask.create(mask1.size(), CV_8U);
}
CV_Assert(_R.needed() && _t.needed());
_R.create(3, 3, R1.type());
_t.create(3, 1, t.type());
int good1 = countNonZero(mask1);
int good2 = countNonZero(mask2);
int good3 = countNonZero(mask3);
int good4 = countNonZero(mask4);
if (good1 >= good2 && good1 >= good3 && good1 >= good4)
{
R1.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask1.copyTo(_mask);
return good1;
}
else if (good2 >= good1 && good2 >= good3 && good2 >= good4)
{
R2.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask2.copyTo(_mask);
return good2;
}
else if (good3 >= good1 && good3 >= good2 && good3 >= good4)
{
t = -t;
R1.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask3.copyTo(_mask);
return good3;
}
else
{
t = -t;
R2.copyTo(_R);
t.copyTo(_t);
if (_mask.needed()) mask4.copyTo(_mask);
return good4;
}
}
// old raft
void Model::MakeRaft(GCodeState &state, double &z)
{
vector<Intersection> HitsBuffer;
double raftSize = settings.Raft.Size;
Vector3d raftMin = settings.Hardware.PrintMargin + Min;
Vector3d raftMax = settings.Hardware.PrintMargin + Max + 2 * raftSize;
Vector2d Center = Vector2d((raftMin.x + raftMax.x) / 2,
(raftMin.y + raftMax.y) / 2);
// bbox of object
double Length = (std::max(raftMax.x,raftMax.y) -
std::min(raftMin.x, raftMin.y))/sqrt(2.0);
double rot;
uint LayerNr = 0;
uint layerCount = settings.Raft.Phase[0].LayerCount +
settings.Raft.Phase[1].LayerCount;
Settings::RaftSettings::PhasePropertiesType *props = &settings.Raft.Phase[0];
double thickness = props->Thickness * settings.Hardware.LayerThickness;
double extrusionfactor = settings.Hardware.GetExtrudeFactor(thickness)
* props->MaterialDistanceRatio;
while(LayerNr < layerCount)
{
// If we finished phase 0, start phase 1 of the raft...
if (LayerNr >= settings.Raft.Phase[0].LayerCount)
props = &settings.Raft.Phase[1];
rot = (props->Rotation+(double)LayerNr * props->RotationPrLayer)/180.0*M_PI;
Vector2d InfillDirX(cosf(rot), sinf(rot));
Vector2d InfillDirY(-InfillDirX.y, InfillDirX.x);
Vector3d LastPosition;
bool reverseLines = false;
Vector2d P1, P2;
double maxerr = 0.1*props->Distance;
for(double x = -Length ; x < Length ; x+=props->Distance)
{
P1 = (InfillDirX * Length)+(InfillDirY*x) + Center;
P2 = (InfillDirX * -Length)+(InfillDirY*x) + Center;
if(reverseLines)
{
Vector2d tmp = P1;
P1 = P2;
P2 = tmp;
}
// glBegin(GL_LINES);
// glVertex2fv(&P1.x);
// glVertex2fv(&P2.x);
// Crop lines to bbox*size
Vector3d point;
Intersection hit;
HitsBuffer.clear();
Vector2d P3(raftMin.x, raftMin.y);
Vector2d P4(raftMin.x, raftMax.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit,maxerr)) //Intersect edges of bbox
HitsBuffer.push_back(hit);
P3 = Vector2d(raftMax.x,raftMax.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit,maxerr))
HitsBuffer.push_back(hit);
P4 = Vector2d(raftMax.x,raftMin.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit,maxerr))
HitsBuffer.push_back(hit);
P3 = Vector2d(raftMin.x,raftMin.y);
// glVertex2fv(&P3.x);
// glVertex2fv(&P4.x);
if(IntersectXY(P1,P2,P3,P4,hit,maxerr))
HitsBuffer.push_back(hit);
// glEnd();
if(HitsBuffer.size() == 0) // it can only be 2 or zero
continue;
if(HitsBuffer.size() != 2)
continue;
std::sort(HitsBuffer.begin(), HitsBuffer.end());
P1 = HitsBuffer[0].p;
P2 = HitsBuffer[1].p;
state.MakeGCodeLine (Vector3d(P1.x,P1.y,z),
Vector3d(P2.x,P2.y,z),
Vector3d(0,0,0), 0,
settings.Hardware.MaxPrintSpeedXY * 60,
extrusionfactor, 0,
z,
settings.Slicing, settings.Hardware);
reverseLines = !reverseLines;
}
// Set startspeed for Z-move
Command g;
g.Code = SETSPEED;
g.where = Vector3d(P2.x, P2.y, z);
g.f=settings.Hardware.MinPrintSpeedZ * 60;
g.comment = "Move Z";
g.e = 0;
gcode.commands.push_back(g);
z += thickness;
// Move Z
g.Code = ZMOVE;
g.where = Vector3d(P2.x, P2.y, z);
g.f = settings.Hardware.MinPrintSpeedZ * 60;
g.comment = "Move Z";
g.e = 0;
gcode.commands.push_back(g);
LayerNr++;
}
// restore the E state
// Command gotoE;
// gotoE.Code = GOTO;
// gotoE.e = 0;
// gotoE.comment = _("Reset E for the remaining print");
// gcode.commands.push_back(gotoE);
}
void srTConstTrjDat::DetermineIntegLimitsForArbTrj(srTWfrSmp& DistrInfoDat, double& sStart, double& sEnd)
{
const int AmOfSafetyInterv = 10; // To steer
const double RelTolNotRotate = 1.E-05; // To steer
TVector3d uLab(MagConst.Bx, 0., MagConst.Bz);
double Bcon = sqrt(uLab.x*uLab.x + uLab.z*uLab.z);
double Rmag = 3.33564076253*(EbmDat.Energy)/Bcon;
double ds = 0;
if(DistrInfoDat.AssumeAllPhotonEnergies)
{
ds = Rmag*sqrt(EbmDat.GammaEm2);
}
else
{
double LambMax_m;
if(DistrInfoDat.TreatLambdaAsEnergyIn_eV)
{
double eMin = DistrInfoDat.LambStart;
LambMax_m = 1.24E-06/eMin;
}
else
{
LambMax_m = 1.E-09*((DistrInfoDat.nLamb > 1)? DistrInfoDat.LambEnd : DistrInfoDat.LambStart);
}
double ds1 = pow(LambMax_m*Rmag*Rmag, 1./3.);
double ds2 = LambMax_m/EbmDat.GammaEm2;
//ds = (ds1 > ds2)? ds1 : ds2;
ds = (ds1 < ds2)? ds1 : ds2;
}
uLab = (1./Bcon)*uLab;
TVector3d uLoc(0.,0.,1.), Zero(0.,0.,0.);
TVector3d uLab_mi_uLoc = uLab - uLoc;
//double NormEst = Abs(uLab_mi_uLoc);
double NormEst = uLab_mi_uLoc.Abs();
gmTrans Rot;
if(NormEst < RelTolNotRotate)
{
Rot.SetupIdent();
}
else
{
TVector3d AxVect = uLab^uLoc;
double uLab_uLoc = uLab*uLoc;
double Angle = acos(uLab_uLoc);
Rot.SetupRotation(Zero, AxVect, Angle);
TVector3d TestVect = Rot.TrBiPoint(uLab);
double TestScal = TestVect*uLab;
if(TestScal < 0.)
{
Rot.SetupRotation(Zero, AxVect, -Angle);
}
}
TVector3d P1(DistrInfoDat.xStart, 0., DistrInfoDat.zStart);
P1 = Rot.TrPoint(P1);
double xLocMin = P1.x, xLocMax = P1.x;
if(DistrInfoDat.nx > 1)
{
TVector3d P2(DistrInfoDat.xEnd, 0., DistrInfoDat.zStart);
P2 = Rot.TrPoint(P2);
if(xLocMin > P2.x) xLocMin = P2.x;
if(xLocMax < P2.x) xLocMax = P2.x;
}
if(DistrInfoDat.nz > 1)
{
TVector3d P3(DistrInfoDat.xStart, 0., DistrInfoDat.zEnd);
P3 = Rot.TrPoint(P3);
if(xLocMin > P3.x) xLocMin = P3.x;
if(xLocMax < P3.x) xLocMax = P3.x;
if(DistrInfoDat.nx > 1)
{
TVector3d P4(DistrInfoDat.xEnd, 0., DistrInfoDat.zEnd);
P4 = Rot.TrPoint(P4);
if(xLocMin > P4.x) xLocMin = P4.x;
if(xLocMax < P4.x) xLocMax = P4.x;
}
}
TVector3d InitCoord(EbmDat.x0, 0., EbmDat.z0);
InitCoord = Rot.TrPoint(InitCoord);
TVector3d InitAng(EbmDat.dxds0, 0., EbmDat.dzds0);
InitAng = Rot.TrPoint(InitAng);
double y1Inv = 1./(DistrInfoDat.yStart - EbmDat.s0);
double xAngMin = (xLocMin - InitCoord.x)*y1Inv - InitAng.x;
double xAngMax = (xLocMax - InitCoord.x)*y1Inv - InitAng.x;
sStart = EbmDat.s0 + xAngMin*Rmag - AmOfSafetyInterv*ds;
sEnd = EbmDat.s0 + xAngMax*Rmag + AmOfSafetyInterv*ds;
}
int main() {
Node center_node(41,29,-99);
// The numbering of the nodes corresponds to gslib's output order
Node P5(38,28,0.5740);
Node P8(45,29,1.2110);
Node P4(41,26,2.1270);
Node P3(39,31,8.3400);
Node P6(38,31,18.6420);
Node P2(39,30,7.9380);
Node P7(39,32,2.2840);
Node P1(40,31,2.5090);
neighborhood voisin;
voisin.add_node(P1);
voisin.add_node(P2);
voisin.add_node(P3);
voisin.add_node(P4);
voisin.add_node(P5);
voisin.add_node(P6);
voisin.add_node(P7);
voisin.add_node(P8);
typedef TNT_lib<double> TNT;
typedef matrix_lib_traits<TNT>::Vector TNTvector;
typedef matrix_lib_traits<TNT>::Symmetric_matrix TNTMatrix;
covariance covar;
//_____________________________
// gaussian cdf estimator
//_____________________________
std::cout << std::endl <<std::endl;
std::cout << "____________________________________________"
<< std::endl
<< "estimating gaussian cdf with SK"
<< std::endl << std::endl;
Gaussian_cdf g_ccdf;
typedef Kriging_combiner<std::vector<double>::const_iterator,
neighborhood> KCombiner;
KCombiner sk_combiner( new SK_combiner<std::vector<double>::const_iterator,
neighborhood>( 9.0 ) );
typedef Kriging_constraints<neighborhood, Point, TNT> KConstraints;
KConstraints constraints( new SKConstraints_impl<neighborhood, Point,TNT> );
Gaussian_cdf_Kestimator<covariance,
neighborhood,KConstraints,TNT> gK_estimator( covar, constraints, sk_combiner );
gK_estimator(center_node, voisin, g_ccdf);
std::cout << "gaussian cdf : mean= " << g_ccdf.mean() << " variance= "
<< g_ccdf.variance() << std::endl;
/*
Gaussian_cdf g_ccdf2;
Gaussian_cdf_Kestimator<covariance,
neighborhood, SK_constraints> gK_estimator2( covar, SK_constraints(), sk_combiner2 );
gK_estimator2(center, voisin, g_ccdf2);
std::cout << "gaussian cdf : mean= " << g_ccdf2.mean() << " variance= "
<< g_ccdf2.variance() << std::endl;
*/
}
