static void
gps_hdt_message_handler(carmen_gps_gphdt_message *message)
{
detect_gps_performance_change(message);
xsens_handler.gps_hdt = *message;
if (!xsens_handler.initial_state_initialized)
return;
// if (check_time_difference(message->timestamp))
// {
// initialized_gps = 1;
// }
sensor_vector_xsens_xyz *sensor_vector = create_sensor_vector_gps_hdt(message);
if (!kalman_filter)
{
if ((message->valid) && (xsens_handler.gps_xyz.gps_quality == 4))
correction(measure_weight_orientation_hdt, sensor_vector, fused_odometry_parameters);
}
destroy_sensor_vector_xsens_xyz(sensor_vector);
}
static void
carmen_gps_xyz_message_handler(carmen_gps_xyz_message *message)
{
detect_gps_performance_change(message);
if (!message_from_best_gps(message))
return;
if (!xsens_handler.initial_state_initialized)
return;
if (check_time_difference(message->timestamp))
{
reinitialized_gps = 1;
}
sensor_vector_xsens_xyz *sensor_vector = create_sensor_vector_gps_xyz(message);
if (!kalman_filter)
{
if (message->gps_quality)
correction(measure_weight_position, sensor_vector, fused_odometry_parameters);
}
destroy_sensor_vector_xsens_xyz(sensor_vector);
}
int expose_hook(t_env *env)
{
double x;
t_ray ray;
t_dda dda;
t_draw draw;
env->old_time = clock();
x = 0;
while (x <= env->vec.width)
{
init_ray(*env, &ray, x);
init_dda(*env, ray, &dda);
compute_dda(*env, &dda, ray);
correction(&dda, ray);
get_height(&draw, *env, dda);
draw_walls(*env, draw, dda, x);
draw_decor(*env, draw, x);
x++;
}
mlx_put_image_to_window(env->mlx, env->win, env->img.img, 0, 0);
env->time = clock();
compute_fps(*env);
move(env);
return (0);
}
log_double_t correction(const Parameters& P,const vector<int>& nodes)
{
log_double_t C = 1.0;
for(int i=0; i<P.n_data_partitions(); i++)
C *= correction(P[i],nodes);
return C;
}
bool VisualOdometry::resetposSrvCallback(
Save::Request& request,
Save::Response& response)
{
const std::string& new_tr = request.filename;
// f2b_
double m00,m01,m02,m03,m10,m11,m12,m13,m20,m21,m22,m23,m30,m31,m32,m33;
int res;
res = sscanf(new_tr.c_str(), "%lg %lg %lg %lg\n%lg %lg %lg %lg\n%lg %lg %lg %lg\n%lg %lg %lg %lg\n)",
&m00,&m01,&m02,&m03,&m10,&m11,&m12,&m13,&m20,&m21,&m22,&m23,&m30,&m31,&m32,&m33);
if (res != 16)
{
res = sscanf(new_tr.c_str(), "%lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg %lg)",
&m00,&m01,&m02,&m03,&m10,&m11,&m12,&m13,&m20,&m21,&m22,&m23,&m30,&m31,&m32,&m33);
if (res != 16)
{
return false;
}
}
// Eigen::Affine3d pose;
// pose(0,0) = m00;
// pose(0,1) = m01;
// pose(0,2) = m02;
// pose(0,3) = m03;
// pose(1,0) = m10;
// pose(1,1) = m11;
// pose(1,2) = m12;
// pose(1,3) = m13;
// pose(2,0) = m20;
// pose(2,1) = m21;
// pose(2,2) = m22;
// pose(2,3) = m23;
// pose(3,0) = m30;
// pose(3,1) = m31;
// pose(3,2) = m32;
// pose(3,3) = m33;
double yaw,pitch,roll;
tf::Matrix3x3 rot(m00,m01,m02,m10,m11,m12,m20,m21,m22);
tf::Matrix3x3 correction(0,0,1,-1,0,0,0,-1,0); //convert from camera pose to world
rot = rot*correction.inverse();
rot.getEulerYPR(yaw,pitch,roll);
Eigen::Affine3d pose;
pose.setIdentity();
pose = Eigen::Translation<double,3>(m03,m13,m23)*
Eigen::AngleAxis<double>(yaw,Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxis<double>(pitch,Eigen::Vector3d::UnitY()) *
Eigen::AngleAxis<double>(roll,Eigen::Vector3d::UnitX());
// pose = pose * Eigen::AngleAxis<double>( M_PI,Eigen::Vector3d::UnitZ());
tf::TransformEigenToTF(pose,f2b_);
motion_estimation_.resetModel();
// tf::Vector3 ori(m03,m13,m23);
// f2b_.setBasis(rot);
// f2b_.setOrigin(ori);
return true;
}
TEST_FIXTURE(EnchantDictionaryStoreReplacement_TestFixture,
EnchantDictStoreReplacment_OnBrokerPwl)
{
std::string misspelling("helo");
std::string correction("hello");
enchant_dict_store_replacement(_pwl,
misspelling.c_str(),
-1,
correction.c_str(),
-1);
}
static void
xsens_mti_message_handler(carmen_xsens_global_quat_message *xsens_mti)
{
if (!xsens_handler.initial_state_initialized)
{
initialize_states(xsens_mti);
return;
}
// Must check if timestamp has changed due to log file jump on log playback
if (reinitialized_gps)//if (check_time_difference(xsens_mti->timestamp))
{
carmen_fused_odometry_initialize(fused_odometry_parameters);
initialize_states(xsens_mti);
reinitialized_gps = 0;
return;
}
xsens_sensor_vector_index = (xsens_sensor_vector_index + 1) % XSENS_SENSOR_VECTOR_SIZE;
if (xsens_sensor_vector[xsens_sensor_vector_index] != NULL)
destroy_sensor_vector_xsens_xyz(xsens_sensor_vector[xsens_sensor_vector_index]);
sensor_vector_xsens_xyz *sensor_vector = create_sensor_vector_xsens_mti(xsens_mti, &xsens_handler.gps_xyz);
xsens_sensor_vector[xsens_sensor_vector_index] = sensor_vector;
set_best_yaw_estimate(xsens_sensor_vector, xsens_sensor_vector_index);
xsens_handler.orientation = sensor_vector->orientation; // Used by create_car_odometry_control()
xsens_handler.ang_velocity = sensor_vector->ang_velocity; // Used by create_car_odometry_control()
if (!kalman_filter)
{
if (xsens_handler.gps_performance_changed)
{
xsens_handler.gps_performance_degradation = 50.0;
xsens_handler.gps_performance_changed = 0;
}
if (xsens_handler.gps_orientation_performance_changed)
{
xsens_handler.gps_orientation_performance_degradation = 40.0;
xsens_handler.gps_orientation_performance_changed = 0;
}
prediction(sensor_vector->timestamp, fused_odometry_parameters);
correction(measure_weight_orientation, sensor_vector, fused_odometry_parameters);
publish_fused_odometry();
xsens_handler.gps_performance_degradation *= 0.98; // fator de decaimento
xsens_handler.gps_orientation_performance_degradation *= 0.98; // fator de decaimento
}
xsens_handler.last_xsens_message_timestamp = xsens_mti->timestamp;
}
/**
* Calcule l'angle que le robot doit prendre pour suivre la route.
*/
void computePID(int in_light, int *factorLeft, int *factorRight)
{ /*
if(in_light < MIN_LUMINOSITY_TRESHOLD)
*out_angle = (MIN_LUMINOSITY + MAX_LUMINOSITY) / 2 - in_light;
else if(in_light > MAX_LUMINOSITY_TRESHOLD)
*out_angle = -(in_light - (MIN_LUMINOSITY + MAX_LUMINOSITY) / 2);
else
*out_angle = 0;
*/
if ((in_light < MIN_LUMINOSITY_TRESHOLD) || (in_light > MAX_LUMINOSITY_TRESHOLD)) {
// *factorLeft = (100 * in_light) / ((MAX_LUMINOSITY + MIN_LUMINOSITY) / 2);
// *factorRight = (100 * ((MAX_LUMINOSITY + MIN_LUMINOSITY) / 2)) / in_light;
*factorLeft=correction(in_light,MAX_LUMINOSITY,MIN_LUMINOSITY,1);
*factorRight=correction(in_light,MAX_LUMINOSITY,MIN_LUMINOSITY,-1);
}
else {
*factorLeft = 100;
*factorRight = 100;
}
}
/////////////////////////////////////////////////////////////////////////////
// Test Normal Operation
TEST_FIXTURE(EnchantDictionaryStoreReplacement_TestFixture,
EnchantDictStoreReplacment_ExplicitWordLength)
{
std::string misspelling("helo");
std::string correction("hello");
enchant_dict_store_replacement(_dict,
misspelling.c_str(),
misspelling.size(),
correction.c_str(),
correction.size());
CHECK(storeReplacementCalled);
CHECK_EQUAL(EnchantDictionaryStoreReplacement_TestFixture::misspelling, misspelling);
CHECK_EQUAL(EnchantDictionaryStoreReplacement_TestFixture::correction, correction);
}
TEST_FIXTURE(EnchantDictionaryStoreReplacement_TestFixture,
EnchantDictStoreReplacment_ExplicitWordLengthDoesNotCoincideWithNulTerminator)
{
std::string misspelling("helo1");
std::string correction("hello1");
enchant_dict_store_replacement(_dict,
misspelling.c_str(),
misspelling.size()-1,
correction.c_str(),
correction.size()-1);
misspelling.resize(misspelling.size()-1);
correction.resize(correction.size()-1);
CHECK(storeReplacementCalled);
CHECK_EQUAL(EnchantDictionaryStoreReplacement_TestFixture::misspelling, misspelling);
CHECK_EQUAL(EnchantDictionaryStoreReplacement_TestFixture::correction, correction);
}
bool valve_request(gui::updateValvePos::Request &req,
gui::updateValvePos::Response &res ) {
//Create Point Cloud Objects Needed for this function (Cleanup later?)
pcl::PointCloud<pcl::PointXYZ> input_cloud;
pcl::PointCloud<pcl::PointXYZ> valve_cloud;
pcl::PointCloud<pcl::PointXYZ> valve_cloud_input;
pcl::PointCloud<pcl::PointXYZ> valve_cloud_aligned;
pcl::PointCloud<pcl::PointXYZ>::Ptr icp_target (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr icp_input (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ> Final; //Used for ICP Alignment
sensor_msgs::PointCloud2 output_aligned; //Output back to ROS
sensor_msgs::PointCloud2 output_valve; //Output back to ROS
//Create a new msg that will hold the pose of the valve in the cloud's frame
geometry_msgs::PoseStamped v_cloud;
//Wait for the transform to become available
std::string pointcloud_frame;
std::string target_frame;
ros::param::get("pointcloud_frame", pointcloud_frame);
ros::param::get("target_frame", target_frame);
myListener->waitForTransform((std::string)pointcloud_frame, (std::string)target_frame, req.valve.header.stamp, ros::Duration(1));
//Transform the valve into the cloud frame
myListener->transformPose((std::string)pointcloud_frame,
req.valve.header.stamp,
req.valve,
(std::string)target_frame,
v_cloud);
// myListener->transformPose((std::string)pointcloud_frame, req.valve, v_cloud);
//Initialize the cloud that will hold the points in the bounding box of the valve
input_cloud.width = 0;
input_cloud.height = 0;
input_cloud.is_dense = false;
valve_cloud.width = 0;
valve_cloud.height = 0;
valve_cloud.is_dense = false;
//Request a Cloud from the Pointcloud Client
sensor_msgs::PointCloud2 myCloud;
gui::pointcloud pointcloud_update;
pointcloud_update.request.voxel = true;
pointcloud_update.request.x = .01;
pointcloud_update.request.y = .01;
pointcloud_update.request.z = .01;
if (pointcloud_update_client.call(pointcloud_update)) {
myCloud = pointcloud_update.response.pointcloud;
}
else { ROS_ERROR("/gui/valve_icp_test.cpp: Pointcloud Update FAILED"); }
//Add a Voxel filter to the cloud before creating the points
sensor_msgs::PointCloud2 input_voxel;
const sensor_msgs::PointCloud2::Ptr input_cloud_ptr;
// ROS_INFO("About To Fail");
// *input_cloud_ptr = myCloud;
// ROS_INFO("Never Prints This");
// pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_filter;
// voxel_filter.setInputCloud(input_cloud_ptr);
// voxel_filter.setLeafSize(.01, .01, .01);
// voxel_filter.filter(input_voxel);
input_voxel = myCloud;
//These functions will convert from the sensor_msg to the cloud of real points
pcl::fromROSMsg(input_voxel, recent_cloud);
//Add points to the cloud that are in the bounding box of the valve
for (unsigned int i = 0; i < recent_cloud.size(); i++){
if ((recent_cloud.points[i].x > v_cloud.pose.position.x - .25 &&
recent_cloud.points[i].x < v_cloud.pose.position.x + .25 ) &&
(recent_cloud.points[i].y > v_cloud.pose.position.y - .25 &&
recent_cloud.points[i].y < v_cloud.pose.position.y + .25 ) &&
(recent_cloud.points[i].z > v_cloud.pose.position.z - .25 &&
recent_cloud.points[i].z < v_cloud.pose.position.z + .25 )) {
pcl::PointXYZ myPoint;
myPoint.x = recent_cloud.points[i].x;
myPoint.y = recent_cloud.points[i].y;
myPoint.z = recent_cloud.points[i].z;
input_cloud.points.push_back(myPoint);
input_cloud.width++;
input_cloud.height = 1;
}
}
//Create the two clouds that will be used by the ICP, in the future this will
//Changed so that icp_target is created above and icp_input is created from
//The location of the valve and a radius around the valve.
icp_target->width = input_cloud.width;
icp_target->height = input_cloud.height;
icp_target->is_dense = input_cloud.is_dense;
icp_target->points.resize(icp_target->width * icp_target->height);
//Create the original Valve Cloud
int points_to_create = 360;
float delta = (2 * PI) / points_to_create;
float radius = req.radius;
int rows = 2;
for (unsigned int i = 0; i < points_to_create; i++){
//TODO Check that this works
for (int j = 0; j < rows; j++) {
pcl::PointXYZ myPoint;
myPoint.x = (radius + ((float)j / 100)) * cos(i * delta);
myPoint.y = (radius + ((float)j / 100)) * sin(i * delta);
myPoint.z = 0;
valve_cloud.points.push_back(myPoint);
valve_cloud.width++;
valve_cloud.height = 1;
}
}
tf::Quaternion valveOrientation;
tf::quaternionMsgToTF(v_cloud.pose.orientation, valveOrientation);
//This is so f*****g assinine
tf::Matrix3x3 valveRotationMatrix(valveOrientation);
// ROS_INFO(" ");
// ROS_INFO("Raw Matrix From Valve");
// for (int i = 0; i < 3; i++){
// ROS_INFO("[%f, %f, %f]", valveRotationMatrix[i][0], valveRotationMatrix[i][1], valveRotationMatrix[i][2]);
// }
tfScalar yaw_1 = 0;
tfScalar pitch_1 = 0;
tfScalar roll_1 = 0;
valveRotationMatrix.getEulerYPR(yaw_1, pitch_1, roll_1);
// ROS_INFO("Yaw: %f, Pitch: %f, Roll: %f", yaw_1, pitch_1, roll_1);
tf::Matrix3x3 correction(0, 0, 1,
0, 1, 0,
-1, 0, 0);
valveRotationMatrix*=correction;
Eigen::Matrix4f valveTransformMatrix;
valveTransformMatrix(0,0) = valveRotationMatrix[0][0];
valveTransformMatrix(0,1) = valveRotationMatrix[0][1];
valveTransformMatrix(0,2) = valveRotationMatrix[0][2];
valveTransformMatrix(0,3) = v_cloud.pose.position.x;
valveTransformMatrix(1,0) = valveRotationMatrix[1][0];
valveTransformMatrix(1,1) = valveRotationMatrix[1][1];
valveTransformMatrix(1,2) = valveRotationMatrix[1][2];
valveTransformMatrix(1,3) = v_cloud.pose.position.y;
valveTransformMatrix(2,0) = valveRotationMatrix[2][0];
valveTransformMatrix(2,1) = valveRotationMatrix[2][1];
valveTransformMatrix(2,2) = valveRotationMatrix[2][2];
valveTransformMatrix(2,3) = v_cloud.pose.position.z;
valveTransformMatrix(3,0) = 0;
valveTransformMatrix(3,1) = 0;
valveTransformMatrix(3,2) = 0;
valveTransformMatrix(3,3) = 1;
pcl::transformPointCloud(valve_cloud, valve_cloud_input, valveTransformMatrix);
//Create the icp_input cloud
icp_input->width = points_to_create * rows;
icp_input->height = 1;
icp_input->is_dense = input_cloud.is_dense;
icp_input->points.resize(icp_input->width * icp_input->height);
for (int i = 0; i < valve_cloud_input.size(); i++) {
icp_input->points[i].x = valve_cloud_input.points[i].x;
icp_input->points[i].y = valve_cloud_input.points[i].y;
icp_input->points[i].z = valve_cloud_input.points[i].z;
}
//Copy the points into the new cloud and translate the valve by a little bit
//So that we can see how well the algorithm performs
for (unsigned int i = 0; i < input_cloud.size(); i++){
icp_target->points[i].x = input_cloud.points[i].x;
icp_target->points[i].y = input_cloud.points[i].y;
icp_target->points[i].z = input_cloud.points[i].z;
}
//Create a ICP object, set parameters, give it input and target
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setMaxCorrespondenceDistance (0.25f);
icp.setMaximumIterations (600);
icp.setInputCloud(icp_input);
icp.setInputTarget(icp_target);
//Align the two clouds
icp.align(Final);
//Do something to copy over output cloud here
pcl::transformPointCloud(valve_cloud_input, valve_cloud_aligned, icp.getFinalTransformation());
//EVERYTHING ABOVE HERE WORKS PERFECTLY
//Save the matrix from the transformation
Eigen::Matrix4f myTransform = icp.getFinalTransformation();
Eigen::Matrix4f finalPosition = myTransform*valveTransformMatrix;
// ROS_INFO(" ");
// ROS_INFO("Transformation Matrix");
// for (int i = 0; i < 4; i++){
// ROS_INFO("[%f, %f, %f, %f]", myTransform(i,0), myTransform(i,1), myTransform(i,2), myTransform(i,3));
// }
// ROS_INFO(" ");
// ROS_INFO("Final Position Matrix");
// for (int i = 0; i < 4; i++){
// ROS_INFO("[%f, %f, %f, %f]", finalPosition(i,0), finalPosition(i,1), finalPosition(i,2), finalPosition(i,3));
// }
tf::Matrix3x3 myTfMatrix(myTransform(0,0), myTransform(0,1), myTransform(0,2),
myTransform(1,0), myTransform(1,1), myTransform(1,2),
myTransform(2,0), myTransform(2,1), myTransform(2,2));
//Create the update to the valve
v_cloud.pose.position.x = finalPosition(0, 3);
v_cloud.pose.position.y = finalPosition(1, 3);
v_cloud.pose.position.z = finalPosition(2, 3);
// ROS_INFO(" ");
// ROS_INFO("Post Correction Matrix:");
// for (int i = 0; i < 3; i++){
// ROS_INFO("[%f, %f, %f]", myTfMatrix[i][0], myTfMatrix[i][1], myTfMatrix[i][2]); }
//Turn the rotation matrix into a Quaternion
tfScalar yaw = 0;
tfScalar pitch = 0;
tfScalar roll = 0;
myTfMatrix.getEulerYPR(yaw, pitch, roll);
// ROS_INFO("Yaw: %f, Pitch: %f, Roll: %f", yaw, roll, pitch);
//Multiply the Quaternions to get my new Orientation
// tf::Quaternion myTfQuaternion(-1 * roll, yaw, -1 * pitch);
// tf::Quaternion myTfQuaternion(roll, yaw, -1 * pitch);
tf::Quaternion myTfQuaternion(roll, yaw, -1 * pitch);
valveOrientation*=myTfQuaternion;
tf::quaternionTFToMsg(valveOrientation, v_cloud.pose.orientation);
//Transform the valve back into it's own tf
myListener->transformPose((std::string)target_frame, v_cloud, res.valve);
//TODO Delete all of this and make it populate the service response and return true
res.valve.header = req.valve.header;
return true;
//Turn the output back into a sensor_msg
// pcl::toROSMsg(valve_cloud_aligned, output_aligned);
// pcl::toROSMsg(valve_cloud_input, output_valve);
//Give the output a header so that we know where it is in the world
// output_aligned.header = recent_raw_cloud.header;
// output_valve.header = recent_raw_cloud.header;
//Publish
// pub_valve_update.publish(valve);
// pub_valve_origin.publish(output_valve);
// pub_valve_aligned.publish(output_aligned);
}
bool Map::LoadV2(FILE *f) {
uint32 data_size;
if (fread(&data_size, sizeof(data_size), 1, f) != 1) {
return false;
}
uint32 buffer_size;
if (fread(&buffer_size, sizeof(buffer_size), 1, f) != 1) {
return false;
}
std::vector<char> data;
data.resize(data_size);
if (fread(&data[0], data_size, 1, f) != 1) {
return false;
}
std::vector<char> buffer;
buffer.resize(buffer_size);
uint32 v = InflateData(&data[0], data_size, &buffer[0], buffer_size);
char *buf = &buffer[0];
uint32 vert_count;
uint32 ind_count;
uint32 nc_vert_count;
uint32 nc_ind_count;
uint32 model_count;
uint32 plac_count;
uint32 plac_group_count;
uint32 tile_count;
uint32 quads_per_tile;
float units_per_vertex;
vert_count = *(uint32*)buf;
buf += sizeof(uint32);
ind_count = *(uint32*)buf;
buf += sizeof(uint32);
nc_vert_count = *(uint32*)buf;
buf += sizeof(uint32);
nc_ind_count = *(uint32*)buf;
buf += sizeof(uint32);
model_count = *(uint32*)buf;
buf += sizeof(uint32);
plac_count = *(uint32*)buf;
buf += sizeof(uint32);
plac_group_count = *(uint32*)buf;
buf += sizeof(uint32);
tile_count = *(uint32*)buf;
buf += sizeof(uint32);
quads_per_tile = *(uint32*)buf;
buf += sizeof(uint32);
units_per_vertex = *(float*)buf;
buf += sizeof(float);
std::vector<glm::vec3> verts;
verts.reserve(vert_count);
std::vector<uint32> indices;
indices.reserve(ind_count);
for (uint32 i = 0; i < vert_count; ++i) {
float x;
float y;
float z;
x = *(float*)buf;
buf += sizeof(float);
y = *(float*)buf;
buf += sizeof(float);
z = *(float*)buf;
buf += sizeof(float);
verts.emplace_back(x, y, z);
}
for (uint32 i = 0; i < ind_count; ++i) {
indices.emplace_back(*(uint32 *)buf);
buf += sizeof(uint32);
}
for (uint32 i = 0; i < nc_vert_count; ++i) {
buf += sizeof(float) * 3;
}
for (uint32 i = 0; i < nc_ind_count; ++i) {
buf += sizeof(uint32);
}
std::map<std::string, std::unique_ptr<ModelEntry>> models;
for (uint32 i = 0; i < model_count; ++i) {
std::unique_ptr<ModelEntry> me(new ModelEntry);
std::string name = buf;
buf += name.length() + 1;
uint32 vert_count = *(uint32*)buf;
buf += sizeof(uint32);
uint32 poly_count = *(uint32*)buf;
buf += sizeof(uint32);
me->verts.reserve(vert_count);
for (uint32 j = 0; j < vert_count; ++j) {
float x = *(float*)buf;
buf += sizeof(float);
float y = *(float*)buf;
buf += sizeof(float);
float z = *(float*)buf;
buf += sizeof(float);
me->verts.emplace_back(x, y, z);
}
me->polys.reserve(poly_count);
for (uint32 j = 0; j < poly_count; ++j) {
uint32 v1 = *(uint32*)buf;
buf += sizeof(uint32);
uint32 v2 = *(uint32*)buf;
buf += sizeof(uint32);
uint32 v3 = *(uint32*)buf;
buf += sizeof(uint32);
uint8 vis = *(uint8*)buf;
buf += sizeof(uint8);
ModelEntry::Poly p;
p.v1 = v1;
p.v2 = v2;
p.v3 = v3;
p.vis = vis;
me->polys.push_back(p);
}
models[name] = std::move(me);
}
for (uint32 i = 0; i < plac_count; ++i) {
std::string name = buf;
buf += name.length() + 1;
float x = *(float*)buf;
buf += sizeof(float);
float y = *(float*)buf;
buf += sizeof(float);
float z = *(float*)buf;
buf += sizeof(float);
float x_rot = *(float*)buf;
buf += sizeof(float);
float y_rot = *(float*)buf;
buf += sizeof(float);
float z_rot = *(float*)buf;
buf += sizeof(float);
float x_scale = *(float*)buf;
buf += sizeof(float);
float y_scale = *(float*)buf;
buf += sizeof(float);
float z_scale = *(float*)buf;
buf += sizeof(float);
if (models.count(name) == 0)
continue;
auto &model = models[name];
auto &mod_polys = model->polys;
auto &mod_verts = model->verts;
for (uint32 j = 0; j < mod_polys.size(); ++j) {
auto ¤t_poly = mod_polys[j];
if (current_poly.vis == 0)
continue;
auto v1 = mod_verts[current_poly.v1];
auto v2 = mod_verts[current_poly.v2];
auto v3 = mod_verts[current_poly.v3];
RotateVertex(v1, x_rot, y_rot, z_rot);
RotateVertex(v2, x_rot, y_rot, z_rot);
RotateVertex(v3, x_rot, y_rot, z_rot);
ScaleVertex(v1, x_scale, y_scale, z_scale);
ScaleVertex(v2, x_scale, y_scale, z_scale);
ScaleVertex(v3, x_scale, y_scale, z_scale);
TranslateVertex(v1, x, y, z);
TranslateVertex(v2, x, y, z);
TranslateVertex(v3, x, y, z);
verts.emplace_back(v1.y, v1.x, v1.z); // x/y swapped
verts.emplace_back(v2.y, v2.x, v2.z);
verts.emplace_back(v3.y, v3.x, v3.z);
indices.emplace_back((uint32)verts.size() - 3);
indices.emplace_back((uint32)verts.size() - 2);
indices.emplace_back((uint32)verts.size() - 1);
}
}
for (uint32 i = 0; i < plac_group_count; ++i) {
float x = *(float*)buf;
buf += sizeof(float);
float y = *(float*)buf;
buf += sizeof(float);
float z = *(float*)buf;
buf += sizeof(float);
float x_rot = *(float*)buf;
buf += sizeof(float);
float y_rot = *(float*)buf;
buf += sizeof(float);
float z_rot = *(float*)buf;
buf += sizeof(float);
float x_scale = *(float*)buf;
buf += sizeof(float);
float y_scale = *(float*)buf;
buf += sizeof(float);
float z_scale = *(float*)buf;
buf += sizeof(float);
float x_tile = *(float*)buf;
buf += sizeof(float);
float y_tile = *(float*)buf;
buf += sizeof(float);
float z_tile = *(float*)buf;
buf += sizeof(float);
uint32 p_count = *(uint32*)buf;
buf += sizeof(uint32);
for (uint32 j = 0; j < p_count; ++j) {
std::string name = buf;
buf += name.length() + 1;
float p_x = *(float*)buf;
buf += sizeof(float);
float p_y = *(float*)buf;
buf += sizeof(float);
float p_z = *(float*)buf;
buf += sizeof(float);
float p_x_rot = *(float*)buf * 3.14159f / 180;
buf += sizeof(float);
float p_y_rot = *(float*)buf * 3.14159f / 180;
buf += sizeof(float);
float p_z_rot = *(float*)buf * 3.14159f / 180;
buf += sizeof(float);
float p_x_scale = *(float*)buf;
buf += sizeof(float);
float p_y_scale = *(float*)buf;
buf += sizeof(float);
float p_z_scale = *(float*)buf;
buf += sizeof(float);
if (models.count(name) == 0)
continue;
auto &model = models[name];
for (size_t k = 0; k < model->polys.size(); ++k) {
auto &poly = model->polys[k];
if (poly.vis == 0)
continue;
glm::vec3 v1, v2, v3;
v1 = model->verts[poly.v1];
v2 = model->verts[poly.v2];
v3 = model->verts[poly.v3];
ScaleVertex(v1, p_x_scale, p_y_scale, p_z_scale);
ScaleVertex(v2, p_x_scale, p_y_scale, p_z_scale);
ScaleVertex(v3, p_x_scale, p_y_scale, p_z_scale);
TranslateVertex(v1, p_x, p_y, p_z);
TranslateVertex(v2, p_x, p_y, p_z);
TranslateVertex(v3, p_x, p_y, p_z);
RotateVertex(v1, x_rot * 3.14159f / 180.0f, 0, 0);
RotateVertex(v2, x_rot * 3.14159f / 180.0f, 0, 0);
RotateVertex(v3, x_rot * 3.14159f / 180.0f, 0, 0);
RotateVertex(v1, 0, y_rot * 3.14159f / 180.0f, 0);
RotateVertex(v2, 0, y_rot * 3.14159f / 180.0f, 0);
RotateVertex(v3, 0, y_rot * 3.14159f / 180.0f, 0);
glm::vec3 correction(p_x, p_y, p_z);
RotateVertex(correction, x_rot * 3.14159f / 180.0f, 0, 0);
TranslateVertex(v1, -correction.x, -correction.y, -correction.z);
TranslateVertex(v2, -correction.x, -correction.y, -correction.z);
TranslateVertex(v3, -correction.x, -correction.y, -correction.z);
RotateVertex(v1, p_x_rot, 0, 0);
RotateVertex(v2, p_x_rot, 0, 0);
RotateVertex(v3, p_x_rot, 0, 0);
RotateVertex(v1, 0, -p_y_rot, 0);
RotateVertex(v2, 0, -p_y_rot, 0);
RotateVertex(v3, 0, -p_y_rot, 0);
RotateVertex(v1, 0, 0, p_z_rot);
RotateVertex(v2, 0, 0, p_z_rot);
RotateVertex(v3, 0, 0, p_z_rot);
TranslateVertex(v1, correction.x, correction.y, correction.z);
TranslateVertex(v2, correction.x, correction.y, correction.z);
TranslateVertex(v3, correction.x, correction.y, correction.z);
RotateVertex(v1, 0, 0, z_rot * 3.14159f / 180.0f);
RotateVertex(v2, 0, 0, z_rot * 3.14159f / 180.0f);
RotateVertex(v3, 0, 0, z_rot * 3.14159f / 180.0f);
ScaleVertex(v1, x_scale, y_scale, z_scale);
ScaleVertex(v2, x_scale, y_scale, z_scale);
ScaleVertex(v3, x_scale, y_scale, z_scale);
TranslateVertex(v1, x_tile, y_tile, z_tile);
TranslateVertex(v2, x_tile, y_tile, z_tile);
TranslateVertex(v3, x_tile, y_tile, z_tile);
TranslateVertex(v1, x, y, z);
TranslateVertex(v2, x, y, z);
TranslateVertex(v3, x, y, z);
verts.emplace_back(v1.y, v1.x, v1.z); // x/y swapped
verts.emplace_back(v2.y, v2.x, v2.z);
verts.emplace_back(v3.y, v3.x, v3.z);
indices.emplace_back((uint32)verts.size() - 3);
indices.emplace_back((uint32)verts.size() - 2);
indices.emplace_back((uint32)verts.size() - 1);
}
}
}
uint32 ter_quad_count = (quads_per_tile * quads_per_tile);
uint32 ter_vert_count = ((quads_per_tile + 1) * (quads_per_tile + 1));
std::vector<uint8> flags;
std::vector<float> floats;
flags.resize(ter_quad_count);
floats.resize(ter_vert_count);
for (uint32 i = 0; i < tile_count; ++i) {
bool flat;
flat = *(bool*)buf;
buf += sizeof(bool);
float x;
x = *(float*)buf;
buf += sizeof(float);
float y;
y = *(float*)buf;
buf += sizeof(float);
if (flat) {
float z;
z = *(float*)buf;
buf += sizeof(float);
float QuadVertex1X = x;
float QuadVertex1Y = y;
float QuadVertex1Z = z;
float QuadVertex2X = QuadVertex1X + (quads_per_tile * units_per_vertex);
float QuadVertex2Y = QuadVertex1Y;
float QuadVertex2Z = QuadVertex1Z;
float QuadVertex3X = QuadVertex2X;
float QuadVertex3Y = QuadVertex1Y + (quads_per_tile * units_per_vertex);
float QuadVertex3Z = QuadVertex1Z;
float QuadVertex4X = QuadVertex1X;
float QuadVertex4Y = QuadVertex3Y;
float QuadVertex4Z = QuadVertex1Z;
uint32 current_vert = (uint32)verts.size() + 3;
verts.emplace_back(QuadVertex1X, QuadVertex1Y, QuadVertex1Z);
verts.emplace_back(QuadVertex2X, QuadVertex2Y, QuadVertex2Z);
verts.emplace_back(QuadVertex3X, QuadVertex3Y, QuadVertex3Z);
verts.emplace_back(QuadVertex4X, QuadVertex4Y, QuadVertex4Z);
indices.emplace_back(current_vert);
indices.emplace_back(current_vert - 2);
indices.emplace_back(current_vert - 1);
indices.emplace_back(current_vert);
indices.emplace_back(current_vert - 3);
indices.emplace_back(current_vert - 2);
}
else {
//read flags
for (uint32 j = 0; j < ter_quad_count; ++j) {
uint8 f;
f = *(uint8*)buf;
buf += sizeof(uint8);
flags[j] = f;
}
//read floats
for (uint32 j = 0; j < ter_vert_count; ++j) {
float f;
f = *(float*)buf;
buf += sizeof(float);
floats[j] = f;
}
int row_number = -1;
std::map<std::tuple<float, float, float>, uint32> cur_verts;
for (uint32 quad = 0; quad < ter_quad_count; ++quad) {
if ((quad % quads_per_tile) == 0) {
++row_number;
}
if (flags[quad] & 0x01)
continue;
float QuadVertex1X = x + (row_number * units_per_vertex);
float QuadVertex1Y = y + (quad % quads_per_tile) * units_per_vertex;
float QuadVertex1Z = floats[quad + row_number];
float QuadVertex2X = QuadVertex1X + units_per_vertex;
float QuadVertex2Y = QuadVertex1Y;
float QuadVertex2Z = floats[quad + row_number + quads_per_tile + 1];
float QuadVertex3X = QuadVertex1X + units_per_vertex;
float QuadVertex3Y = QuadVertex1Y + units_per_vertex;
float QuadVertex3Z = floats[quad + row_number + quads_per_tile + 2];
float QuadVertex4X = QuadVertex1X;
float QuadVertex4Y = QuadVertex1Y + units_per_vertex;
float QuadVertex4Z = floats[quad + row_number + 1];
uint32 i1, i2, i3, i4;
std::tuple<float, float, float> t = std::make_tuple(QuadVertex1X, QuadVertex1Y, QuadVertex1Z);
auto iter = cur_verts.find(t);
if (iter != cur_verts.end()) {
i1 = iter->second;
}
else {
i1 = (uint32)verts.size();
verts.emplace_back(QuadVertex1X, QuadVertex1Y, QuadVertex1Z);
cur_verts[std::make_tuple(QuadVertex1X, QuadVertex1Y, QuadVertex1Z)] = i1;
}
t = std::make_tuple(QuadVertex2X, QuadVertex2Y, QuadVertex2Z);
iter = cur_verts.find(t);
if (iter != cur_verts.end()) {
i2 = iter->second;
}
else {
i2 = (uint32)verts.size();
verts.emplace_back(QuadVertex2X, QuadVertex2Y, QuadVertex2Z);
cur_verts[std::make_tuple(QuadVertex2X, QuadVertex2Y, QuadVertex2Z)] = i2;
}
t = std::make_tuple(QuadVertex3X, QuadVertex3Y, QuadVertex3Z);
iter = cur_verts.find(t);
if (iter != cur_verts.end()) {
i3 = iter->second;
}
else {
i3 = (uint32)verts.size();
verts.emplace_back(QuadVertex3X, QuadVertex3Y, QuadVertex3Z);
cur_verts[std::make_tuple(QuadVertex3X, QuadVertex3Y, QuadVertex3Z)] = i3;
}
t = std::make_tuple(QuadVertex4X, QuadVertex4Y, QuadVertex4Z);
iter = cur_verts.find(t);
if (iter != cur_verts.end()) {
i4 = iter->second;
}
else {
i4 = (uint32)verts.size();
verts.emplace_back(QuadVertex4X, QuadVertex4Y, QuadVertex4Z);
cur_verts[std::make_tuple(QuadVertex4X, QuadVertex4Y, QuadVertex4Z)] = i4;
}
indices.emplace_back(i4);
indices.emplace_back(i2);
indices.emplace_back(i3);
indices.emplace_back(i4);
indices.emplace_back(i1);
indices.emplace_back(i2);
}
}
}
uint32 face_count = indices.size() / 3;
if (imp) {
imp->rm->release();
imp->rm = nullptr;
}
else {
imp = new impl;
}
imp->rm = createRaycastMesh((RmUint32)verts.size(), (const RmReal*)&verts[0], face_count, &indices[0]);
if (!imp->rm) {
delete imp;
imp = nullptr;
return false;
}
return true;
}
int main(int argc, char* argv[])
{
#ifdef CH_MPI
MPI_Init (&argc, &argv);
#endif
// test parameters
const int nGrids = 3;
const int nCells0 = 32;
// xLo has to be zero in order for DiriBc to work.
const RealVect xLo = RealVect::Zero;
const Real xHi = 1.0;
const Box box0(IntVect::Zero, (nCells0-1)*IntVect::Unit);
const int nGhosts = 1;
const int resNT = 2; // norm Type
const int errNT = 0;
// A test is considered as a failure
// if its convergence rate is smaller than below.
const Real targetConvergeRate = 1.75;
// solver parameters
// To converge within 10 V-Cycles in 1D,
// nRelax=3 is the minimum number of relaxations.
const int nRelax = 3; // m_pre=m_post
// cycle Type, 1 : V-Cycle; -1 : FMG-Cycle
const int cycleType[2] =
{
1, -1
};
const std::string cycleStr[2] =
{
" V" , " FMG"
};
// test results holder
const int nCycles[2] =
{
9, 5
};
const int maxCycles = 10; // > max(nCycles)
// Real resNorm[nGrids][nCycles+1], errNorm[nGrids][nCycles+1];
Real resNorm[nGrids][maxCycles], errNorm[nGrids][maxCycles];
Real convergeRate[nGrids-1][2];
const Real log2r = 1.0/log(2.0);
// status records the number of errors detected.
int status = 0;
for (int j=0; j<2; j++)
{
pout() << "\n**************************************************\n"
<< "\nTesting MultiGrid::oneCycle(correction, residual)\n"
<< " cycle type = " << cycleStr[j]
<< "; m_pre = m_post = " << nRelax << "\n";
for (int iGrid=0; iGrid<nGrids; iGrid++)
{
int ref = 1;
for (int i=0; i<iGrid; i++)
ref*=2;
const Real dx = xHi/nCells0/ref;
const Box domain = refine(box0,ref);
const Box ghostBox = grow(domain,nGhosts);
pout() << "\n----------------------------------------------------\n";
pout() << "nCells = " << nCells0*ref << " ; dx = " << dx << " \n";
FArrayBox phi(ghostBox, 1);
FArrayBox correction(ghostBox, 1);
FArrayBox rhs(domain, 1);
FArrayBox error(domain, 1);
FArrayBox phiExact(domain, 1);
FArrayBox residual(domain, 1);
// set initial guess
phi.setVal(0.0);
// set RHS and the exact solution
for (BoxIterator bit(domain); bit.ok(); ++bit)
{
const RealVect offset = bit()-domain.smallEnd();
const RealVect x = xLo + dx*(0.5+offset);
rhs(bit()) = rhsFunc( x );
phiExact(bit()) = exactSolution( x );
}
// Initialize big objects
NewPoissonOpFactory opFactory;
opFactory.define(dx*RealVect(IntVect::Unit), constDiriBC);
MultiGrid<FArrayBox> solver;
BiCGStabSolver<FArrayBox> bottomSolver;
bottomSolver.m_verbosity = 0;
MGLevelOp<FArrayBox>* op = opFactory.MGnewOp(domain,0);
solver.m_numMG = 1;
solver.m_bottom = 1;
solver.m_pre = nRelax;
solver.m_post = nRelax;
solver.m_cycle = cycleType[j];
solver.define(opFactory, &bottomSolver, domain);
// put the data into residual-correction form
op->residual(residual, phi, rhs);
resNorm[iGrid][0] = residual.norm(resNT);
op->axby(error, phi, phiExact, 1, -1);
errNorm[iGrid][0] = error.norm(errNT);
solver.init(correction, residual);
// Solve the problem using MultiGrid::oneCycle
for (int i=0; i<nCycles[j]; i++)
{
correction.setVal(0.0);
solver.oneCycle(correction, residual);
op->incr(phi, correction, 1);
op->residual(residual, phi, rhs);
resNorm[iGrid][i+1] = residual.norm(resNT);
op->axby(error, phi, phiExact, 1, -1);
errNorm[iGrid][i+1] = error.norm(errNT);
}
delete op;
// output a table of results
pout()<< cycleStr[j] << "-Cycle N.O. | residual " << resNT
<< "-norm | Error " << errNT << "-norm \n";
for (int i=0; i<nCycles[j]+1; i++)
{
pout() << " " << i << " | " << resNorm[iGrid][i]
<< " | " << errNorm[iGrid][i] << "\n";
}
} // end grid loop
pout() << "\nConvergence Rate based on the error in the last cycle:\n";
for (int i=0; i<nGrids-1; i++)
{
Real ratio = errNorm[i][nCycles[j]]/errNorm[i+1][nCycles[j]];
convergeRate[i][j] = log(ratio)*log2r;
if (convergeRate[i][j] < targetConvergeRate)
{
status += 1;
}
pout() << " " << convergeRate[i][j] << "\n";
}
}// end cycle type
if (status==0)
{
pout() << "All tests passed!\n";
}
else
{
pout() << status << " tests failed!\n";
}
#ifdef CH_MPI
MPI_Finalize ();
#endif
return status;
}
std::vector<value_type> corrections() const { return { correction() }; }
sendHMI(" end correction");
return true;
}
else{
sendHMI(" end correction");
return false;
}
}
return false;
}
void timeProtocolTask(void){
/*
#ifdef SLAVEMODE
delayRequest();
#else
sync();
#endif
while(1){
//sendHMI("time protocol task.");
//pc.printf("timeprotocol task");
if(timeProt.synchroTimeReceive!=NULL){
sender();
if( xSemaphoreTake(timeProt.synchroTimeReceive,500/portTICK_RATE_MS) == pdTRUE ){
vTaskDelay(100/portTICK_RATE_MS);
receiver();
}
}
correction();
}
vTaskDelay(500/portTICK_RATE_MS);
*/
Disable_global_interrupt();
#ifdef MASTERMODE
uint8_t i ;
for(i=0;i<=MAX_SLAVE_CLOCK;i++){
timeProt.saveTime[i].second=0;
}
#else
timeProt.rx.sign=true;
timeProt.rxDelay.sign=true;
timeProt.rxSync.sign=true;
timeProt.tx.sign=true;
#endif
timeProt.correction.nbCorrection=0;
sumOffset.second=0;
sumOffset.halfmillis=0;
sumOffset.sign=true;
timeProt.correction.currentTimeOffsetSync.second=0;
timeProt.correction.currentTimeOffsetSync.halfmillis=0;
timeProt.correction.currentTimeOffsetSync.sign=true;
timeProt.correction.previoustimeOffset.second=0;
timeProt.correction.previoustimeOffset.halfmillis=0;
timeProt.correction.previoustimeOffset.sign=true;
//timeProt.correction.timeSoftCor=0;
timeProt.correction.indiceMoySoftCor=0;
timeProt.correction.indiceFull=0;
timeProt.delay.second=0;
log_double_t acceptance_ratio(const Parameters& P1,const vector<int>& nodes1,
const Parameters& P2,const vector<int>& nodes2)
{
return correction(P1,nodes1)/correction(P2,nodes2);
}
void KalmanFilter::updateWithObservation(messages::VBall visionBall)
{
// Declare C and C transpose (ublas)
// C takes state estimate to observation frame so
// c = 1 0 0 0
// 0 1 0 0
ufmatrix c (2,4);
for(unsigned i=0; i<c.size1(); i++){
for(unsigned j=0; j<c.size2(); j++){
if(i == j)
c(i,j) = 1.f;
else
c(i,j) = 0.f;
}
}
ufmatrix cTranspose(4,2);
cTranspose = trans(c);
// Calculate the gain
// Calc c*cov*c^t
ufmatrix cCovCTranspose(2,2);
cCovCTranspose = prod(cov,cTranspose);
cCovCTranspose = prod(c,cCovCTranspose);
cCovCTranspose(0,0) += params.obsvRelXVariance;
cCovCTranspose(1,1) += params.obsvRelYVariance;
// gain = cov*c^t*(c*cov*c^t + var)^-1
ufmatrix kalmanGain(2,2);
kalmanGain = prod(cTranspose,NBMath::invert2by2(cCovCTranspose));
kalmanGain = prod(cov,kalmanGain);
ufvector posEstimates(2);
posEstimates = prod(c, x);
// x straight ahead, y to the right
float sinB,cosB;
sincosf(visionBall.bearing(),&sinB,&cosB);
ufvector measurement(2);
measurement(0) = visionBall.distance()*cosB;
measurement(1) = visionBall.distance()*sinB;
ufvector innovation(2);
innovation = measurement - posEstimates;
ufvector correction(4);
correction = prod(kalmanGain,innovation);
x += correction;
//cov = cov - k*c*cov
ufmatrix4 identity;
identity = boost::numeric::ublas::identity_matrix <float>(4);
ufmatrix4 modifyCov;
modifyCov = identity - prod(kalmanGain,c);
cov = prod(modifyCov,cov);
// Housekeep
filteredDist = std::sqrt(x(0)*x(0) + x(1)*x(1));
filteredBear = NBMath::safe_atan2(x(1),x(0));
float curErr = std::sqrt(correction(0)*correction(0) + correction(1)*correction(1));
correctionMagBuffer[curEntry] = curErr;
score = 0.f;
for(int i=0; i<BUFF_LENGTH; i++)
score+= correctionMagBuffer[i];
score = score/10; // avg correction over 10 frames
//get magnitude of correction
// std::cout << "Is moving filter " << isStationary() << std::endl;
// std::cout << "Score: " << score << " cur error " << curErr << std::endl;
}
void MappingExtrapolate::v_CorrectPressureBCs(
const Array<OneD, NekDouble> &pressure)
{
if(m_HBCdata.num_elements()>0)
{
int cnt, n;
int physTot = m_fields[0]->GetTotPoints();
int nvel = m_fields.num_elements()-1;
Array<OneD, NekDouble> Vals;
// Remove previous correction
for(cnt = n = 0; n < m_PBndConds.num_elements(); ++n)
{
if(m_PBndConds[n]->GetUserDefined() == "H")
{
int nq = m_PBndExp[n]->GetNcoeffs();
Vmath::Vsub(nq, &(m_PBndExp[n]->GetCoeffs()[0]), 1,
&(m_bcCorrection[cnt]), 1,
&(m_PBndExp[n]->UpdateCoeffs()[0]), 1);
cnt += nq;
}
}
// Calculate new correction
Array<OneD, NekDouble> Jac(physTot, 0.0);
m_mapping->GetJacobian(Jac);
Array<OneD, Array<OneD, NekDouble> > correction(nvel);
Array<OneD, Array<OneD, NekDouble> > gradP(nvel);
Array<OneD, Array<OneD, NekDouble> > wk(nvel);
Array<OneD, Array<OneD, NekDouble> > wk2(nvel);
for (int i=0; i<nvel; i++)
{
wk[i] = Array<OneD, NekDouble> (physTot, 0.0);
gradP[i] = Array<OneD, NekDouble> (physTot, 0.0);
correction[i] = Array<OneD, NekDouble> (physTot, 0.0);
}
// Calculate G(p)
for(int i = 0; i < nvel; ++i)
{
m_fields[0]->PhysDeriv(MultiRegions::DirCartesianMap[i], pressure, gradP[i]);
if(m_fields[0]->GetWaveSpace())
{
m_fields[0]->HomogeneousBwdTrans(gradP[i], wk[i]);
}
else
{
Vmath::Vcopy(physTot, gradP[i], 1, wk[i], 1);
}
}
m_mapping->RaiseIndex(wk, correction); // G(p)
// alpha*J*(G(p))
if (!m_mapping->HasConstantJacobian())
{
for(int i = 0; i < nvel; ++i)
{
Vmath::Vmul(physTot, correction[i], 1, Jac, 1, correction[i], 1);
}
}
for(int i = 0; i < nvel; ++i)
{
Vmath::Smul(physTot, m_pressureRelaxation, correction[i], 1, correction[i], 1);
}
if(m_pressure->GetWaveSpace())
{
for(int i = 0; i < nvel; ++i)
{
m_pressure->HomogeneousFwdTrans(correction[i], correction[i]);
}
}
// p_i - alpha*J*div(G(p))
for (int i = 0; i < nvel; ++i)
{
Vmath::Vsub(physTot, gradP[i], 1, correction[i], 1, correction[i], 1);
}
// Get value at boundary and calculate Inner product
StdRegions::StdExpansionSharedPtr Pbc;
StdRegions::StdExpansionSharedPtr elmt;
Array<OneD, Array<OneD, const NekDouble> > correctionElmt(m_bnd_dim);
Array<OneD, Array<OneD, NekDouble> > BndValues(m_bnd_dim);
for(int i = 0; i < m_bnd_dim; i++)
{
BndValues[i] = Array<OneD, NekDouble> (m_pressureBCsMaxPts,0.0);
}
for(int j = 0 ; j < m_HBCdata.num_elements() ; j++)
{
/// Casting the boundary expansion to the specific case
Pbc = boost::dynamic_pointer_cast<StdRegions::StdExpansion>
(m_PBndExp[m_HBCdata[j].m_bndryID]
->GetExp(m_HBCdata[j].m_bndElmtID));
/// Picking up the element where the HOPBc is located
elmt = m_pressure->GetExp(m_HBCdata[j].m_globalElmtID);
/// Assigning
for(int i = 0; i < m_bnd_dim; i++)
{
correctionElmt[i] = correction[i] + m_HBCdata[j].m_physOffset;
}
Vals = m_bcCorrection + m_HBCdata[j].m_coeffOffset;
// Getting values on the edge and filling the correction
switch(m_pressure->GetExpType())
{
case MultiRegions::e2D:
case MultiRegions::e3DH1D:
{
elmt->GetEdgePhysVals(m_HBCdata[j].m_elmtTraceID, Pbc,
correctionElmt[0], BndValues[0]);
elmt->GetEdgePhysVals(m_HBCdata[j].m_elmtTraceID, Pbc,
correctionElmt[1], BndValues[1]);
// InnerProduct
Pbc->NormVectorIProductWRTBase(BndValues[0], BndValues[1],
Vals);
}
break;
case MultiRegions::e3D:
{
elmt->GetFacePhysVals(m_HBCdata[j].m_elmtTraceID, Pbc,
correctionElmt[0], BndValues[0]);
elmt->GetFacePhysVals(m_HBCdata[j].m_elmtTraceID, Pbc,
correctionElmt[1], BndValues[1]);
elmt->GetFacePhysVals(m_HBCdata[j].m_elmtTraceID, Pbc,
correctionElmt[2], BndValues[2]);
Pbc->NormVectorIProductWRTBase(BndValues[0], BndValues[1],
BndValues[2], Vals);
}
break;
default:
ASSERTL0(0,"Dimension not supported");
break;
}
}
// Apply new correction
for(cnt = n = 0; n < m_PBndConds.num_elements(); ++n)
{
if(m_PBndConds[n]->GetUserDefined() == "H")
{
int nq = m_PBndExp[n]->GetNcoeffs();
Vmath::Vadd(nq, &(m_PBndExp[n]->GetCoeffs()[0]), 1,
&(m_bcCorrection[cnt]), 1,
&(m_PBndExp[n]->UpdateCoeffs()[0]), 1);
cnt += nq;
}
}
}
}
bool ZoneMap::LoadV2(FILE *f) {
uint32_t data_size;
if (fread(&data_size, sizeof(data_size), 1, f) != 1) {
return false;
}
uint32_t buffer_size;
if (fread(&buffer_size, sizeof(buffer_size), 1, f) != 1) {
return false;
}
std::vector<char> data;
data.resize(data_size);
if (fread(&data[0], data_size, 1, f) != 1) {
return false;
}
std::vector<char> buffer;
buffer.resize(buffer_size);
uint32_t v = InflateData(&data[0], data_size, &buffer[0], buffer_size);
char *buf = &buffer[0];
uint32_t vert_count;
uint32_t ind_count;
uint32_t nc_vert_count;
uint32_t nc_ind_count;
uint32_t model_count;
uint32_t plac_count;
uint32_t plac_group_count;
uint32_t tile_count;
uint32_t quads_per_tile;
float units_per_vertex;
vert_count = *(uint32_t*)buf;
buf += sizeof(uint32_t);
ind_count = *(uint32_t*)buf;
buf += sizeof(uint32_t);
nc_vert_count = *(uint32_t*)buf;
buf += sizeof(uint32_t);
nc_ind_count = *(uint32_t*)buf;
buf += sizeof(uint32_t);
model_count = *(uint32_t*)buf;
buf += sizeof(uint32_t);
plac_count = *(uint32_t*)buf;
buf += sizeof(uint32_t);
plac_group_count = *(uint32_t*)buf;
buf += sizeof(uint32_t);
tile_count = *(uint32_t*)buf;
buf += sizeof(uint32_t);
quads_per_tile = *(uint32_t*)buf;
buf += sizeof(uint32_t);
units_per_vertex = *(float*)buf;
buf += sizeof(float);
for (uint32_t i = 0; i < vert_count; ++i) {
float x;
float y;
float z;
x = *(float*)buf;
buf += sizeof(float);
y = *(float*)buf;
buf += sizeof(float);
z = *(float*)buf;
buf += sizeof(float);
glm::vec3 vert(x, y, z);
imp->verts.push_back(vert);
}
for (uint32_t i = 0; i < ind_count; ++i) {
uint32_t index;
index = *(uint32_t*)buf;
buf += sizeof(uint32_t);
imp->inds.push_back(index);
}
for (uint32_t i = 0; i < nc_vert_count; ++i) {
float x;
float y;
float z;
x = *(float*)buf;
buf += sizeof(float);
y = *(float*)buf;
buf += sizeof(float);
z = *(float*)buf;
buf += sizeof(float);
glm::vec3 vert(x, y, z);
imp->nc_verts.push_back(vert);
}
for (uint32_t i = 0; i < nc_ind_count; ++i) {
uint32_t index;
index = *(uint32_t*)buf;
buf += sizeof(uint32_t);
imp->nc_inds.push_back(index);
}
std::map<std::string, std::shared_ptr<ModelEntry>> models;
for (uint32_t i = 0; i < model_count; ++i) {
std::shared_ptr<ModelEntry> me(new ModelEntry);
std::string name = buf;
buf += name.length() + 1;
uint32_t vert_count = *(uint32_t*)buf;
buf += sizeof(uint32_t);
uint32_t poly_count = *(uint32_t*)buf;
buf += sizeof(uint32_t);
me->verts.resize(vert_count);
for (uint32_t j = 0; j < vert_count; ++j) {
float x = *(float*)buf;
buf += sizeof(float);
float y = *(float*)buf;
buf += sizeof(float);
float z = *(float*)buf;
buf += sizeof(float);
me->verts[j] = glm::vec3(x, y, z);
}
me->polys.resize(poly_count);
for (uint32_t j = 0; j < poly_count; ++j) {
uint32_t v1 = *(uint32_t*)buf;
buf += sizeof(uint32_t);
uint32_t v2 = *(uint32_t*)buf;
buf += sizeof(uint32_t);
uint32_t v3 = *(uint32_t*)buf;
buf += sizeof(uint32_t);
uint8_t vis = *(uint8_t*)buf;
buf += sizeof(uint8_t);
ModelEntry::Poly p;
p.v1 = v1;
p.v2 = v2;
p.v3 = v3;
p.vis = vis;
me->polys[j] = p;
}
models[name] = me;
}
for (uint32_t i = 0; i < plac_count; ++i) {
std::string name = buf;
buf += name.length() + 1;
float x = *(float*)buf;
buf += sizeof(float);
float y = *(float*)buf;
buf += sizeof(float);
float z = *(float*)buf;
buf += sizeof(float);
float x_rot = *(float*)buf;
buf += sizeof(float);
float y_rot = *(float*)buf;
buf += sizeof(float);
float z_rot = *(float*)buf;
buf += sizeof(float);
float x_scale = *(float*)buf;
buf += sizeof(float);
float y_scale = *(float*)buf;
buf += sizeof(float);
float z_scale = *(float*)buf;
buf += sizeof(float);
if (models.count(name) == 0)
continue;
auto model = models[name];
auto &mod_polys = model->polys;
auto &mod_verts = model->verts;
for (uint32_t j = 0; j < mod_polys.size(); ++j) {
auto ¤t_poly = mod_polys[j];
auto v1 = mod_verts[current_poly.v1];
auto v2 = mod_verts[current_poly.v2];
auto v3 = mod_verts[current_poly.v3];
RotateVertex(v1, x_rot, y_rot, z_rot);
RotateVertex(v2, x_rot, y_rot, z_rot);
RotateVertex(v3, x_rot, y_rot, z_rot);
ScaleVertex(v1, x_scale, y_scale, z_scale);
ScaleVertex(v2, x_scale, y_scale, z_scale);
ScaleVertex(v3, x_scale, y_scale, z_scale);
TranslateVertex(v1, x, y, z);
TranslateVertex(v2, x, y, z);
TranslateVertex(v3, x, y, z);
float t = v1.x;
v1.x = v1.y;
v1.y = t;
t = v2.x;
v2.x = v2.y;
v2.y = t;
t = v3.x;
v3.x = v3.y;
v3.y = t;
if (current_poly.vis != 0) {
imp->verts.push_back(v1);
imp->verts.push_back(v2);
imp->verts.push_back(v3);
imp->inds.push_back((uint32_t)imp->verts.size() - 3);
imp->inds.push_back((uint32_t)imp->verts.size() - 2);
imp->inds.push_back((uint32_t)imp->verts.size() - 1);
} else {
imp->nc_verts.push_back(v1);
imp->nc_verts.push_back(v2);
imp->nc_verts.push_back(v3);
imp->nc_inds.push_back((uint32_t)imp->nc_verts.size() - 3);
imp->nc_inds.push_back((uint32_t)imp->nc_verts.size() - 2);
imp->nc_inds.push_back((uint32_t)imp->nc_verts.size() - 1);
}
}
}
for (uint32_t i = 0; i < plac_group_count; ++i) {
float x = *(float*)buf;
buf += sizeof(float);
float y = *(float*)buf;
buf += sizeof(float);
float z = *(float*)buf;
buf += sizeof(float);
float x_rot = *(float*)buf;
buf += sizeof(float);
float y_rot = *(float*)buf;
buf += sizeof(float);
float z_rot = *(float*)buf;
buf += sizeof(float);
float x_scale = *(float*)buf;
buf += sizeof(float);
float y_scale = *(float*)buf;
buf += sizeof(float);
float z_scale = *(float*)buf;
buf += sizeof(float);
float x_tile = *(float*)buf;
buf += sizeof(float);
float y_tile = *(float*)buf;
buf += sizeof(float);
float z_tile = *(float*)buf;
buf += sizeof(float);
uint32_t p_count = *(uint32_t*)buf;
buf += sizeof(uint32_t);
for (uint32_t j = 0; j < p_count; ++j) {
std::string name = buf;
buf += name.length() + 1;
float p_x = *(float*)buf;
buf += sizeof(float);
float p_y = *(float*)buf;
buf += sizeof(float);
float p_z = *(float*)buf;
buf += sizeof(float);
float p_x_rot = *(float*)buf * 3.14159f / 180;
buf += sizeof(float);
float p_y_rot = *(float*)buf * 3.14159f / 180;
buf += sizeof(float);
float p_z_rot = *(float*)buf * 3.14159f / 180;
buf += sizeof(float);
float p_x_scale = *(float*)buf;
buf += sizeof(float);
float p_y_scale = *(float*)buf;
buf += sizeof(float);
float p_z_scale = *(float*)buf;
buf += sizeof(float);
if (models.count(name) == 0)
continue;
auto &model = models[name];
for (size_t k = 0; k < model->polys.size(); ++k) {
auto &poly = model->polys[k];
glm::vec3 v1, v2, v3;
v1 = model->verts[poly.v1];
v2 = model->verts[poly.v2];
v3 = model->verts[poly.v3];
ScaleVertex(v1, p_x_scale, p_y_scale, p_z_scale);
ScaleVertex(v2, p_x_scale, p_y_scale, p_z_scale);
ScaleVertex(v3, p_x_scale, p_y_scale, p_z_scale);
TranslateVertex(v1, p_x, p_y, p_z);
TranslateVertex(v2, p_x, p_y, p_z);
TranslateVertex(v3, p_x, p_y, p_z);
RotateVertex(v1, x_rot * 3.14159f / 180.0f, 0, 0);
RotateVertex(v2, x_rot * 3.14159f / 180.0f, 0, 0);
RotateVertex(v3, x_rot * 3.14159f / 180.0f, 0, 0);
RotateVertex(v1, 0, y_rot * 3.14159f / 180.0f, 0);
RotateVertex(v2, 0, y_rot * 3.14159f / 180.0f, 0);
RotateVertex(v3, 0, y_rot * 3.14159f / 180.0f, 0);
glm::vec3 correction(p_x, p_y, p_z);
RotateVertex(correction, x_rot * 3.14159f / 180.0f, 0, 0);
TranslateVertex(v1, -correction.x, -correction.y, -correction.z);
TranslateVertex(v2, -correction.x, -correction.y, -correction.z);
TranslateVertex(v3, -correction.x, -correction.y, -correction.z);
RotateVertex(v1, p_x_rot, 0, 0);
RotateVertex(v2, p_x_rot, 0, 0);
RotateVertex(v3, p_x_rot, 0, 0);
RotateVertex(v1, 0, -p_y_rot, 0);
RotateVertex(v2, 0, -p_y_rot, 0);
RotateVertex(v3, 0, -p_y_rot, 0);
RotateVertex(v1, 0, 0, p_z_rot);
RotateVertex(v2, 0, 0, p_z_rot);
RotateVertex(v3, 0, 0, p_z_rot);
TranslateVertex(v1, correction.x, correction.y, correction.z);
TranslateVertex(v2, correction.x, correction.y, correction.z);
TranslateVertex(v3, correction.x, correction.y, correction.z);
RotateVertex(v1, 0, 0, z_rot * 3.14159f / 180.0f);
RotateVertex(v2, 0, 0, z_rot * 3.14159f / 180.0f);
RotateVertex(v3, 0, 0, z_rot * 3.14159f / 180.0f);
ScaleVertex(v1, x_scale, y_scale, z_scale);
ScaleVertex(v2, x_scale, y_scale, z_scale);
ScaleVertex(v3, x_scale, y_scale, z_scale);
TranslateVertex(v1, x_tile, y_tile, z_tile);
TranslateVertex(v2, x_tile, y_tile, z_tile);
TranslateVertex(v3, x_tile, y_tile, z_tile);
TranslateVertex(v1, x, y, z);
TranslateVertex(v2, x, y, z);
TranslateVertex(v3, x, y, z);
float t = v1.x;
v1.x = v1.y;
v1.y = t;
t = v2.x;
v2.x = v2.y;
v2.y = t;
t = v3.x;
v3.x = v3.y;
v3.y = t;
if (poly.vis != 0) {
imp->verts.push_back(v1);
imp->verts.push_back(v2);
imp->verts.push_back(v3);
imp->inds.push_back((uint32_t)imp->verts.size() - 3);
imp->inds.push_back((uint32_t)imp->verts.size() - 2);
imp->inds.push_back((uint32_t)imp->verts.size() - 1);
} else {
imp->nc_verts.push_back(v1);
imp->nc_verts.push_back(v2);
imp->nc_verts.push_back(v3);
imp->nc_inds.push_back((uint32_t)imp->nc_verts.size() - 3);
imp->nc_inds.push_back((uint32_t)imp->nc_verts.size() - 2);
imp->nc_inds.push_back((uint32_t)imp->nc_verts.size() - 1);
}
}
}
}
uint32_t ter_quad_count = (quads_per_tile * quads_per_tile);
uint32_t ter_vert_count = ((quads_per_tile + 1) * (quads_per_tile + 1));
std::vector<uint8_t> flags;
std::vector<float> floats;
flags.resize(ter_quad_count);
floats.resize(ter_vert_count);
for (uint32_t i = 0; i < tile_count; ++i) {
bool flat;
flat = *(bool*)buf;
buf += sizeof(bool);
float x;
x = *(float*)buf;
buf += sizeof(float);
float y;
y = *(float*)buf;
buf += sizeof(float);
if (flat) {
float z;
z = *(float*)buf;
buf += sizeof(float);
float QuadVertex1X = x;
float QuadVertex1Y = y;
float QuadVertex1Z = z;
float QuadVertex2X = QuadVertex1X + (quads_per_tile * units_per_vertex);
float QuadVertex2Y = QuadVertex1Y;
float QuadVertex2Z = QuadVertex1Z;
float QuadVertex3X = QuadVertex2X;
float QuadVertex3Y = QuadVertex1Y + (quads_per_tile * units_per_vertex);
float QuadVertex3Z = QuadVertex1Z;
float QuadVertex4X = QuadVertex1X;
float QuadVertex4Y = QuadVertex3Y;
float QuadVertex4Z = QuadVertex1Z;
uint32_t current_vert = (uint32_t)imp->verts.size() + 3;
imp->verts.push_back(glm::vec3(QuadVertex1X, QuadVertex1Y, QuadVertex1Z));
imp->verts.push_back(glm::vec3(QuadVertex2X, QuadVertex2Y, QuadVertex2Z));
imp->verts.push_back(glm::vec3(QuadVertex3X, QuadVertex3Y, QuadVertex3Z));
imp->verts.push_back(glm::vec3(QuadVertex4X, QuadVertex4Y, QuadVertex4Z));
imp->inds.push_back(current_vert - 0);
imp->inds.push_back(current_vert - 1);
imp->inds.push_back(current_vert - 2);
imp->inds.push_back(current_vert - 2);
imp->inds.push_back(current_vert - 3);
imp->inds.push_back(current_vert - 0);
}
else {
//read flags
for (uint32_t j = 0; j < ter_quad_count; ++j) {
uint8_t f;
f = *(uint8_t*)buf;
buf += sizeof(uint8_t);
flags[j] = f;
}
//read floats
for (uint32_t j = 0; j < ter_vert_count; ++j) {
float f;
f = *(float*)buf;
buf += sizeof(float);
floats[j] = f;
}
int row_number = -1;
std::map<std::tuple<float, float, float>, uint32_t> cur_verts;
for (uint32_t quad = 0; quad < ter_quad_count; ++quad) {
if ((quad % quads_per_tile) == 0) {
++row_number;
}
if (flags[quad] & 0x01)
continue;
float QuadVertex1X = x + (row_number * units_per_vertex);
float QuadVertex1Y = y + (quad % quads_per_tile) * units_per_vertex;
float QuadVertex1Z = floats[quad + row_number];
float QuadVertex2X = QuadVertex1X + units_per_vertex;
float QuadVertex2Y = QuadVertex1Y;
float QuadVertex2Z = floats[quad + row_number + quads_per_tile + 1];
float QuadVertex3X = QuadVertex1X + units_per_vertex;
float QuadVertex3Y = QuadVertex1Y + units_per_vertex;
float QuadVertex3Z = floats[quad + row_number + quads_per_tile + 2];
float QuadVertex4X = QuadVertex1X;
float QuadVertex4Y = QuadVertex1Y + units_per_vertex;
float QuadVertex4Z = floats[quad + row_number + 1];
uint32_t i1, i2, i3, i4;
std::tuple<float, float, float> t = std::make_tuple(QuadVertex1X, QuadVertex1Y, QuadVertex1Z);
auto iter = cur_verts.find(t);
if (iter != cur_verts.end()) {
i1 = iter->second;
}
else {
i1 = (uint32_t)imp->verts.size();
imp->verts.push_back(glm::vec3(QuadVertex1X, QuadVertex1Y, QuadVertex1Z));
cur_verts[std::make_tuple(QuadVertex1X, QuadVertex1Y, QuadVertex1Z)] = i1;
}
t = std::make_tuple(QuadVertex2X, QuadVertex2Y, QuadVertex2Z);
iter = cur_verts.find(t);
if (iter != cur_verts.end()) {
i2 = iter->second;
}
else {
i2 = (uint32_t)imp->verts.size();
imp->verts.push_back(glm::vec3(QuadVertex2X, QuadVertex2Y, QuadVertex2Z));
cur_verts[std::make_tuple(QuadVertex2X, QuadVertex2Y, QuadVertex2Z)] = i2;
}
t = std::make_tuple(QuadVertex3X, QuadVertex3Y, QuadVertex3Z);
iter = cur_verts.find(t);
if (iter != cur_verts.end()) {
i3 = iter->second;
}
else {
i3 = (uint32_t)imp->verts.size();
imp->verts.push_back(glm::vec3(QuadVertex3X, QuadVertex3Y, QuadVertex3Z));
cur_verts[std::make_tuple(QuadVertex3X, QuadVertex3Y, QuadVertex3Z)] = i3;
}
t = std::make_tuple(QuadVertex4X, QuadVertex4Y, QuadVertex4Z);
iter = cur_verts.find(t);
if (iter != cur_verts.end()) {
i4 = iter->second;
}
else {
i4 = (uint32_t)imp->verts.size();
imp->verts.push_back(glm::vec3(QuadVertex4X, QuadVertex4Y, QuadVertex4Z));
cur_verts[std::make_tuple(QuadVertex4X, QuadVertex4Y, QuadVertex4Z)] = i4;
}
imp->inds.push_back(i4);
imp->inds.push_back(i3);
imp->inds.push_back(i2);
imp->inds.push_back(i2);
imp->inds.push_back(i1);
imp->inds.push_back(i4);
}
}
}
float t;
for(auto &v : imp->verts) {
t = v.y;
v.y = v.z;
v.z = t;
}
for(auto &vert : imp->verts) {
if(vert.x < imp->min.x) {
imp->min.x = vert.x;
}
if(vert.y < imp->min.y && vert.y > -15000) {
imp->min.y = vert.y;
}
if(vert.z < imp->min.z) {
imp->min.z = vert.z;
}
if(vert.x > imp->max.x) {
imp->max.x = vert.x;
}
if(vert.y > imp->max.y) {
imp->max.y = vert.y;
}
if(vert.z > imp->max.z) {
imp->max.z = vert.z;
}
}
for(auto &v : imp->nc_verts) {
t = v.y;
v.y = v.z;
v.z = t;
}
for(auto &vert : imp->nc_verts) {
if(vert.x < imp->nc_min.x) {
imp->nc_min.x = vert.x;
}
if(vert.y < imp->nc_min.y) {
imp->nc_min.y = vert.y;
}
if(vert.z < imp->nc_min.z) {
imp->nc_min.z = vert.z;
}
if(vert.x > imp->nc_max.x) {
imp->nc_max.x = vert.x;
}
if(vert.y > imp->nc_max.y) {
imp->nc_max.y = vert.y;
}
if(vert.z > imp->nc_max.z) {
imp->nc_max.z = vert.z;
}
}
return true;
}
CMatrix* CIkRoutine::getJacobian(CikEl* ikElement,C4X4Matrix& tooltipTransf,std::vector<int>* rowJointIDs,std::vector<int>* rowJointStages)
{ // rowJointIDs is NULL by default. If not null, it will contain the ids of the joints
// corresponding to the rows of the jacobian.
// Return value NULL means that is ikElement is either inactive, either invalid
// tooltipTransf is the cumulative transformation matrix of the tooltip,
// computed relative to the base!
// The temporary joint parameters need to be initialized before calling this function!
// We check if the ikElement's base is in the chain and that tooltip is valid!
CDummy* tooltip=ct::objCont->getDummy(ikElement->getTooltip());
if (tooltip==NULL)
{ // Should normally never happen!
ikElement->setActive(false);
return(NULL);
}
C3DObject* base=ct::objCont->getObject(ikElement->getBase());
if ( (base!=NULL)&&(!tooltip->isObjectParentedWith(base)) )
{ // This case can happen (when the base's parenting was changed for instance)
ikElement->setBase(-1);
ikElement->setActive(false);
return(NULL);
}
// We check the number of degrees of freedom and prepare the rowJointIDs vector:
C3DObject* iterat=tooltip;
int doF=0;
while (iterat!=base)
{
iterat=iterat->getParent();
if ( (iterat!=NULL)&&(iterat!=base) )
{
if (iterat->getObjectType()==sim_object_joint_type)
{
if ( (((CJoint*)iterat)->getJointMode()==sim_jointmode_ik)||(((CJoint*)iterat)->getJointMode()==sim_jointmode_ikdependent) )
{
int d=((CJoint*)iterat)->getDoFs();
for (int i=d-1;i>=0;i--)
{
if (rowJointIDs!=NULL)
{
rowJointIDs->push_back(iterat->getID());
rowJointStages->push_back(i);
}
}
doF+=d;
}
}
}
}
CMatrix* J=new CMatrix(6,(unsigned char)doF);
std::vector<C4X4FullMatrix*> jMatrices;
for (int i=0;i<(doF+1);i++)
{
C4X4FullMatrix* matr=new C4X4FullMatrix();
if (i==0)
(*matr).setIdentity();
else
(*matr).clear();
jMatrices.push_back(matr);
}
// Now we go from tip to base:
iterat=tooltip;
C4X4FullMatrix buff;
buff.setIdentity();
int positionCounter=0;
C4X4FullMatrix d0;
C4X4FullMatrix dp;
C4X4FullMatrix paramPart;
CJoint* lastJoint=NULL;
int indexCnt=-1;
int indexCntLast=-1;
while (iterat!=base)
{
C3DObject* nextIterat=iterat->getParent();
C7Vector local;
if (iterat->getObjectType()==sim_object_joint_type)
{
if ( (((CJoint*)iterat)->getJointMode()!=sim_jointmode_ik)&&(((CJoint*)iterat)->getJointMode()!=sim_jointmode_ikdependent) )
local=iterat->getLocalTransformation(true);
else
{
CJoint* it=(CJoint*)iterat;
if (it->getJointType()==sim_joint_spherical_subtype)
{
if (indexCnt==-1)
indexCnt=it->getDoFs()-1;
it->getLocalTransformationExPart1(local,indexCnt--,true);
if (indexCnt!=-1)
nextIterat=iterat; // We keep the same object! (but indexCnt has decreased)
}
else
local=iterat->getLocalTransformationPart1(true);
}
}
else
local=iterat->getLocalTransformation(true);
buff=C4X4FullMatrix(local.getMatrix())*buff;
iterat=nextIterat;
bool activeJoint=false;
if (iterat!=NULL) // Following lines recently changed!
{
if (iterat->getObjectType()==sim_object_joint_type)
activeJoint=( (((CJoint*)iterat)->getJointMode()==sim_jointmode_ik)||(((CJoint*)iterat)->getJointMode()==sim_jointmode_ikdependent) );
}
if ( (iterat==base)||activeJoint )
{ // If base is NULL then the second part is not evaluated (iterat->getObjectType())
if (positionCounter==0)
{ // Here we have the first part (from tooltip to first joint)
d0=buff;
dp.clear();
multiply(d0,dp,0,jMatrices);
}
else
{ // Here we have a joint:
if (lastJoint->getJointType()==sim_joint_revolute_subtype)
{
buildDeltaZRotation(d0,dp,lastJoint->getScrewPitch());
multiply(d0,dp,positionCounter,jMatrices);
paramPart.buildZRotation(lastJoint->getPosition(true));
}
else if (lastJoint->getJointType()==sim_joint_prismatic_subtype)
{
buildDeltaZTranslation(d0,dp);
multiply(d0,dp,positionCounter,jMatrices);
paramPart.buildTranslation(0.0f,0.0f,lastJoint->getPosition(true));
}
else
{ // Spherical joint part!
buildDeltaZRotation(d0,dp,0.0f);
multiply(d0,dp,positionCounter,jMatrices);
if (indexCntLast==-1)
indexCntLast=lastJoint->getDoFs()-1;
paramPart.buildZRotation(lastJoint->getTempParameterEx(indexCntLast--));
}
d0=buff*paramPart;
dp.clear();
multiply(d0,dp,0,jMatrices);
}
buff.setIdentity();
lastJoint=(CJoint*)iterat;
positionCounter++;
}
}
int alternativeBaseForConstraints=ikElement->getAlternativeBaseForConstraints();
if (alternativeBaseForConstraints!=-1)
{
CDummy* alb=ct::objCont->getDummy(alternativeBaseForConstraints);
if (alb!=NULL)
{ // We want everything relative to the alternativeBaseForConstraints dummy orientation!
C7Vector alternativeBase(alb->getCumulativeTransformationPart1(true));
C7Vector currentBase;
currentBase.setIdentity();
if (base!=NULL)
currentBase=base->getCumulativeTransformation(true); // could be a joint, we want also the joint intrinsic transformation part!
C4X4FullMatrix correction((alternativeBase.getInverse()*currentBase).getMatrix());
dp.clear();
multiply(correction,dp,0,jMatrices);
}
}
// The x-, y- and z-component:
for (int i=0;i<doF;i++)
{
(*J)(0,i)=(*jMatrices[1+i])(0,3);
(*J)(1,i)=(*jMatrices[1+i])(1,3);
(*J)(2,i)=(*jMatrices[1+i])(2,3);
}
// We divide all delta components (to avoid distorsions)...
for (int i=0;i<doF;i++)
(*jMatrices[1+i])/=IK_DIVISION_FACTOR;
// ...and add the cumulative transform to the delta-components:
for (int i=0;i<doF;i++)
(*jMatrices[1+i])+=(*jMatrices[0]);
// We also copy the cumulative transform to 'tooltipTransf':
tooltipTransf=(*jMatrices[0]);
// Now we extract the delta Euler components:
C4X4FullMatrix mainInverse(*jMatrices[0]);
mainInverse.invert();
C4X4FullMatrix tmp;
// Alpha-, Beta- and Gamma-components:
for (int i=0;i<doF;i++)
{
tmp=mainInverse*(*jMatrices[1+i]);
C3Vector euler(tmp.getEulerAngles());
(*J)(3,i)=euler(0);
(*J)(4,i)=euler(1);
(*J)(5,i)=euler(2);
}
// We free the memory allocated for each joint variable:
for (int i=0;i<int(jMatrices.size());i++)
delete jMatrices[i];
return(J);
}
