void egami::ImageMono::resize(const ivec2& _size, const ivec2& _startPos) {
if (_size == m_size) {
// same size == > nothing to do ...
return;
}
// grow size :
egami::ImageMono tmpImage(*this);
m_size=_size;
uint8_t tmpBg(0);
m_data.resize(m_size.x()*m_size.y(), tmpBg);
for (int32_t jjj=0; jjj<m_size.y(); jjj++) {
for (int32_t iii=0; iii<m_size.y(); iii++) {
ivec2 tmppos(iii,jjj);
set(tmppos, tmpImage.get(tmppos));
}
}
}
/*
* \brief Run Calibration
*
*
*/
bool MapDemonCtrl::runCalibration()
{
std::ostringstream errorMsg;
std::vector<std::string> JointNames = params_->getJointNames();
std::vector<double> MaxVel = params_->getMaxVel();
/// Shut robot output in case it was already enabled...
usleep(200000);
if( !sd_->FlushInBuffer() )
return false; /// ...and flush serial port input buffer
usleep(200000);
/// Run encoder calibration
sd_->PutString("L\n");
size_t found = std::string::npos;
char retry = 0; //
/// get messages till 'L' is received (this is necessary because if the message output of the robot was enabled, between the flush buffer and this we still could have received a few characters from that, before the 'L'.
while( found == std::string::npos )
{
std::string str;
sd_->GetString(str); // read another message
found = str.find_first_of("L", 0); // find L
retry++;
std::cout << "calib cb: " << str << std::endl;
if(retry == 4) // experimental value, normally one or two messages are received btw flushinbuffer and getstring.
return false; /// if tryout
}
//char *cstr = new char (str.size()+1);
//strcpy(cstr, str.c_str());
//if(strchr(cstr, 'L') == NULL)
// return false;
/// Reposition to Home in case of existant Offsets
std::vector<double> tmppos( 2, 0.0f );
movePos(tmppos);
return true;
}
VertexData TransformUnit::ReadVertex(VertexReader& vreader)
{
VertexData vertex;
float pos[3];
// VertexDecoder normally scales z, but we want it unscaled.
vreader.ReadPosThroughZ16(pos);
if (!gstate.isModeClear() && gstate.isTextureMapEnabled() && vreader.hasUV()) {
float uv[2];
vreader.ReadUV(uv);
vertex.texturecoords = Vec2<float>(uv[0], uv[1]);
}
if (vreader.hasNormal()) {
float normal[3];
vreader.ReadNrm(normal);
vertex.normal = Vec3<float>(normal[0], normal[1], normal[2]);
if (gstate.areNormalsReversed())
vertex.normal = -vertex.normal;
}
if (vertTypeIsSkinningEnabled(gstate.vertType) && !gstate.isModeThrough()) {
float W[8] = { 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f };
vreader.ReadWeights(W);
Vec3<float> tmppos(0.f, 0.f, 0.f);
Vec3<float> tmpnrm(0.f, 0.f, 0.f);
for (int i = 0; i < vertTypeGetNumBoneWeights(gstate.vertType); ++i) {
Mat3x3<float> bone(&gstate.boneMatrix[12*i]);
tmppos += (bone * ModelCoords(pos[0], pos[1], pos[2]) + Vec3<float>(gstate.boneMatrix[12*i+9], gstate.boneMatrix[12*i+10], gstate.boneMatrix[12*i+11])) * W[i];
if (vreader.hasNormal())
tmpnrm += (bone * vertex.normal) * W[i];
}
pos[0] = tmppos.x;
pos[1] = tmppos.y;
pos[2] = tmppos.z;
if (vreader.hasNormal())
vertex.normal = tmpnrm;
}
if (vreader.hasColor0()) {
float col[4];
vreader.ReadColor0(col);
vertex.color0 = Vec4<int>(col[0]*255, col[1]*255, col[2]*255, col[3]*255);
} else {
vertex.color0 = Vec4<int>(gstate.getMaterialAmbientR(), gstate.getMaterialAmbientG(), gstate.getMaterialAmbientB(), gstate.getMaterialAmbientA());
}
if (vreader.hasColor1()) {
float col[3];
vreader.ReadColor1(col);
vertex.color1 = Vec3<int>(col[0]*255, col[1]*255, col[2]*255);
} else {
vertex.color1 = Vec3<int>(0, 0, 0);
}
if (!gstate.isModeThrough()) {
vertex.modelpos = ModelCoords(pos[0], pos[1], pos[2]);
vertex.worldpos = WorldCoords(TransformUnit::ModelToWorld(vertex.modelpos));
ModelCoords viewpos = TransformUnit::WorldToView(vertex.worldpos);
vertex.clippos = ClipCoords(TransformUnit::ViewToClip(viewpos));
if (gstate.isFogEnabled()) {
float fog_end = getFloat24(gstate.fog1);
float fog_slope = getFloat24(gstate.fog2);
// Same fixup as in ShaderManagerGLES.cpp
if (my_isnanorinf(fog_end)) {
// Not really sure what a sensible value might be, but let's try 64k.
fog_end = std::signbit(fog_end) ? -65535.0f : 65535.0f;
}
if (my_isnanorinf(fog_slope)) {
fog_slope = std::signbit(fog_slope) ? -65535.0f : 65535.0f;
}
vertex.fogdepth = (viewpos.z + fog_end) * fog_slope;
} else {
vertex.fogdepth = 1.0f;
}
vertex.screenpos = ClipToScreenInternal(vertex.clippos, &outside_range_flag);
if (vreader.hasNormal()) {
vertex.worldnormal = TransformUnit::ModelToWorldNormal(vertex.normal);
// TODO: Isn't there a flag that controls whether to normalize the normal?
vertex.worldnormal /= vertex.worldnormal.Length();
} else {
vertex.worldnormal = Vec3<float>(0.0f, 0.0f, 1.0f);
}
Lighting::Process(vertex, vreader.hasColor0());
} else {
vertex.screenpos.x = (int)(pos[0] * 16) + gstate.getOffsetX16();
vertex.screenpos.y = (int)(pos[1] * 16) + gstate.getOffsetY16();
vertex.screenpos.z = pos[2];
vertex.clippos.w = 1.f;
vertex.fogdepth = 1.f;
}
return vertex;
}
static VertexData ReadVertex(VertexReader& vreader)
{
VertexData vertex;
float pos[3];
// VertexDecoder normally scales z, but we want it unscaled.
vreader.ReadPosZ16(pos);
if (!gstate.isModeClear() && gstate.isTextureMapEnabled() && vreader.hasUV()) {
float uv[2];
vreader.ReadUV(uv);
vertex.texturecoords = Vec2<float>(uv[0], uv[1]);
}
if (vreader.hasNormal()) {
float normal[3];
vreader.ReadNrm(normal);
vertex.normal = Vec3<float>(normal[0], normal[1], normal[2]);
if (gstate.areNormalsReversed())
vertex.normal = -vertex.normal;
}
if (gstate.isSkinningEnabled() && !gstate.isModeThrough()) {
float W[8] = { 1.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f };
vreader.ReadWeights(W);
Vec3<float> tmppos(0.f, 0.f, 0.f);
Vec3<float> tmpnrm(0.f, 0.f, 0.f);
for (int i = 0; i < gstate.getNumBoneWeights(); ++i) {
Mat3x3<float> bone(&gstate.boneMatrix[12*i]);
tmppos += W[i] * (bone * ModelCoords(pos[0], pos[1], pos[2]) + Vec3<float>(gstate.boneMatrix[12*i+9], gstate.boneMatrix[12*i+10], gstate.boneMatrix[12*i+11]));
if (vreader.hasNormal())
tmpnrm += W[i] * (bone * vertex.normal);
}
pos[0] = tmppos.x;
pos[1] = tmppos.y;
pos[2] = tmppos.z;
if (vreader.hasNormal())
vertex.normal = tmpnrm;
}
if (vreader.hasColor0()) {
float col[4];
vreader.ReadColor0(col);
vertex.color0 = Vec4<int>(col[0]*255, col[1]*255, col[2]*255, col[3]*255);
} else {
vertex.color0 = Vec4<int>(gstate.getMaterialAmbientR(), gstate.getMaterialAmbientG(), gstate.getMaterialAmbientB(), gstate.getMaterialAmbientA());
}
if (vreader.hasColor1()) {
float col[3];
vreader.ReadColor1(col);
vertex.color1 = Vec3<int>(col[0]*255, col[1]*255, col[2]*255);
} else {
vertex.color1 = Vec3<int>(0, 0, 0);
}
if (!gstate.isModeThrough()) {
vertex.modelpos = ModelCoords(pos[0], pos[1], pos[2]);
vertex.worldpos = WorldCoords(TransformUnit::ModelToWorld(vertex.modelpos));
vertex.clippos = ClipCoords(TransformUnit::ViewToClip(TransformUnit::WorldToView(vertex.worldpos)));
vertex.screenpos = ClipToScreenInternal(vertex.clippos);
if (vreader.hasNormal()) {
vertex.worldnormal = TransformUnit::ModelToWorld(vertex.normal) - Vec3<float>(gstate.worldMatrix[9], gstate.worldMatrix[10], gstate.worldMatrix[11]);
vertex.worldnormal /= vertex.worldnormal.Length(); // TODO: Shouldn't be necessary..
}
Lighting::Process(vertex);
} else {
vertex.screenpos.x = (u32)pos[0] * 16 + gstate.getOffsetX16();
vertex.screenpos.y = (u32)pos[1] * 16 + gstate.getOffsetY16();
vertex.screenpos.z = pos[2];
vertex.clippos.w = 1.f;
}
return vertex;
}
void FBMReference::computeHE() {
const unsigned int n = 3 * particleCount;
const unsigned int m = projectionMatrix.dim2();
TNT::Array2D<double> HE( n, m, 0.0 );
// Compute eps.
const double eps = params.sDelta;
// Make a temp copy of positions.
vector<Vec3> tmppos( initialPositions );
#ifdef FIRST_ORDER
vector<Vec3> forces_start = context.getState( State::Forces ).getForces();
#endif
// Loop over i.
for( unsigned int k = 0; k < m; k++ ) {
// Perturb positions.
int pos = 0;
// forward perturbations
for( unsigned int i = 0; i < particleCount; i++ ) {
for( unsigned int j = 0; j < 3; j++ ) {
tmppos[i][j] = initialPositions[i][j] + eps * projectionMatrix[3 * i + j][k] / sqrt( mParticleMass[i] );
pos++;
}
}
context.setPositions( tmppos );
// Calculate F(xi).
vector<Vec3> forces_forward = context.getState( State::Forces ).getForces();
#ifndef FIRST_ORDER
// backward perturbations
for( unsigned int i = 0; i < particleCount; i++ ) {
for( unsigned int j = 0; j < 3; j++ ) {
tmppos[i][j] = initialPositions[i][j] - eps * projectionMatrix[3 * i + j][k] / sqrt( mParticleMass[i] );
}
}
context.setPositions( tmppos );
// Calculate forces
vector<Vec3> forces_backward = context.getState( State::Forces ).getForces();
#endif
for( int i = 0; i < n; i++ ) {
#ifdef FIRST_ORDER
const double scaleFactor = sqrt( mParticleMass[i / 3] ) * 1.0 * eps;
HE[i][k] = ( forces_forward[i / 3][i % 3] - forces_start[i / 3][i % 3] ) / scaleFactor;
#else
const double scaleFactor = sqrt( mParticleMass[i / 3] ) * 2.0 * eps;
HE[i][k] = ( forces_forward[i / 3][i % 3] - forces_backward[i / 3][i % 3] ) / scaleFactor;
#endif
}
// restore positions
for( unsigned int i = 0; i < particleCount; i++ ) {
for( unsigned int j = 0; j < 3; j++ ) {
tmppos[i][j] = initialPositions[i][j];
}
}
}
// *****************************************************************
// restore unperturbed positions
context.setPositions( initialPositions );
productMatrix = HE.copy();
}
