int cylinder(t_vector r, t_figure cy, double *t, t_vector eye)
{
double coeff[3];
eye = rotations(eye, cy.angle.x, cy.angle.y, cy.angle.z);
r = rotations(r, cy.angle.x, cy.angle.y, cy.angle.z);
cy.center = rotations(cy.center, cy.angle.x, cy.angle.y, cy.angle.z);
coeff[0] = r.x * r.x + r.z * r.z;
coeff[1] = 2 * (r.x * (eye.x - cy.center.x) + r.z * (eye.z - cy.center.z));
coeff[2] = (eye.x - cy.center.x) * (eye.x - cy.center.x) +
(eye.z - cy.center.z) * (eye.z - cy.center.z) - cy.radius * cy.radius;
return (distance(coeff, t));
}
void GLC_CuttingPlane::moveManipulatorRep(const GLC_Point3d& pos)
{
// Create the widget rotation matrix
const GLC_Matrix4x4 rotationMatrix(m_CompMatrix.rotationMatrix());
const GLC_Matrix4x4 translationMatrix(pos);
const GLC_Matrix4x4 offsetMatrix(m_Normal * m_ManipulatorOffsetFactor);
GLC_Matrix4x4 scaleMatrix;
scaleMatrix.setMatScaling(m_ScaleFactor, m_ScaleFactor, m_ScaleFactor);
GLC_3DWidget::instanceHandle(1)->setMatrix(offsetMatrix * translationMatrix * rotationMatrix *scaleMatrix);
// Rotation manipulator
QVector<GLC_Matrix4x4> rotations(3);
rotations[0].setMatRot(glc::Y_AXIS, glc::PI / 2.0); // X
rotations[0]= GLC_Matrix4x4(glc::X_AXIS, -glc::PI / 2.0) * rotations[0];
rotations[1].setMatRot(glc::X_AXIS, -glc::PI / 2.0); // Y
// Z
for (int i= 0; i < 3; ++i)
{
GLC_3DWidget::instanceHandle(2 + i)->setMatrix(offsetMatrix * translationMatrix * rotationMatrix * rotations.at(i) * scaleMatrix);
}
GLC_Arrow* pArrow= dynamic_cast<GLC_Arrow*>(GLC_3DWidget::instanceHandle(1)->geomAt(0));
Q_ASSERT(NULL != pArrow);
pArrow->setViewDir(rotationMatrix * GLC_3DWidget::widgetManagerHandle()->cameraHandle()->forward().normalize());
}
Quaternionfs getRotations( const GIDSet& gids ) const final
{
const float deg2rad = float( M_PI ) / 180.f;
const brion::NeuronMatrix& data =
_circuit.get( gids, brion::NEURON_ROTATION );
Quaternionfs rotations( gids.size( ));
#pragma omp parallel for
for( size_t i = 0; i < gids.size(); ++i )
{
try
{
// transform rotation Y angle in degree into rotation quaternion
const float angle = lexical_cast<float>( data[i][0] ) * deg2rad;
rotations[i] = Quaternionf( angle, Vector3f( 0, 1, 0 ));
}
catch( const boost::bad_lexical_cast& )
{
GIDSet::const_iterator gid = gids.begin();
std::advance( gid, i );
LBWARN << "Error parsing circuit orientation for gid "
<< *gid << std::endl;
}
}
return rotations;
}
int cone(t_vector r, t_figure co, double *t, t_vector eye)
{
double coeff[3];
if (co.name == CONE)
co.radius = 0;
eye = rotations(eye, co.angle.x, co.angle.y, co.angle.z);
r = rotations(r, co.angle.x, co.angle.y, co.angle.z);
co.center = rotations(co.center, co.angle.x, co.angle.y, co.angle.z);
coeff[0] = r.x * r.x - r.y * r.y + r.z * r.z;
coeff[1] = 2 * (r.x * (eye.x - co.center.x) - r.y * (eye.y - co.center.y) +
r.z * (eye.z - co.center.z));
coeff[2] = ((eye.x - co.center.x) * (eye.x - co.center.x) -
(eye.y - co.center.y) * (eye.y - co.center.y) + (eye.z - co.center.z) *
(eye.z - co.center.z)) - co.radius;
return (distance(coeff, t));
}
void EntityModel::setEntityRotation(int index, float value)
{
auto axis_angle = getUniverse()->getRotation(m_entity).getAxisAngle();
((float*)&axis_angle)[index] = value;
axis_angle.axis.normalize();
Lumix::StackAllocator<256> allocator;
Lumix::Array<Lumix::Entity> entities(allocator);
Lumix::Array<Lumix::Quat> rotations(allocator);
entities.push(m_entity);
rotations.push(Lumix::Quat(axis_angle.axis, axis_angle.angle));
m_editor.setEntitiesRotations(entities, rotations);
}
void createSomeModel(int numTiles) {
std::vector<MoTile> tiles(numTiles,
MoTile(QImage(20, 30, QImage::Format_RGBA8888)));
model.constructInitialState(targetImage, tiles);
std::vector<float> x(numTiles);
model.setXCoords(&x[0], &x[0] + numTiles);
std::vector<float> y(numTiles);
model.setXCoords(&y[0], &y[0] + numTiles);
std::vector<float> rotations(numTiles, 0.0f);
model.setRotations(&rotations[0], &rotations[0] + numTiles);
std::vector<float> scales(numTiles, 0.0f);
model.setRotations(&scales[0], &scales[0] + numTiles);
}
t_vector normal_torus(t_vector i, t_figure to)
{
t_vector n;
double c;
i = rotations(i, to.angle.x, to.angle.y, to.angle.z);
c = ((i.x - to.center.x) * (i.x - to.center.x) + (i.y - to.center.y) * (i.y
- to.center.y) + (i.z - to.center.z) * (i.z - to.center.z)
- to.separation * to.separation - to.radius * to.radius);
n.x = 4 * c * (i.x - to.center.x);
n.y = 4 * (i.y - to.center.y) * (c + 2 * to.separation * to.separation);
n.z = 4 * c * (i.z - to.center.z);
return (n);
}
/**
Render the model list to the screen
*/
void ShadowableScene::renderModelList(const vector<Model3D*> &modelList)
{
Model3D *aModel;
for (int index = 0; index < modelList.size(); index++)
{
aModel = modelList[index];
glPushMatrix();
// Position the model
Vector3D position(aModel->getPosition());
glTranslatef(position.x, position.y, position.z);
// Orient the model
FTM rotations(aModel->getFTM());
float tempMatrix[] =
{
rotations._00,rotations._01,rotations._02,rotations._03,
rotations._10,rotations._11,rotations._12,rotations._13,
rotations._20,rotations._21,rotations._22,rotations._23,
rotations._30,rotations._31,rotations._32,rotations._33,
};
glMultMatrixf(tempMatrix);
if (!aModel->isLit())
{
// Set the color (for non lit scenes)
glColor3f(aModel->getRed(),aModel->getGreen(),aModel->getBlue());
glDisable(GL_LIGHTING);
}
else
{
// Set the material props (for lit scenes)
Material tempMaterial = aModel->getMaterial();
float matSpecular[] = {tempMaterial.specularRed, tempMaterial.specularGreen, tempMaterial.specularBlue, tempMaterial.specularAlpha};
glMaterialfv(GL_FRONT, GL_SPECULAR, matSpecular);
float matShininess[] = {tempMaterial.shininess};
glMaterialfv(GL_FRONT, GL_SHININESS, matShininess);
float matAmbDiff[] = { tempMaterial.ambientDiffuseRed, tempMaterial.ambientDiffuseGreen, tempMaterial.ambientDiffuseBlue, tempMaterial.ambientDiffuseAlpha };
glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, matAmbDiff);
glEnable(GL_LIGHTING);
}
// draw the model
glCallList(aModel->getCallListId());
glPopMatrix();
}
}
void goStraight() {
double maxRotate = 5;
std::vector<int> iterations;
Simlink simulator;
rl::FidoControlSystem learner = rl::FidoControlSystem(1, {-1}, {1}, 11);
//rl::WireFitQLearn learner = rl::WireFitQLearn(1, 1, 1, 3, 4, {-1}, {1}, 11, new rl::LSInterpolator(), new net::Backpropagation(0.01, 0.9, 0.1, 35000), 0.95, 0.4);
for(int a = 0; a < 50; a++) {
learner.reset();
simulator.placeRobotInRandomPosition();
int iter = 0;
std::vector<double> rotations(0);
double average = 0;
while((rotations.size() < 5 || average > 0.2) && iter < 40) {
rl::Action action;
action = learner.chooseBoltzmanAction({ 1 }, 0.4);
//action = learner.chooseBoltzmanAction({ 1 }, exploration);
simulator.robot.rotate(action[0]*maxRotate);
//simulator.robot.go(10, 10, 5, 5);
learner.applyReinforcementToLastAction(1-fabs(action[0]*2), {1});
iter++;
rotations.push_back(action[0]);
if(rotations.size() > 5) {
rotations.erase(rotations.begin());
}
average = 0;
for(unsigned int b = 0; b < rotations.size(); b++) {
average += fabs(rotations[b]);
}
average /= (double)rotations.size();
}
iterations.push_back(iter);
printStats(iterations);
}
}
std::vector<geometry_msgs::Quaternion> classification::PoseDataPoint::getRotations() const
{
std::vector<geometry_msgs::Quaternion> rotations(15);
rotations[0] = extractRotation(poseData.head);
rotations[1] = extractRotation(poseData.neck);
rotations[2] = extractRotation(poseData.torso);
rotations[3] = extractRotation(poseData.left_shoulder);
rotations[4] = extractRotation(poseData.left_elbow);
rotations[5] = extractRotation(poseData.left_hand);
rotations[6] = extractRotation(poseData.right_shoulder);
rotations[7] = extractRotation(poseData.right_elbow);
rotations[8] = extractRotation(poseData.right_hand);
rotations[9] = extractRotation(poseData.left_hip);
rotations[10] = extractRotation(poseData.left_knee);
rotations[11] = extractRotation(poseData.left_foot);
rotations[12] = extractRotation(poseData.right_hip);
rotations[13] = extractRotation(poseData.right_knee);
rotations[14] = extractRotation(poseData.right_foot);
return rotations;
}
int main() {
int r[MAX_ROTATIONS];
size_t count = 0;
int ispr[NR_VALS];
for (size_t i = 0; i < NR_VALS; ++i)
ispr[i] = isprime(i);
for (size_t i = 2; i < NR_VALS; ++i) {
size_t nrot = rotations(i, r);
int allprime = 1;
for (size_t j = 0; j < nrot; ++j)
if (!isprime(r[j]))
allprime = 0;
if (allprime)
count++;
}
printf("%lu\n", count);
return 0;
}
IMPEM_BEGIN_NAMESPACE
algebra::Transformation3Ds PCAAligner::align(const algebra::Vector3Ds& points)
const {
algebra::Transformation3Ds transforms;
IMP::algebra::PrincipalComponentAnalysis points_pc_ =
IMP::algebra::get_principal_components(points);
IMP::algebra::Vector3D x = map_pc_.get_principal_component(0);
IMP::algebra::Vector3D y = map_pc_.get_principal_component(1);
IMP::algebra::Rotation3D r = get_rotation_matrix(x, y);
IMP::algebra::Transformation3D map_trans(
IMP::algebra::Transformation3D(r, -(r * map_pc_.get_centroid())));
IMP::algebra::Transformation3D inverse_map_trans = map_trans.get_inverse();
// align the principal components by enumeration 6 xy choices
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (i != j) {
IMP::algebra::Vector3D xi = points_pc_.get_principal_component(i);
IMP::algebra::Vector3D yi = points_pc_.get_principal_component(j);
IMP::algebra::Rotation3Ds rotations(4);
rotations[0] = get_rotation_matrix(xi, yi);
rotations[1] = get_rotation_matrix(xi, -yi);
rotations[2] = get_rotation_matrix(-xi, yi);
rotations[3] = get_rotation_matrix(-xi, -yi);
for (unsigned int k = 0; k < rotations.size(); k++) {
IMP::algebra::Transformation3D points_trans =
IMP::algebra::Transformation3D(
rotations[k], -(rotations[k] * points_pc_.get_centroid()));
IMP::algebra::Transformation3D ps2dens =
inverse_map_trans * points_trans;
transforms.push_back(ps2dens);
}
}
}
}
return transforms;
}
void Object3D::draw(void)
{
beforeTransformations();
glMatrixMode(GL_MODELVIEW_MATRIX);
glPushMatrix();
glTranslatef(translation().x(), translation().y(), translation().z());
glTranslatef(center().x(), center().y(), center().z());
QQuaternion t = interactiveQuartenion();
glRotatef(degreeFromCos(t.scalar()),
0, 0, t.z());
//glMultMatrixf(rotations().constData());
glMultMatrixd(rotations().constData());
m_modelView = glGetMatrix(GL_MODELVIEW_MATRIX);
m_projection = glGetMatrix(GL_PROJECTION_MATRIX);
afterTransformations();
drawGeometry();
glPopMatrix();
}
void Camera::move(char c, const std::vector<Entity> &entities,
const Map &map, int col, int row) {
Eigen::Quaternionf q1;
Eigen::Vector3f axis1(0.0f, 1.0f, 0.0f);
q1 = Eigen::AngleAxisf(rotations(0), axis1);
Eigen::Matrix4f R1 = Eigen::Matrix4f::Identity();
R1.block<3,3>(0,0) = q1.toRotationMatrix();
Eigen::Vector3f last_valid_location = translations;
Eigen::Vector3f point;
switch (c) {
case 's':
point = Eigen::Vector3f(0.0f, 0.0f, 1.0f);
break;
case 'd':
point = Eigen::Vector3f(-1.0f, 0.0f, 0.0f);
break;
case 'w':
point = Eigen::Vector3f(0.0f, 0.0f, -1.0f);
break;
case 'a':
point = Eigen::Vector3f(1.0f, 0.0f, 0.0f);
break;
default:
assert(0);
}
point = R1.block<3,3>(0,0) * point;
translations(0) += (point(0) * tfactor);
translations(2) -= (point(2) * tfactor);
if (col >= 0 && row >= 0) {
Neighbors neighbors = map.getNearbyWalls(col, row);
bool reset_x = false, reset_z = false;
if ((neighbors.up && this->collides(*neighbors.up)) ||
(neighbors.down && this->collides(*neighbors.down))) {
// std::cout << "COLLIDES Z" << std::endl;
reset_z = true;
}
if ((neighbors.left && this->collides(*neighbors.left)) ||
(neighbors.right && this->collides(*neighbors.right))) {
// std::cout << "COLLIDES X" << std::endl;
reset_x = true;
}
for (auto it = entities.begin(); it != entities.end(); it++) {
if (this->collides(*it)) {
reset_x = reset_z = true;
break;
}
}
if (reset_x || reset_z) {
translations = last_valid_location;
if (reset_x) {
point(0) = 0;
}
if (reset_z) {
point(2) = 0;
}
translations(0) += (point(0) * tfactor);
translations(2) -= (point(2) * tfactor);
}
}
}
/** Compute the pose of the table plane
* @param inputs
* @param outputs
* @return
*/
int
process(const tendrils& inputs, const tendrils& outputs)
{
std::vector<tabletop_object_detector::TabletopObjectRecognizer<pcl::PointXYZ>::TabletopResult > results;
// Process each table
pose_results_->clear();
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters_merged;
// Map to store the transformation for each cluster (table_index)
std::map<pcl::PointCloud<pcl::PointXYZ>::Ptr, size_t> cluster_table;
std::vector<cv::Vec3f> translations(clusters_->size());
std::vector<cv::Matx33f> rotations(clusters_->size());
for (size_t table_index = 0; table_index < clusters_->size(); ++table_index)
{
getPlaneTransform((*table_coefficients_)[table_index], rotations[table_index], translations[table_index]);
// Make the clusters be in the table frame
size_t n_clusters = (*clusters_)[table_index].size();
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clusters(n_clusters);
cv::Matx33f Rinv = rotations[table_index].t();
cv::Vec3f Tinv = -Rinv*translations[table_index];
for (size_t cluster_index = 0; cluster_index < n_clusters; ++cluster_index)
{
clusters[cluster_index] = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
for(size_t i = 0; i < (*clusters_)[table_index][cluster_index].size(); ++i)
{
cv::Vec3f res = Rinv*(*clusters_)[table_index][cluster_index][i] + Tinv;
clusters[cluster_index]->push_back(pcl::PointXYZ(res[0], res[1], res[2]));
}
cluster_table.insert(std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, size_t>(clusters[cluster_index], table_index));
}
clusters_merged.insert(clusters_merged.end(), clusters.begin(), clusters.end());
}
object_recognizer_.objectDetection(clusters_merged, confidence_cutoff_, perform_fit_merge_, results);
for (size_t i = 0; i < results.size(); ++i)
{
const tabletop_object_detector::TabletopObjectRecognizer<pcl::PointXYZ>::TabletopResult & result = results[i];
const size_t table_index = cluster_table[result.cloud_];
PoseResult pose_result;
// Add the object id
std::stringstream ss;
ss << result.object_id_;
pose_result.set_object_id(db_, ss.str());
// Add the pose
const geometry_msgs::Pose &pose = result.pose_;
cv::Vec3f T(pose.position.x, pose.position.y, pose.position.z);
Eigen::Quaternionf quat(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
cv::Vec3f new_T = rotations[table_index] * T + translations[table_index];
pose_result.set_T(cv::Mat(new_T));
pose_result.set_R(quat);
cv::Mat R = cv::Mat(rotations[table_index] * pose_result.R<cv::Matx33f>());
pose_result.set_R(R);
pose_result.set_confidence(result.confidence_);
// Add the cluster of points
std::vector<sensor_msgs::PointCloud2ConstPtr> ros_clouds (1);
sensor_msgs::PointCloud2Ptr cluster_cloud (new sensor_msgs::PointCloud2());
#if PCL_VERSION_COMPARE(>=,1,7,0)
::pcl::PCLPointCloud2 pcd_tmp;
::pcl::toPCLPointCloud2(*result.cloud_, pcd_tmp);
pcl_conversions::fromPCL(pcd_tmp, *cluster_cloud);
#else
pcl::toROSMsg(*result.cloud_, *cluster_cloud);
#endif
ros_clouds[0] = cluster_cloud;
pose_result.set_clouds(ros_clouds);
pose_results_->push_back(pose_result);
}
return ecto::OK;
}
void VineCopulaStructureSelect(double *bounds, int type, double *Structure, double *Families, double *Rotations, std::vector<double>& Thetas, double *U, int d, unsigned int n, int StructuringRule, double *familyset, int m)
{
std::vector<int> families((d-1)*d/2);
std::vector<int> rotations((d-1)*d/2);
std::vector<double> thetas;
int i,j,k,I;
switch (type)
{
case 0: // C-Vine
{
std::vector<int> structure(d);
for (i=0;i<d;i++)
{
structure[i] = i;
}
switch (StructuringRule)
{
case 0: // Maximizing the sum of absolute Kendall's tau in every tree
{
I = UpdateStructure(structure, 0, U, d, d, n);
std::vector<double> V(n*d);
for (i=0;i<(int)n;i++)
{
V[i] = U[I*n+i];
}
for (j=0;j<I;j++)
{
for (i=0;i<(int)n;i++)
{
V[(j+1)*n+i] = U[j*n+i];
}
}
for (j=I+1;j<d;j++)
{
for (i=0;i<(int)n;i++)
{
V[j*n+i] = U[j*n+i];
}
}
TreeSelect(bounds,type, &families[0], thetas, &rotations[0], &V[0], d, n, familyset, m);
GetPseudoObs(&families[0], thetas, &rotations[0], &V[0], d, n);
for (k=1;k<d-2;k++)
{
I = UpdateStructure(structure, 0, &V[n], d, d-k, n);
for (i=0;i<(int)n;i++)
{
V[i] = V[(I+1)*n+i];
}
for (j=I+1;j<d-k;j++)
{
for (i=0;i<(int)n;i++)
{
V[j*n+i] = V[(j+1)*n+i];
}
}
TreeSelect(bounds,type, &families[d*k-k*(k+1)/2], thetas, &rotations[d*k-k*(k+1)/2], &V[0], d-k, n, familyset, m);
GetPseudoObs(&families[d*k-k*(k+1)/2], thetas, &rotations[d*k-k*(k+1)/2], &V[0], d-k, n);
}
TreeSelect(bounds,type, &families[d*(d-1)/2-1], thetas, &rotations[d*(d-1)/2-1], &V[n], 2, n, familyset, m);
break;
}
// case 2: Choose the most dependent variable as the root node and list the other variables by their dependence to the root node in increasing order (Nikoloulopoulos et al. 2012, p.3665)
// case 3: Choose the most dependent variable as the root node and list the other variables sequentially by choosing the variable which is least dependent with the previously selcted one (Nikoloulopoulos et al. 2012, p.3665)
// case 1: Choose the most dependent variable as the root node and list the other variables by their dependence to the root node in decreasing order (Nikoloulopoulos et al. 2012, p.3665)
default:
{
I = UpdateStructure(structure, StructuringRule, U, d, d, n);
std::vector<double> V(n*d);
for (j=0;j<d;j++)
{
for (i=0;i<(int)n;i++)
{
V[j*n+i] = U[structure[j]*n+i];
}
}
TreeSelect(bounds,type, &families[0], thetas, &rotations[0], &V[0], d, n, familyset, m);
GetPseudoObs(&families[0], thetas, &rotations[0], &V[0], d, n);
for (k=1;k<d-2;k++)
{
TreeSelect(bounds,type, &families[d*k-k*(k+1)/2], thetas, &rotations[d*k-k*(k+1)/2], &V[k*n], d-k, n, familyset, m);
GetPseudoObs(&families[d*k-k*(k+1)/2], thetas, &rotations[d*k-k*(k+1)/2], &V[k*n], d-k, n);
}
TreeSelect(bounds,type, &families[d*(d-1)/2-1], thetas, &rotations[d*(d-1)/2-1], &V[(d-2)*n], 2, n, familyset, m);
}
}
Thetas.resize(thetas.size());
switch (StructuringRule)
{
case 0:
{
std::vector<int> NumbParams((d-1)*d/2+1);
int J=0;
for (i=0;i<(d-1)*d/2;i++)
{
switch((int) families[i]){
case 0:
{
NumbParams[i+1] = NumbParams[i];
break;
}
case 18:
{
NumbParams[i+1] = NumbParams[i] +3;
break;
}
case 3: case 4: case 5: case 6: case 12: case 16: case 17: case 19:
{
NumbParams[i+1] = NumbParams[i] +2;
break;
}
default:
{
NumbParams[i+1] = NumbParams[i] +1;
}
}
}
std::vector<std::pair<int, int> > R(d-1);
std::vector<int> Ranks(d-1);
for (k=0;k<d-1;k++)
{
// Computing ranks
for (i=0;i<d-k-1;i++)
{
R[i] = std::make_pair(structure[i+1+k],i);
}
std::sort(&R[0],&R[d-k-1]);
for (i=0;i<d-k-1;i++)
{
Ranks[R[i].second] = i;
}
for (i=0;i<d-k-1;i++)
{
Families[d*k-k*(k+1)/2+i] = (double) families[d*k-k*(k+1)/2+Ranks[i]];
Rotations[d*k-k*(k+1)/2+i] = (double) rotations[d*k-k*(k+1)/2+Ranks[i]];
switch(NumbParams[d*k-k*(k+1)/2+Ranks[i]+1]-NumbParams[d*k-k*(k+1)/2+Ranks[i]]){
case 0:
{
break;
}
case 1:
{
Thetas[J] = thetas[NumbParams[d*k-k*(k+1)/2+Ranks[i]]];
J++;
break;
}
case 2:
{
Thetas[J] = thetas[NumbParams[d*k-k*(k+1)/2+Ranks[i]]];
Thetas[J+1] = thetas[NumbParams[d*k-k*(k+1)/2+Ranks[i]]+1];
J = J+2;
break;
}
default:
{
Thetas[J] = thetas[NumbParams[d*k-k*(k+1)/2+Ranks[i]]];
Thetas[J+1] = thetas[NumbParams[d*k-k*(k+1)/2+Ranks[i]]+1];
Thetas[J+2] = thetas[NumbParams[d*k-k*(k+1)/2+Ranks[i]]+2];
J = J+3;
}
}
}
}
break;
}
default:
{
for (i=0;i<(d-1)*d/2;i++)
{
Families[i] = (double) families[i];
Rotations[i] = (double) rotations[i];
}
for (i=0;i<(int) thetas.size();i++)
{
Thetas[i] = thetas[i];
}
}
}
for (i=0;i<d;i++)
{
Structure[i] = (double) structure[i];
}
break;
}
case 1: // D-Vine
{
std::vector<double> V((d-2)*n);
std::vector<double> H((d-2)*n);
std::vector<double> U1(n),V1(n);
TreeSelect(bounds,type, &families[0], thetas, &rotations[0], &U[0], d, n, familyset, m);
int J =0;
for (i=0;i<d-1;i++)
{
if (rotations[i]>0)
{
Rotate_Obs(&U[i*n],&U[(i+1)*n],&U1[0],&V1[0],rotations[i],n);
if (i<d-2) {
PairCopulaHfun_Rotated_Obs(families[i], rotations[i], &thetas[J], &U1[0], &V1[0], &H[i*n], n);
}
if (i>0) {
PairCopulaVfun_Rotated_Obs(families[i], rotations[i], &thetas[J], &U1[0], &V1[0], &V[(i-1)*n], n);
}
}
else
{
if (i<d-2) {
PairCopulaHfun(families[i], &thetas[J], &U[i*n], &U[(i+1)*n], &H[i*n], n);
}
if (i>0) {
PairCopulaVfun(families[i], &thetas[J], &U[i*n], &U[(i+1)*n], &V[(i-1)*n], n);
}
}
switch(families[i]){
case 0:
{
break;
}
case 18:
{
J += 3;
break;
}
case 3: case 4: case 5: case 6: case 12: case 16: case 17: case 19:
{
J += 2;
break;
}
default:
{
J += 1;
}
}
}
for (k=1;k<d-2;k++)
{
TreeSelect(bounds,type, &families[d*k-k*(k+1)/2], thetas, &rotations[d*k-k*(k+1)/2], &H[0], &V[0], d-k, n, familyset, m);
for (i=0;i<d-k-1;i++)
{
if (rotations[d*k-k*(k+1)/2+i]>0)
{
Rotate_Obs(&H[i*n],&V[i*n],&U1[0],&V1[0],rotations[d*k-k*(k+1)/2+i],n);
if (i>0) {
PairCopulaVfun_Rotated_Obs(families[d*k-k*(k+1)/2+i], rotations[d*k-k*(k+1)/2+i], &thetas[J], &U1[0], &V1[0], &V[(i-1)*n], n);
}
if (i<d-k-2) {
PairCopulaHfun_Rotated_Obs(families[d*k-k*(k+1)/2+i], rotations[d*k-k*(k+1)/2+i], &thetas[J], &U1[0], &V1[0], &H[i*n], n);
}
}
else
{
if (i>0) {
PairCopulaVfun(families[d*k-k*(k+1)/2+i], &thetas[J], &H[i*n], &V[i*n], &V[(i-1)*n], n);
}
if (i<d-k-2) {
PairCopulaHfun(families[d*k-k*(k+1)/2+i], &thetas[J], &H[i*n], &V[i*n], &H[i*n], n);
}
}
switch(families[d*k-k*(k+1)/2+i]){
case 0:
{
break;
}
case 18:
{
J += 3;
break;
}
case 3: case 4: case 5: case 6: case 12: case 16: case 17: case 19:
{
J += 2;
break;
}
default:
{
J += 1;
}
}
}
}
TreeSelect(bounds,type, &families[d*(d-1)/2-1], thetas, &rotations[d*(d-1)/2-1], &H[0], &V[0], 2, n, familyset, m);
Thetas.resize(thetas.size());
for (i=0;i<(d-1)*d/2;i++)
{
Families[i] = (double) families[i];
Rotations[i] = (double) rotations[i];
}
for (i=0;i<(int) thetas.size();i++)
{
Thetas[i] = thetas[i];
}
}
}
return;
}
void Camera::move(char c, const Level &level_one,
const Map &map, int col, int row, float mov) {
Eigen::Quaternionf q1;
Eigen::Vector3f axis1(0.0f, 1.0f, 0.0f);
q1 = Eigen::AngleAxisf(rotations(0), axis1);
Eigen::Matrix4f R1 = Eigen::Matrix4f::Identity();
R1.block<3,3>(0,0) = q1.toRotationMatrix();
Eigen::Vector3f last_valid_location = translations;
Eigen::Vector3f point;
switch (c) {
case 's':
point = Eigen::Vector3f(0.0f, 0.0f, mov);
break;
case 'd':
point = Eigen::Vector3f(-mov, 0.0f, 0.0f);
break;
case 'w':
point = Eigen::Vector3f(0.0f, 0.0f, -mov);
break;
case 'a':
point = Eigen::Vector3f(mov, 0.0f, 0.0f);
break;
default:
assert(0);
}
point = R1.block<3,3>(0,0) * point;
translations(0) += (point(0) * tfactor);
translations(2) -= (point(2) * tfactor);
if (col >= 0 && row >= 0) {
Neighbors neighbors = map.getNearbyWalls(col, row);
bool reset_x = false, reset_z = false;
if ((neighbors.up && this->collides(*neighbors.up)) ||
(neighbors.down && this->collides(*neighbors.down))) {
// std::cout << "COLLIDES Z" << std::endl;
reset_z = true;
}
if ((neighbors.left && this->collides(*neighbors.left)) ||
(neighbors.right && this->collides(*neighbors.right))) {
// std::cout << "COLLIDES X" << std::endl;
reset_x = true;
}
for (int i = 0; i < level_one.getNumRooms(); ++i) {
std::vector<Entity> b_entities;
b_entities = (level_one.getRooms())[i]->boundaries;
for (auto it = b_entities.begin(); it != b_entities.end(); it++) {
if (this->collides(*it)) {
reset_x = reset_z = true;
i = level_one.getNumRooms();
break;
}
}
if (i != level_one.getNumRooms()) {
std::vector<Entity> t_entities;
t_entities = (level_one.getRooms())[i]->entities;
for (auto it = t_entities.begin(); it != t_entities.end(); it++) {
if (this->collides(*it)) {
reset_x = reset_z = true;
i = level_one.getNumRooms();
break;
}
}
}
}
if (reset_x || reset_z) {
translations = last_valid_location;
if (reset_x) {
point(0) = 0;
}
if (reset_z) {
point(2) = 0;
}
translations(0) += (point(0) * tfactor);
translations(2) -= (point(2) * tfactor);
}
else {
for (int i = 0; i < level_one.getNumRooms(); ++i) {
(level_one.getRooms())[i]->triggerRoom(vec2(col, row));
}
}
}
}
void Camera::applyViewMatrix(MatrixStack *MV) const
{
MV->rotate(rotations(1), Eigen::Vector3f(1.0f, 0.0f, 0.0f));
MV->rotate(rotations(0), Eigen::Vector3f(0.0f, 1.0f, 0.0f));
MV->translate(translations);
}
