void CreateBox(double height, double deep, double width, Vector2D origin, vector<Face>& box)
{
box.resize(6);
box[0].setBase(Transformation3D(origin.x,origin.y,0,X_AXIS,PI/2));
box[0].addVertex(0.0,0.0);
box[0].addVertex(0.0,height);
box[0].addVertex(deep,height);
box[0].addVertex(deep,0.0);
box[1]=box[0];
box[1].setBase(Transformation3D(origin.x,origin.y+width,0,X_AXIS,PI/2));
box[2].setBase(Transformation3D(origin.x,origin.y,height));
box[2].addVertex(0.0,0.0);
box[2].addVertex(0.0,width);
box[2].addVertex(deep,width);
box[2].addVertex(deep,0.0);
box[3].setBase(Transformation3D(origin.x,origin.y,0,Y_AXIS,-PI/2));
box[3].addVertex(0.0,0.0);
box[3].addVertex(height,0.0);
box[3].addVertex(height,width);
box[3].addVertex(0.0,width);
box[4]=box[3];
box[4].setBase(Transformation3D(origin.x+deep,origin.y,0,Y_AXIS,-PI/2));
}
Gaussian3D get_gaussian_from_covariance(const IMP_Eigen::Matrix3d &covar,
const Vector3D ¢er) {
Rotation3D rot;
Vector3D radii;
// get eigen decomposition and sort by eigen vector
IMP_Eigen::EigenSolver<IMP_Eigen::Matrix3d> es(covar);
IMP_Eigen::Matrix3d evecs = es.eigenvectors().real();
IMP_Eigen::Vector3d evals = es.eigenvalues().real();
// fill in sorted stuff
for (int i = 0; i < 3; i++) {
radii[i] = evals[i];
}
// reflect if necessary
double det = evecs.determinant();
// std::cout<<"Determinant is "<<det<<std::endl;
if (det < 0) {
IMP_Eigen::Matrix3d reflect = IMP_Eigen::Vector3d(1, 1, -1).asDiagonal();
evecs = evecs * reflect;
}
// create rotation matrix and return
IMP_Eigen::Quaterniond eq(evecs);
rot = Rotation3D(eq.w(), eq.x(), eq.y(), eq.z());
return Gaussian3D(ReferenceFrame3D(Transformation3D(rot, center)), radii);
}
Face calculateFace(Vector2D ini, Vector2D end, double h)
{
double width=0, height=0;
Transformation3D trans;
if(ini.y==end.y)//paralela eje X
{
if(ini.x<end.x)
trans=Transformation3D(ini.x,ini.y,0.0,X_AXIS,PI/2);
else
trans=Transformation3D(end.x,end.y,0.0,X_AXIS,PI/2);
width=ini.distance(end);
height=h;
}
else if(ini.x==end.x)//paralela eje Y
{
if(ini.y<end.y)
trans=Transformation3D(end.x,end.y,0.0,Y_AXIS,-PI/2);
else
trans=Transformation3D(ini.x,ini.y,0.0,Y_AXIS,-PI/2);
height=-ini.distance(end);
width=h;
}
else
{
//pared diagonal
}
Face ret;
ret.setBase(trans);
ret.addVertex(0,0);
ret.addVertex(width,0);
ret.addVertex(width,height);
ret.addVertex(0,height);
//ret.setColor((float)0.9, (float)0.9, (float)0.5, (float)1);
return ret;
}
void *ApoloPort::handleConnections(void *server)
{
Socket *aux_sock= (Socket*) server;
Socket socket=*aux_sock;
PositionableEntity *pos;
RobotSim *robot;
WheeledBaseSim *wb;
static int valid=0, total=0;
while(1)
{
Socket *temp=socket.acceptConnection();
while(temp->IsConnected())
{
char buffer[10000];
int size=0;
if(0<(size=temp->Receive(buffer,10000,-1)))
{
ApoloMessage *m=0;
char *pbuffer=buffer;//recorrepbuffer
while(m=ApoloMessage::getApoloMessage(&pbuffer,size))
{
size-=m->getSize();
total++;
char *nworld=m->getWorld();
char *name=m->getObjectName();
int worldindex=0;
PositionableEntity *element=getElement(nworld,name,&worldindex);
switch(m->getType())
{
case AP_CHECKJOINTS:
case AP_SETJOINTS:
if(element){
robot=dynamic_cast<RobotSim*>(element);
int numJoints=m->getCharAt(0);
for(int i=0;i<numJoints;i++)
if(robot)robot->setJointValue(i,m->getDoubleAt(1+8*i));
if(m->getType()==AP_CHECKJOINTS){
bool res=false;
if(robot){
res=robot->checkRobotColision();
}
char resp[50];
int tam=ApoloMessage::writeBOOL(resp,res);
temp->Send(resp,tam);
}
valid++;
}
break;
case AP_GETLOCATION:
case AP_GETLOCATION_WB:
if(element){
double d[6];
Vector3D p=element->getRelativePosition();
double o[3];
element->getRelativeOrientation(d[3],d[4],d[5]);
for(int i=0;i<3;i++)d[i]=p[i];
char resp[70];
int tam;
if(m->getType()==AP_GETLOCATION)tam=ApoloMessage::writeDoubleVector(resp,6,d);
else {
d[3]=d[5];
tam=ApoloMessage::writeDoubleVector(resp,4,d);
}
temp->Send(resp,tam);
valid++;
}
break;
case AP_PLACE:
if(element){
double d[6];
for(int i=0;i<6;i++)d[i]=m->getDoubleAt(8*i);
element->setAbsoluteT3D(Transformation3D(d[0],d[1],d[2],d[3],d[4],d[5]));
valid++;
}
break;
case AP_PLACE_WB: //place a wheeled based robot. computes the grounded location and collision
if(element){
wb=dynamic_cast<WheeledBaseSim *>(element);
bool res=false;
double d[4];
for(int i=0;i<4;i++)d[i]=m->getDoubleAt(8*i);
Transformation3D taux(d[0],d[1],d[2],mr::Z_AXIS,d[3]);
if(wb)res=wb->dropWheeledBase(taux);
if(res){
res=!(wb->getWorld()->checkCollisionWith(*wb));
}
char resp[50];
int tam=ApoloMessage::writeBOOL(resp,res);
temp->Send(resp,tam);
valid++;
}
break;
case AP_MOVE_WB:
if(element){
wb=dynamic_cast<WheeledBaseSim *>(element);
double d[3];
for(int i=0;i<3;i++)d[i]=m->getDoubleAt(8*i);
wb->move(d[0],d[1]);
wb->simulate(d[2]);
wb->move(0,0);
valid++;
}
break;
case AP_UPDATEWORLD:
if(worldindex>=0){//updates one world
(*world)[worldindex]->getChild()->RefreshChild();
}else //updates all worlds
{
for(int i=0; i<world->size();i++)
(*world)[i]->getChild()->RefreshChild();
}
valid++;
break;
case AP_LINK_TO_ROBOT_TCP:
if(element){
robot=dynamic_cast<RobotSim*>(element);
char *objectname=m->getStringAt(0);
PositionableEntity *target=getElement(nworld,objectname,&worldindex);
if(target&&robot){
target->LinkTo(robot->getTcp());
}
valid++;
}
break;
}
delete m;//aseguro limpieza
}
char messageLog[150];
sprintf(messageLog,"Commands Executed: %d/%d",valid,total);
wxLogStatus(messageLog);
//temp->Send(request,50);
}
}
temp->close();
delete temp;
}
}
//creates a robot state with a set of doubles. if invalid return false
RobotState *WBState::createStateFromSample(vector<double> values)
{
//valid conditions: there is a robt and a world defined, there are 3 or 4 doubles
if(!world)return 0;
if(!robot)return 0;
if(values.size()<3)return 0;
//two options: a 3D position, and a 3D position + orientation
double val(-11*PI/12),inc(PI/6),half_inc(PI/12.5);
bool nuevoSector=false;
Transformation3D t;
int i=0;
bool flag=true;
double hmax;
WheeledBaseSim *r=robot;
/*primero selecciono la altura más alta valida... que sera a partir de quien genero*/
//Si tengo 3 argumentos... la altura queda determinada por el más alto de los validos
//si tengo más de 3 (4), la altura queda determinada por la orientación indicada (si es valida, claro)
if(values.size()==3)
{
//bucle para detectar el hmax
for(i=0;i<12;i++)
{
t=Transformation3D(values[0],values[1],values[2],Z_AXIS,i*inc+half_inc);
if(checkPose(t)){ ///revisar ¡que pasa con t!
if(flag)hmax=t.position.z;
flag=false;
if(t.position.z>hmax)hmax=t.position.z;
}
}
if(flag)return 0; //no valid poses for that coordinates.
}
else
{ //hmax is the z, after the drop, if there is no valid pose.. returns 0
t=Transformation3D(values[0],values[1],values[2],Z_AXIS,values[3]);
if(checkPose(t))hmax=t.position.z;
else return 0;
}
WBState aux(r,world);
//Create the valid sectors
hmax+=2*r->getWheelRadius();
for(i=0;i<12;i++)
{
t=Transformation3D(values[0],values[1],hmax,Z_AXIS,i*inc+half_inc);
if(checkPose(t)){ ///revisar ¡que pasa con t!
//creo un sector
Sector newSector;
newSector.min=i*inc;
newSector.max=(i+1)*inc;
newSector.pose=t;
if(t.position.z>hmax)hmax=t.position.z;
aux.sectors.push_back(newSector);
}
}
//utilizo el pos mas alto y elimino todos los sectores que esten por debajo una distancia mayor que
//la admitida por la gota que es el radio de la rueda del robot
for(i=(int)aux.sectors.size()-1;i>=0;i--)
if(hmax-aux.sectors[i].pose.position.z>(4*r->getWheelRadius()))aux.sectors.erase(aux.sectors.begin()+i);
if(aux.sectors.empty())return 0;
//At his point we obtain the medium position value(X, Y are the same but,
Vector3D newPose(0,0,0);
int numvs=aux.sectors.size();
double factorns=1.0/((double)numvs);
for(i=0;i<numvs;i++)newPose=newPose+((aux.sectors[i].pose.position)*factorns);
//fusión de sectores
i=1;
while(i<(int)aux.sectors.size())
{
if(aux.sectors[i].min-aux.sectors[i-1].max<EPS){
aux.sectors[i-1].max=aux.sectors[i].max;
aux.sectors.erase(aux.sectors.begin()+i);
}
else i++;
}
//creo el nuevo estado
aux.pose=newPose;
return new WBState(aux);
}
IMPALGEBRA_BEGIN_NAMESPACE
Ellipsoid3D::Ellipsoid3D(const Vector3D& center, double r0, double r1,
double r2, const Rotation3D& rot)
: rf_(Transformation3D(rot, center)), radii_(r0, r1, r2) {}
void CreateWorldRobocityFair(string filename)
{
unsigned int i=0;
World world;
FaceSetPart *building=new FaceSetPart;
double height=5.0, width=23.95, deep=20.55;
Face ground;
ground.setBase(Transformation3D(0,0,0));
ground.addVertex(0,0);
ground.addVertex(deep,0);
ground.addVertex(deep,width);
ground.addVertex(0,width);
ground.setColor((float)0.6, (float)0.4, (float)0.4, 1);
building->addFace(ground);
vector<Face> listWall(4);
listWall[0].setBase(Transformation3D(0,0,0,X_AXIS,PI/2));
listWall[0].addVertex(0,0);
listWall[0].addVertex(deep,0);
listWall[0].addVertex(deep,height);
listWall[0].addVertex(0,height);
listWall[1]=listWall[0];
listWall[1].setBase(Transformation3D(0,width,0,X_AXIS,PI/2));
listWall[2].setBase(Transformation3D(0,0,0,Y_AXIS,-PI/2));
listWall[2].addVertex(0,0);
listWall[2].addVertex(height,0);
listWall[2].addVertex(height,width);
listWall[2].addVertex(0,width);
listWall[3]=listWall[2];
listWall[3].setBase(Transformation3D(deep,0,0,Y_AXIS,-PI/2));
for(i=0;i<listWall.size();i++)
{
listWall[i].setColor((float)0.9, (float)0.9, (float)0.5, (float)0.5);
building->addFace(listWall[i]);
}
double columnWidth=0.50, columnDeep=0.50, columnHeight=height;
vector<Face> column1,column2,column3,column4,column5,column6,column7,column8,column9,column10,column11;
listWall[0].setDefaultColor(0.5,0.5,0.5,1.0);
CreateBox(columnHeight, columnWidth, columnDeep, Vector2D(3.80,4.00), column1);
CreateBox(columnHeight, columnWidth, columnDeep, Vector2D(7.90,4.00), column2);
CreateBox(columnHeight, columnWidth, columnDeep, Vector2D(12.00,4.00), column3);
CreateBox(columnHeight, columnWidth, columnDeep, Vector2D(16.10,4.00), column4);
CreateBox(columnHeight, columnWidth, columnDeep, Vector2D(16.10,8.10), column5);
CreateBox(columnHeight, columnWidth, columnDeep, Vector2D(16.10,14.35), column6);
CreateBox(columnHeight, columnWidth, columnDeep, Vector2D(16.10,18.45), column7);
CreateBox(columnHeight, columnWidth, columnDeep, Vector2D(12.00,18.45), column8);
CreateBox(columnHeight, columnWidth, columnDeep, Vector2D(7.90,18.45), column9);
CreateBox(columnHeight, columnWidth, columnDeep, Vector2D(3.80,18.45), column10);
CreateBox(columnHeight, columnWidth, columnDeep, Vector2D(3.80,14.35), column11);
for(i=0; i<column1.size();i++)
{
building->addFace(column1[i]);
building->addFace(column2[i]);
building->addFace(column3[i]);
building->addFace(column4[i]);
building->addFace(column5[i]);
building->addFace(column6[i]);
building->addFace(column7[i]);
building->addFace(column8[i]);
building->addFace(column9[i]);
building->addFace(column10[i]);
building->addFace(column11[i]);
}
vector<Face> mueble;
column1[0].setDefaultColor(0.6,0.6,0.2,1.0);
CreateBox(2.0,4.50,1.0,Vector2D(7.6,0.0),mueble);
for(i=0;i<mueble.size();i++)
building->addFace(mueble[i]);
//Altura imaginaria de los stands
double standHeight=0.0;
mueble[0].setDefaultColor(0.2,0.4,0.8,1.0);
vector<Face> stand1,stand2,stand3,stand4,stand5,stand6,stand7,stand8,stand9,stand10,stand12,
stand13,stand14,stand16,stand17,stand18,stand19,stand20,stand21,stand22;
CreateBox(standHeight,2,3.05,Vector2D(6.3,13.3),stand1);
CreateBox(standHeight,2,3.05,Vector2D(6.3,10.25),stand2);
CreateBox(standHeight,2,3.05,Vector2D(6.3,7.2),stand3);
CreateBox(standHeight,4.30,2,Vector2D(8.3,7.2),stand4);
CreateBox(standHeight,2,3.05,Vector2D(12.6,7.2),stand5);
CreateBox(standHeight,2,3.05,Vector2D(12.6,10.25),stand6);
CreateBox(standHeight,2,3.05,Vector2D(12.6,13.3),stand7);
CreateBox(standHeight,4.30,2,Vector2D(8.3,14.35),stand8);
CreateBox(standHeight,4.3,2.35,Vector2D(0,14.35),stand9);
CreateBox(standHeight,4.3,2.35,Vector2D(0,16.7),stand10);
CreateBox(standHeight,3.95,3.95,Vector2D(6.05,20.0),stand12);
CreateBox(standHeight,2.5,3.95,Vector2D(10.0,20.0),stand13);
CreateBox(standHeight,3.95,3.95,Vector2D(12.5,20.0),stand14);
CreateBox(standHeight,4.45,2.05,Vector2D(16.10,12.80),stand16);
CreateBox(standHeight,4.45,2.05,Vector2D(16.10,8.1),stand17);
CreateBox(standHeight,4.45,2.05,Vector2D(16.10,6.05),stand18);
CreateBox(standHeight,4.45,2.05,Vector2D(16.10,4.0),stand19);
CreateBox(standHeight,2.5,3.95,Vector2D(12.2,0),stand20);
CreateBox(standHeight,4.6,2.95,Vector2D(7.6,1.0),stand21);
CreateBox(standHeight,3.80,3.95,Vector2D(3.80,0.0),stand22);
for(i=0; i<stand1.size();i++)
{
building->addFace(stand1[i]);
building->addFace(stand2[i]);
building->addFace(stand3[i]);
building->addFace(stand4[i]);
building->addFace(stand5[i]);
building->addFace(stand6[i]);
building->addFace(stand7[i]);
building->addFace(stand8[i]);
building->addFace(stand9[i]);
building->addFace(stand10[i]);
building->addFace(stand12[i]);
building->addFace(stand13[i]);
building->addFace(stand14[i]);
building->addFace(stand16[i]);
building->addFace(stand17[i]);
building->addFace(stand18[i]);
building->addFace(stand19[i]);
building->addFace(stand20[i]);
building->addFace(stand21[i]);
building->addFace(stand22[i]);
}
stand1[1].setDefaultColor(1.0,1.0,1.0,1.0);
double wallHeight=3.0, tableHeight=1.2;
vector<Face> paredes;
paredes.push_back(calculateFace(Vector2D(7.60,0), Vector2D(7.60,3.95), wallHeight));
paredes.push_back(calculateFace(Vector2D(12.2,0), Vector2D(12.2,3.95), wallHeight));
paredes.push_back(calculateFace(Vector2D(20.55,6.05), Vector2D(16.05,6.05), wallHeight));
paredes.push_back(calculateFace(Vector2D(20.55,8.1), Vector2D(16.05,8.1), wallHeight));
paredes.push_back(calculateFace(Vector2D(20.55,12.8), Vector2D(19.55,12.8), wallHeight));
paredes.push_back(calculateFace(Vector2D(20.55,14.85), Vector2D(19.55,14.85), wallHeight));
paredes.push_back(calculateFace(Vector2D(12.5,20), Vector2D(12.5,23.95), wallHeight));
paredes.push_back(calculateFace(Vector2D(10,20), Vector2D(10,23.95), wallHeight));
paredes.push_back(calculateFace(Vector2D(0,16.7), Vector2D(4.3,16.7), wallHeight));
paredes.push_back(calculateFace(Vector2D(6.3,10.25), Vector2D(8.3,10.25), wallHeight));
paredes.push_back(calculateFace(Vector2D(6.3,13.3), Vector2D(8.3,13.3), wallHeight));
paredes.push_back(calculateFace(Vector2D(12.6,10.25), Vector2D(14.6,10.25), wallHeight));
paredes.push_back(calculateFace(Vector2D(12.6,13.3), Vector2D(14.6,13.3), wallHeight));
paredes.push_back(calculateFace(Vector2D(8.3,9.2), Vector2D(12.6,9.2), wallHeight));
paredes.push_back(calculateFace(Vector2D(8.3,9.2), Vector2D(8.3,14.35), wallHeight));
paredes.push_back(calculateFace(Vector2D(8.3,14.35), Vector2D(12.6,14.35), wallHeight));
paredes.push_back(calculateFace(Vector2D(12.65,9.2), Vector2D(12.65,14.35), wallHeight));
for(i=0;i<paredes.size();i++)
building->addFace(paredes[i]);
paredes[0].setDefaultColor((float)0.9, (float)0.9, (float)0.5, (float)0.5);
vector<Face> mesa1,mesa2,mesa3,mesa4,mesa5,mesa6,mesa7,mesa8,mesa9,mesa10,mesa11,mesa12,mesa13,mesa14;
double tableWidth=1.0, tableDeep=0.5;
CreateBox(tableHeight, tableWidth, tableDeep, Vector2D(8.4,3.95-tableDeep), mesa1);
CreateBox(tableHeight, tableWidth, tableDeep, Vector2D(12.5,3.95-tableDeep), mesa2);
CreateBox(tableHeight, tableDeep, tableWidth, Vector2D(17.1,8-tableWidth), mesa3);
CreateBox(tableHeight, tableWidth, tableDeep, Vector2D(17.1,9.6), mesa4);
CreateBox(tableHeight, tableDeep, tableWidth, Vector2D(16.1,13.3), mesa5);
CreateBox(tableHeight, tableWidth, tableDeep, Vector2D(15.45,20), mesa6);
CreateBox(tableHeight, tableWidth, tableDeep, Vector2D(10,20), mesa7);
CreateBox(tableHeight, tableDeep, tableWidth, Vector2D(6.05,22), mesa8);
CreateBox(tableHeight, tableDeep, tableWidth, Vector2D(3.8,16.7), mesa9);
CreateBox(tableHeight, tableWidth, tableDeep, Vector2D(2.8,14.35), mesa10);
CreateBox(tableHeight, tableDeep, tableWidth, Vector2D(6.3,7.2), mesa11);
CreateBox(tableHeight, tableWidth, tableDeep, Vector2D(9.98,7.2), mesa12);
CreateBox(tableHeight, tableWidth, tableDeep, Vector2D(6.8,15.85), mesa13);
CreateBox(tableHeight, tableWidth, tableDeep, Vector2D(9.98,15.85), mesa14);
for(i=0;i<mesa1.size();i++)
{
building->addFace(mesa1[i]);
building->addFace(mesa2[i]);
building->addFace(mesa3[i]);
building->addFace(mesa4[i]);
building->addFace(mesa5[i]);
building->addFace(mesa6[i]);
building->addFace(mesa7[i]);
building->addFace(mesa8[i]);
building->addFace(mesa9[i]);
building->addFace(mesa10[i]);
building->addFace(mesa11[i]);
building->addFace(mesa12[i]);
building->addFace(mesa13[i]);
building->addFace(mesa14[i]);
}
world+=building;
StreamFile myfile(filename,false);
myfile.write(&world);
}
inline namespace VECGEOM_IMPL_NAMESPACE {
const Transformation3D Transformation3D::kIdentity =
Transformation3D();
//Transformation3D::Transformation3D(const Precision tx,
// const Precision ty,
// const Precision tz) :
// fIdentity(false), fHasRotation(true), fHasTranslation(true)
//{
// SetTranslation(tx, ty, tz);
// SetRotation(1, 0, 0, 0, 1, 0, 0, 0, 1);
// SetProperties();
//}
Transformation3D::Transformation3D(
const Precision tx, const Precision ty,
const Precision tz, const Precision phi,
const Precision theta, const Precision psi) :
fIdentity(false), fHasRotation(true), fHasTranslation(true) {
SetTranslation(tx, ty, tz);
SetRotation(phi, theta, psi);
SetProperties();
}
Transformation3D::Transformation3D(
const Precision tx, const Precision ty, const Precision tz,
const Precision r0, const Precision r1, const Precision r2,
const Precision r3, const Precision r4, const Precision r5,
const Precision r6, const Precision r7, const Precision r8) :
fIdentity(false), fHasRotation(true), fHasTranslation(true){
SetTranslation(tx, ty, tz);
SetRotation(r0, r1, r2, r3, r4, r5, r6, r7, r8);
SetProperties();
}
VECGEOM_CUDA_HEADER_BOTH
void Transformation3D::Print() const {
printf("Transformation3D {{%.2f, %.2f, %.2f}, ",
fTranslation[0], fTranslation[1], fTranslation[2]);
printf("{%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f}}",
fRotation[0], fRotation[1], fRotation[2], fRotation[3], fRotation[4],
fRotation[5], fRotation[6], fRotation[7], fRotation[8]);
}
VECGEOM_CUDA_HEADER_BOTH
void Transformation3D::SetTranslation(const Precision tx,
const Precision ty,
const Precision tz) {
fTranslation[0] = tx;
fTranslation[1] = ty;
fTranslation[2] = tz;
}
VECGEOM_CUDA_HEADER_BOTH
void Transformation3D::SetTranslation(Vector3D<Precision> const &vec) {
SetTranslation(vec[0], vec[1], vec[2]);
}
VECGEOM_CUDA_HEADER_BOTH
void Transformation3D::SetProperties() {
fHasTranslation = (
fabs(fTranslation[0]) > kTolerance ||
fabs(fTranslation[1]) > kTolerance ||
fabs(fTranslation[2]) > kTolerance
) ? true : false;
fHasRotation = (GenerateRotationCode() == rotation::kIdentity)
? false : true;
fIdentity = !fHasTranslation && !fHasRotation;
}
VECGEOM_CUDA_HEADER_BOTH
void Transformation3D::SetRotation(const Precision phi,
const Precision theta,
const Precision psi) {
const Precision sinphi = sin(kDegToRad*phi);
const Precision cosphi = cos(kDegToRad*phi);
const Precision sinthe = sin(kDegToRad*theta);
const Precision costhe = cos(kDegToRad*theta);
const Precision sinpsi = sin(kDegToRad*psi);
const Precision cospsi = cos(kDegToRad*psi);
fRotation[0] = cospsi*cosphi - costhe*sinphi*sinpsi;
fRotation[1] = -sinpsi*cosphi - costhe*sinphi*cospsi;
fRotation[2] = sinthe*sinphi;
fRotation[3] = cospsi*sinphi + costhe*cosphi*sinpsi;
fRotation[4] = -sinpsi*sinphi + costhe*cosphi*cospsi;
fRotation[5] = -sinthe*cosphi;
fRotation[6] = sinpsi*sinthe;
fRotation[7] = cospsi*sinthe;
fRotation[8] = costhe;
}
VECGEOM_CUDA_HEADER_BOTH
void Transformation3D::SetRotation(Vector3D<Precision> const &vec) {
SetRotation(vec[0], vec[1], vec[2]);
}
VECGEOM_CUDA_HEADER_BOTH
void Transformation3D::SetRotation(
const Precision rot0, const Precision rot1, const Precision rot2,
const Precision rot3, const Precision rot4, const Precision rot5,
const Precision rot6, const Precision rot7, const Precision rot8) {
fRotation[0] = rot0;
fRotation[1] = rot1;
fRotation[2] = rot2;
fRotation[3] = rot3;
fRotation[4] = rot4;
fRotation[5] = rot5;
fRotation[6] = rot6;
fRotation[7] = rot7;
fRotation[8] = rot8;
}
VECGEOM_CUDA_HEADER_BOTH
RotationCode Transformation3D::GenerateRotationCode() const {
int code = 0;
for (int i = 0; i < 9; ++i) {
// Assign each bit
code |= (1<<i) * (fabs(fRotation[i]) > kTolerance);
}
if (code == rotation::kDiagonal
&& (fRotation[0] == 1. && fRotation[4] == 1. && fRotation[8] == 1.)) {
code = rotation::kIdentity;
}
return code;
}
/**
* Very simple translation code. Kept as an integer in case other cases are to
* be implemented in the future.
* /return The transformation's translation code, which is 0 for transformations
* without translation and 1 otherwise.
*/
VECGEOM_CUDA_HEADER_BOTH
TranslationCode Transformation3D::GenerateTranslationCode() const {
return (fHasTranslation) ? translation::kGeneric : translation::kIdentity;
}
#ifdef VECGEOM_ROOT
// function to convert this transformation to a TGeo transformation
// mainly used for the benchmark comparisons with ROOT
TGeoMatrix * Transformation3D::ConvertToTGeoMatrix() const
{
if( fIdentity ){
return new TGeoIdentity();
}
if( fHasTranslation && ! fHasRotation ) {
return new TGeoTranslation(fTranslation[0], fTranslation[1], fTranslation[2]);
}
if( fHasRotation && ! fHasTranslation ) {
TGeoRotation * tmp = new TGeoRotation();
tmp->SetMatrix( Rotation() );
return tmp;
}
if( fHasTranslation && fHasRotation )
{
TGeoRotation * tmp = new TGeoRotation();
tmp->SetMatrix( Rotation() );
return new TGeoCombiTrans(fTranslation[0], fTranslation[1],
fTranslation[2], tmp);
}
return 0;
}
#endif
std::ostream& operator<<(std::ostream& os,
Transformation3D const &transformation) {
os << "Transformation {" << transformation.Translation() << ", "
<< "(" << transformation.Rotation(0) << ", " << transformation.Rotation(1)
<< ", " << transformation.Rotation(2) << ", " << transformation.Rotation(3)
<< ", " << transformation.Rotation(4) << ", " << transformation.Rotation(5)
<< ", " << transformation.Rotation(6) << ", " << transformation.Rotation(7)
<< ", " << transformation.Rotation(8) << ")}"
<< "; identity(" << transformation.IsIdentity() << "); rotation("
<< transformation.HasRotation() << ")";
return os;
}
#ifdef VECGEOM_CUDA_INTERFACE
DevicePtr<cuda::Transformation3D> Transformation3D::CopyToGpu(DevicePtr<cuda::Transformation3D> const gpu_ptr) const {
gpu_ptr.Construct(fTranslation[0], fTranslation[1], fTranslation[2],
fRotation[0], fRotation[1], fRotation[2],
fRotation[3], fRotation[4], fRotation[5],
fRotation[6], fRotation[7], fRotation[8]);
CudaAssertError();
return gpu_ptr;
}
DevicePtr<cuda::Transformation3D> Transformation3D::CopyToGpu() const {
DevicePtr<cuda::Transformation3D> gpu_ptr;
gpu_ptr.Allocate();
return this->CopyToGpu(gpu_ptr);
}
#endif // VECGEOM_CUDA_INTERFACE
} // End impl namespace
