Vector2 * Vector2::clamp(float min, float max)
{
float len2 = this->len2();
if (len2 == 0) return this;
float max2 = max * max;
if (len2 > max2) return scl((float)sqrt(max2 / len2));
float min2 = min * min;
if (len2 < min2) return scl((float)sqrt(min2 / len2));
return this;
}
void
PatternKnotHolderEntityScale::knot_set(Geom::Point const &p, Geom::Point const &/*origin*/, guint state)
{
SPPattern *pat = SP_PATTERN(SP_STYLE_FILL_SERVER(SP_OBJECT(item)->style));
// FIXME: this snapping should be done together with knowing whether control was pressed. If GDK_CONTROL_MASK, then constrained snapping should be used.
Geom::Point p_snapped = snap_knot_position(p, state);
// get angle from current transform
gdouble theta = sp_pattern_extract_theta(pat);
// Get the new scale from the position of the knotholder
Geom::Point d = p_snapped - sp_pattern_extract_trans(pat);
gdouble pat_x = pattern_width(pat);
gdouble pat_y = pattern_height(pat);
Geom::Scale scl(1);
if ( state & GDK_CONTROL_MASK ) {
// if ctrl is pressed: use 1:1 scaling
gdouble pat_h = hypot(pat_x, pat_y);
scl = Geom::Scale(d.length() / pat_h);
} else {
d *= Geom::Rotate(-theta);
scl = Geom::Scale(d[Geom::X] / pat_x, d[Geom::Y] / pat_y);
}
Geom::Affine rot = (Geom::Affine)scl * Geom::Rotate(theta);
Geom::Point const t = sp_pattern_extract_trans(pat);
rot[4] = t[Geom::X];
rot[5] = t[Geom::Y];
item->adjust_pattern(rot, true);
item->requestDisplayUpdate(SP_OBJECT_MODIFIED_FLAG);
}
void GoSUM::CModelVariables::setNormalization()
{
int i,j,N=size(),eN=expandedSize(),exsize;
trn.setZero(eN);
scl.setOnes(eN);
for ( i=0,j=0; i<N; i++ ) {
const CModelVariable &aMV=mvs[i];
exsize=aMV.expandedSize();
if ( exsize==1 ) {
// trn(j)=aMV.minValue();
// scl(j)=aMV.maxValue()-aMV.minValue();
trn(j)=aMV.expectedValue();
scl(j)=sqrt(aMV.variance());
if ( fabs(scl(j))<TINY ) { trn(j)+=1.; scl(j)=1.; }
}
j+=exsize;
}
}
void I2C::configurePins(bool withSmb){
Pin sda(B, pinbase + 1);
Pin scl(B, pinbase);
*sda.writer() = 1;
*scl.writer() = 1;
if(pinbase == 8) {
theAfioManager.b->i2c1 = 1;
}
sda.FN(50, true); //todo:3 set edge rate by frequency.
scl.FN(50, true);
if(withSmb && pinbase == 10) { //only i2c2 supports smb interrupt
Pin(B, 12).DI('U');
}
} /* configurePins */
void PhysicEngine::addHeightField(float* heights,
int x, int y, float yoffset,
float triSize, float sqrtVerts)
{
const std::string name = "HeightField_"
+ boost::lexical_cast<std::string>(x) + "_"
+ boost::lexical_cast<std::string>(y);
// find the minimum and maximum heights (needed for bullet)
float minh;
float maxh;
for (int i=0; i<sqrtVerts*sqrtVerts; ++i)
{
float h = heights[i];
if (i==0)
{
minh = h;
maxh = h;
}
if (h>maxh) maxh = h;
if (h<minh) minh = h;
}
btHeightfieldTerrainShape* hfShape = new btHeightfieldTerrainShape(
sqrtVerts, sqrtVerts, heights, 1,
minh, maxh, 2,
PHY_FLOAT,true);
hfShape->setUseDiamondSubdivision(true);
btVector3 scl(triSize, triSize, 1);
hfShape->setLocalScaling(scl);
CMotionState* newMotionState = new CMotionState(this,name);
btRigidBody::btRigidBodyConstructionInfo CI = btRigidBody::btRigidBodyConstructionInfo(0,newMotionState,hfShape);
RigidBody* body = new RigidBody(CI,name);
body->collide = true;
body->getWorldTransform().setOrigin(btVector3( (x+0.5)*triSize*(sqrtVerts-1), (y+0.5)*triSize*(sqrtVerts-1), (maxh+minh)/2.f));
HeightField hf;
hf.mBody = body;
hf.mShape = hfShape;
mHeightFieldMap [name] = hf;
dynamicsWorld->addRigidBody(body,COL_WORLD,COL_WORLD|COL_ACTOR_INTERNAL|COL_ACTOR_EXTERNAL);
}
StructureAdapterPtr
createStructureAdapter(const ObjCryst::Crystal& cryst)
{
const double radtodeg = 180 / M_PI;
CrystalStructureAdapterPtr adpt(new CrystalStructureAdapter);
adpt->setLatPar(
cryst.GetLatticePar(0),
cryst.GetLatticePar(1),
cryst.GetLatticePar(2),
radtodeg * cryst.GetLatticePar(3),
radtodeg * cryst.GetLatticePar(4),
radtodeg * cryst.GetLatticePar(5));
// find out number of scatterers in the asymmetric unit
const ObjCryst::ScatteringComponentList& scl =
cryst.GetScatteringComponentList();
size_t nbComponent = scl.GetNbComponent();
adpt->reserve(nbComponent);
Atom ai;
const Lattice& L = adpt->getLattice();
for (size_t i = 0; i < nbComponent; ++i)
{
const ObjCryst::ScatteringComponent& sc = scl(i);
const ObjCryst::ScatteringPower* sp = sc.mpScattPow;
// Skip over this if it is a dummy atom. A dummy atom has no
// mpScattPow, and therefore no type. It's just in a structure as a
// reference position.
if (sp == NULL) continue;
ai.occupancy = sc.mOccupancy;
ai.anisotropy = !(sp->IsIsotropic());
ai.atomtype = sp->GetSymbol();
R3::Vector xyz(sc.mX, sc.mY, sc.mZ);
ai.xyz_cartn = L.cartesian(xyz);
// Store Uij
R3::Matrix uijl = getUij(sp);
ai.uij_cartn = ai.anisotropy ? L.cartesianMatrix(uijl) : uijl;
adpt->append(ai);
}
const ObjCryst::SpaceGroup& spacegroup = cryst.GetSpaceGroup();
CrystalStructureAdapter::SymOpVector symops =
fetchSymmetryOperations(spacegroup);
CrystalStructureAdapter::SymOpVector::const_iterator op;
for (op = symops.begin(); op != symops.end(); ++op) adpt->addSymOp(*op);
adpt->updateSymmetryPositions();
return adpt;
}
int main()
{
DiscoveryBoard board;
RTOS::Time t(board.Tim7);
LedAngleDisplay display (board);
Platform::Gpio scl(board.GpioB[6]);
Platform::Gpio sda(board.GpioB[7]);
Platform::I2C bus(scl, sda, Platform::I2C::I2C_1);
uint8_t target_reg = 0x01;
uint8_t compass_address = 0b01100000;
while(1)
{
bus.write(compass_address, &target_reg, 1);
uint8_t data;
t.msleep(10);
bus.read(compass_address, &data, 1);
display.setValue(data);
t.msleep(100);
}
}
void CloudsVisualSystem3DModelLoader::updateModelTransform()
{
modelTransform.resetTransform();
ofVec3f scl(1,1,1);
if(currentSingleCam != &pathCamera)
{
if(!bDoNotScaleModel)
{
scl = (bAutoScale? modelScl : ofVec3f(1,1,1)) * modelScale;
modelTransform.setScale( scl );
}
if(bCenterModel)
{
modelTransform.setPosition( -boundCenter );
modelTransform.move(0, (maxBound.y-minBound.y)*.5 * scl.y, 0);
}
}
}
void initalize(const vec1<int>& order)
{
D_ASSERT(!fixed_base);
fixed_base = true;
unpacked_stabChain_depth.resize(order.size());
GAP_callFunction(FunObj_ChangeStabChain, stabChain, GAP_make(order));
debug_out(1, "SCC", "Setting up cache");
debug_out(3, "SCC", "Order " << order);
int order_pos = 1;
GAPStabChainWrapper stabChainCpy(stabChain);
do
{
StabChainLevel scl(stabChainCpy);
while(order[order_pos] != scl.base_value)
{
debug_out(3, "SCC", "Skipping depth " << order_pos);
order_pos++;
}
debug_out(3, "SCC", "Setting depth "<<order_pos<<" base point "<<scl.base_value);
levels.push_back(scl);
unpacked_stabChain_depth[order_pos] = levels.size();
stabChainCpy = stabChainCpy.getNextLevel();
}
while(stabChainCpy.hasNextLevel());
#ifndef NO_DEBUG
for(int i : range1(unpacked_stabChain_depth.size()))
{
if(unpacked_stabChain_depth[i] != 0)
{
D_ASSERT(levels[unpacked_stabChain_depth[i]].base_value == order[i]);
}
}
#endif
}
void CGpApp::DrawPlayerLife()
{
if (0 < m_Player.nAlive)
{
FLOAT scale = 0.5f;
LCXVECTOR2 scl(scale, scale);
INT marginWidth = (INT)(PLAYER_WIDTH * (1.0f - scale) * 0.5f);
INT marginHeight = (INT)(PLAYER_HEIGHT * (1.0f - scale) * 0.5f);
LCXVECTOR2 position;
position.x = (FLOAT)(-marginWidth);
position.y = (FLOAT)(m_nScnH - (PLAYER_HEIGHT - marginHeight));
for (INT count = 0; count < m_Player.nAlive; ++count)
{
m_pSpt->DrawEx(m_pTex[m_Player.m_nTex], &m_Cells[3], &position, &scl, NULL, 0, NULL);
position.x = position.x + PLAYER_WIDTH - (marginWidth * 2);
}
}
}
function<void()>
////////////////////////// TYPE APP NAME ON LINE BELOW //////////////////////////
delaun_distr
(RenderWindow&window, ui &UI) {
return [&window, &UI]() {
#ifndef COMMON_INITS
//void smoothmouse(RenderWindow&window, ui &UI){
// tidy code organization, here we predeclare methods just like a header would, it's redundant but necessary
function<void()> draw, loop, init, update;
function<void(Vec2 pos)> mousemoved;
function<void(sf::Mouse::Button button)> mouseclick, mouserelease;
function<void(Event&e)> treatotherevent;
function<void(Keyboard::Key k)> treatkeyevent;
// we don't have a class/interface/struct with data, everything is local to this function, like a classic stack that all programs are anyway.
Texture cursor_tx;
Sprite cursor;
configfile cfg;
cfg.init("bedlab.cfg");
Color background = cfg.getvar<Color>("background");
if (!cursor_tx.loadFromFile(cfg.getstr("cursor")))
cout << "did not load cursor" << endl;
cursor.setTexture(cursor_tx);
cursor.setOrigin(3, 3);
window.setMouseCursorVisible(false);
Vec2 mpos;
bool leftclicked = false, rightclicked = false;
// view and zoom
View view, ui_view;
ui_view = view = window.getDefaultView();
float zoomlevel = 1;
Vec2 mpos_abs;
#endif // COMMON_INITS
// ************************ CODE BEGIN ************************
auto random_graph = [](int pick_count, float distrib_radius)
{
vector<pair<int,Vec2>> points;
vector<pair<int, int>> graph;
//float distrib_radius;// = cfg.getvar<float>("distrib_radius");
paused_distr paus_distr(distrib_radius, 5, 321);
//int winheight = cfg.getvar<Vec2i>("windowsize").y;
//scaler scl(winheight*0.9f, { 0,0 });
DelaunayTriangulation dela(1, 1);
//int pick_count = cfg.getvar<int>("pick_count");
for (int i = 0; i < pick_count; ++i)
{
Vec2 cand;
int retcode = -1;
do
{
retcode = paus_distr.pick5(cand);
} while (retcode == 2);
if (retcode != -1 && retcode != 0)
{
dela.AddPoint(Point(cand.x, cand.y));
//glob_pts.push_back(mkcircle(scl(cand), Color::White, .5f));
//glob_pts.push_back(mkcircle(scl(cand), Color::White, 3.f));
points.push_back({ points.size(),cand });
}
}
//msg(glob_pts.size());
for (auto&triangle : dela.triangles)
{
int
a = triangle.get()->v[0],
b = triangle.get()->v[1],
c = triangle.get()->v[2];
Point
A = dela.points[a],
B = dela.points[b],
C = dela.points[c];
if (
A.x == 0 || A.x == 1
|| A.y == 0 || A.y == 1
|| B.x == 0 || B.x == 1
|| B.y == 0 || B.y == 1
|| C.x == 0 || C.x == 1
|| C.y == 0 || C.y == 1
)
{
continue;
}
graph.push_back({a,b});
graph.push_back({b,c});
graph.push_back({a,c});
// those are the ones!
//segment(scl(Vec2(A.x, A.y)), scl(Vec2(B.x, B.y)));
//segment(scl(Vec2(C.x, C.y)), scl(Vec2(B.x, B.y)));
//segment(scl(Vec2(A.x, A.y)), scl(Vec2(C.x, C.y)));
//segment(Vec2(A.x, A.y), Vec2(B.x, B.y));
//segment(Vec2(C.x, C.y), Vec2(B.x, B.y));
//segment(Vec2(A.x, A.y), Vec2(C.x, C.y));
//segment(pts[a], pts[b]);
//segment(pts[a], pts[c]);
//segment(pts[c], pts[b]);
}
return make_pair(points, graph);
};
float distrib_radius = cfg.getvar<float>("distrib_radius");
paused_distr paus_distr(distrib_radius, 5, 321);
int winheight = cfg.getvar<Vec2i>("windowsize").y;
scaler scl(winheight*0.9f, { 0,0 });
DelaunayTriangulation dela(1, 1);
int pick_count = cfg.getvar<int>("pick_count");
for(int i = 0; i < pick_count; ++i)
{
Vec2 cand;
int retcode = -1;
do
{
retcode = paus_distr.pick5(cand);
} while (retcode == 2);
if (retcode != -1 && retcode != 0)
{
dela.AddPoint(Point(cand.x, cand.y));
//glob_pts.push_back(mkcircle(scl(cand), Color::White, .5f));
glob_pts.push_back(mkcircle(scl(cand), Color::White, 3.f));
}
}
msg(glob_pts.size());
for (auto&triangle : dela.triangles)
{
int
a = triangle.get()->v[0],
b = triangle.get()->v[1],
c = triangle.get()->v[2];
Point
A = dela.points[a],
B = dela.points[b],
C = dela.points[c];
if (
A.x == 0 || A.x == 1
|| A.y == 0 || A.y == 1
|| B.x == 0 || B.x == 1
|| B.y == 0 || B.y == 1
|| C.x == 0 || C.x == 1
|| C.y == 0 || C.y == 1
)
{
continue;
}
segment(scl(Vec2(A.x, A.y)), scl(Vec2(B.x, B.y)));
segment(scl(Vec2(C.x, C.y)), scl(Vec2(B.x, B.y)));
segment(scl(Vec2(A.x, A.y)), scl(Vec2(C.x, C.y)));
//segment(Vec2(A.x, A.y), Vec2(B.x, B.y));
//segment(Vec2(C.x, C.y), Vec2(B.x, B.y));
//segment(Vec2(A.x, A.y), Vec2(C.x, C.y));
//segment(pts[a], pts[b]);
//segment(pts[a], pts[c]);
//segment(pts[c], pts[b]);
}
barstack bst(FONT, FONTSIZE);
slider_finder sl;
size_t edit_mode = 1;
// ************************ INITS END ************************
//then we actually define the functions, note how all function captures the local context by reference
#ifndef LOOP_LAMBDAS
draw = [&]() {
window.setView(view);
// object draw, AFTER normal view
//Vec2 cand;
//int retcode = -1;
//do
//{
// retcode = paus_distr.pick4(cand);
//} while (retcode == 2);
//if (retcode != -1 && retcode != 0)
//{
// //glob_pts.push_back(mkcircle(scl(cand), Color::White, .5f));
// glob_pts.push_back(mkcircle(scl(cand), Color::White, .5f));
//}
//////////////// obj draw START ////////////////
for (auto&a : glob_pts)window.draw(a);
for (auto&a : glob_rects)window.draw(a);
for (auto&a : glob_vert)window.draw(a);
for (auto&a : glob_texts)window.draw(a);
//window.draw(nearest_pt);
//////////////// obj draw END ////////////////
// UI draw, AFTER ui view and BEFORE other draw
window.setView(ui_view);
bst.draw(window);
// nothing after here please
UI.draw(window);
window.draw(cursor);
};
update = [&]() {
};
treatkeyevent = [&](Keyboard::Key k) {
switch (k)
{
case Keyboard::BackSpace:
glob_pts.clear();
glob_texts.clear();
glob_rects.clear();
glob_vert.clear();
break;
case Keyboard::Space:
break;
case Keyboard::Q: break;
}
};
mousemoved = [&](Vec2 pos) {
cursor.setPosition(pos);
if (leftclicked) sl.mouse_callback(pos);
};
mouseclick = [&](sf::Mouse::Button button) {
if (button == Mouse::Button::Left) leftclicked = true;
if (button == Mouse::Button::Right) rightclicked = true;
};
mouserelease = [&](sf::Mouse::Button button) {
if (button == Mouse::Button::Left) leftclicked = false;
if (button == Mouse::Button::Right) rightclicked = false;
sl.release();
};
loop = [&]() {
while (window.isOpen())
{
sf::Event event;
while (window.pollEvent(event))
{
switch (event.type)
{
case sf::Event::KeyPressed:
if (event.key.code == sf::Keyboard::Escape)
window.close();
treatkeyevent(event.key.code);
break;
case sf::Event::Closed:
window.close();
break;
case sf::Event::MouseButtonPressed:
mouseclick(event.mouseButton.button);
break;
case sf::Event::MouseButtonReleased:
mouserelease(event.mouseButton.button);
break;
case sf::Event::MouseMoved:
mpos = Vec2(event.mouseMove.x, event.mouseMove.y);
mousemoved(mpos);
break;
default:
treatotherevent(event);
break;
}
}
window.clear(background);
update();
draw();
window.display();
}
};
treatotherevent = [&](Event&e) {
if (e.type == Event::MouseWheelMoved && e.mouseWheel.delta)
{
mpos_abs = window.mapPixelToCoords(Vec2i(mpos), view);
//view = window.getView();
if (e.mouseWheel.delta < 0)
{
zoomlevel *= 2.f;
view.setSize(view.getSize()*2.f);
view.setCenter(interp(mpos_abs, view.getCenter(), 2.f));
//view.setCenter(interp(mpos_abs, view.getCenter(), 2.f));
}
if (e.mouseWheel.delta > 0)
{
zoomlevel *= 0.5;
view.setSize(view.getSize()*.5f);
view.setCenter(.5f*(view.getCenter() + mpos_abs));
//view.setCenter(.5f*(view.getCenter() + mpos_abs));
}
window.setView(view);
}
};
loop();
#endif // LOOP_LAMBDAS
};
}
Vector2 * Vector2::setLength2(float len2)
{
float oldLen2 = this->len2();
return (oldLen2 == 0 || oldLen2 == len2) ? this : scl((float)sqrt(len2 / oldLen2));
}
Matrix4D Scale(float x, float y, float z)
{
Matrix4D scl(x, 0, 0, 0, 0, y, 0, 0, 0, 0, z, 0, 0, 0, 0, 1);
return scl;
}
bool KstFilterDialogI::saveInputs(KstCPluginPtr plugin, KstSharedPtr<Plugin> p) {
KstReadLocker vl(&KST::vectorList.lock());
KstWriteLocker scl(&KST::scalarList.lock());
KstWriteLocker stl(&KST::stringList.lock());
const QValueList<Plugin::Data::IOValue>& itable = p->data()._inputs;
for (QValueList<Plugin::Data::IOValue>::ConstIterator it = itable.begin(); it != itable.end(); ++it) {
if ((*it)._type == Plugin::Data::IOValue::TableType) {
if ((*it)._name == p->data()._filterInputVector) {
KstVectorPtr v = *KST::vectorList.findTag(_yvector);
if (!v) {
return false;
}
plugin->inputVectors().insert((*it)._name, v);
} else {
QObject *field = _w->_pluginInputOutputFrame->child((*it)._name.latin1(), "VectorSelector");
if (field) {
VectorSelector *vs = static_cast<VectorSelector*>(field);
KstVectorPtr v = *KST::vectorList.findTag(vs->selectedVector());
if (!v) {
return false;
}
plugin->inputVectors().insert((*it)._name, v);
}
}
} else if ((*it)._type == Plugin::Data::IOValue::StringType) {
QObject *field = _w->_pluginInputOutputFrame->child((*it)._name.latin1(), "StringSelector");
if (field) {
StringSelector *ss = static_cast<StringSelector*>(field);
KstStringPtr s = *KST::stringList.findTag(ss->selectedString());
if (s == *KST::stringList.end()) {
QString val = ss->_string->currentText();
// create orphan string
KstStringPtr newString = new KstString(KstObjectTag(ss->_string->currentText(), KstObjectTag::orphanTagContext), 0L, val, true);
plugin->inputStrings().insert((*it)._name, newString);
} else {
return false;
}
}
} else if ((*it)._type == Plugin::Data::IOValue::PidType) {
// Nothing
} else {
QObject *field = _w->_pluginInputOutputFrame->child((*it)._name.latin1(), "ScalarSelector");
if (field) {
ScalarSelector *ss = static_cast<ScalarSelector*>(field);
KstScalarPtr s = *KST::scalarList.findTag(ss->selectedScalar());
if (s == *KST::scalarList.end()) {
bool ok;
double val = ss->_scalar->currentText().toDouble(&ok);
if (ok) {
// create orphan scalar
KstScalarPtr newScalar = new KstScalar(KstObjectTag(ss->_scalar->currentText(), KstObjectTag::orphanTagContext), 0L, val, true, false);
plugin->inputScalars().insert((*it)._name, newScalar);
} else {
return false;
}
} else {
plugin->inputScalars().insert((*it)._name, s);
}
}
}
}
return true;
}
// COMPUTE ======================================
MStatus gear_rollSplineKine::compute(const MPlug& plug, MDataBlock& data)
{
MStatus returnStatus;
// Error check
if (plug != output)
return MS::kUnknownParameter;
// Get inputs matrices ------------------------------
// Inputs Parent
MArrayDataHandle adh = data.inputArrayValue( ctlParent );
int count = adh.elementCount();
if (count < 1)
return MS::kFailure;
MMatrixArray inputsP(count);
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
inputsP[i] = adh.inputValue().asMatrix();
}
// Inputs
adh = data.inputArrayValue( inputs );
if (count != adh.elementCount())
return MS::kFailure;
MMatrixArray inputs(count);
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
inputs[i] = adh.inputValue().asMatrix();
}
adh = data.inputArrayValue( inputsRoll );
if (count != adh.elementCount())
return MS::kFailure;
MDoubleArray roll(adh.elementCount());
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
roll[i] = degrees2radians((double)adh.inputValue().asFloat());
}
// Output Parent
MDataHandle ha = data.inputValue( outputParent );
MMatrix outputParent = ha.asMatrix();
// Get inputs sliders -------------------------------
double in_u = (double)data.inputValue( u ).asFloat();
bool in_resample = data.inputValue( resample ).asBool();
int in_subdiv = data.inputValue( subdiv ).asShort();
bool in_absolute = data.inputValue( absolute ).asBool();
// Process ------------------------------------------
// Get roll, pos, tan, rot, scl
MVectorArray pos(count);
MVectorArray tan(count);
MQuaternion *rot;
rot = new MQuaternion[count];
MVectorArray scl(count);
double threeDoubles[3];
for (int i = 0 ; i < count ; i++){
MTransformationMatrix tp(inputsP[i]);
MTransformationMatrix t(inputs[i]);
pos[i] = t.getTranslation(MSpace::kWorld);
rot[i] = tp.rotation();
t.getScale(threeDoubles, MSpace::kWorld);
scl[i] = MVector(threeDoubles[0], threeDoubles[1], threeDoubles[2]);
tan[i] = MVector(threeDoubles[0] * 2.5, 0, 0).rotateBy(t.rotation());
}
// Get step and indexes
// We define between wich controlers the object is to be able to
// calculate the bezier 4 points front this 2 objects
double step = 1.0 / max( 1, count-1.0 );
int index1 = (int)min( count-2.0, in_u/step );
int index2 = index1+1;
int index1temp = index1;
int index2temp = index2;
double v = (in_u - step * double(index1)) / step;
double vtemp = v;
// calculate the bezier
MVector bezierPos;
MVector xAxis, yAxis, zAxis;
if(!in_resample){
// straight bezier solve
MVectorArray results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],v);
bezierPos = results[0];
xAxis = results[1];
}
else if(!in_absolute){
MVectorArray presample(in_subdiv);
MVectorArray presampletan(in_subdiv);
MDoubleArray samplelen(in_subdiv);
double samplestep = 1.0 / double(in_subdiv-1);
double sampleu = samplestep;
presample[0] = pos[index1];
presampletan[0] = tan[index1];
MVector prevsample(presample[0]);
MVector diff;
samplelen[0] = 0;
double overalllen = 0;
MVectorArray results(2);
for(long i=1;i<in_subdiv;i++,sampleu+=samplestep){
results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],sampleu);
presample[i] = results[0];
presampletan[i] = results[1];
diff = presample[i] - prevsample;
overalllen += diff.length();
samplelen[i] = overalllen;
prevsample = presample[i];
}
// now as we have the
sampleu = 0;
for(long i=0;i<in_subdiv-1;i++,sampleu+=samplestep){
samplelen[i+1] = samplelen[i+1] / overalllen;
if(v>=samplelen[i] && v <= samplelen[i+1]){
v = (v - samplelen[i]) / (samplelen[i+1] - samplelen[i]);
bezierPos = linearInterpolate(presample[i],presample[i+1],v);
xAxis = linearInterpolate(presampletan[i],presampletan[i+1],v);
break;
}
}
}
else{
MVectorArray presample(in_subdiv);
MVectorArray presampletan(in_subdiv);
MDoubleArray samplelen(in_subdiv);
double samplestep = 1.0 / double(in_subdiv-1);
double sampleu = samplestep;
presample[0] = pos[0];
presampletan[0] = tan[0];
MVector prevsample(presample[0]);
MVector diff;
samplelen[0] = 0;
double overalllen = 0;
MVectorArray results;
for(long i=1;i<in_subdiv;i++,sampleu+=samplestep){
index1 = (int)min(count-2,sampleu / step);
index2 = index1+1;
v = (sampleu - step * double(index1)) / step;
results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],v);
presample[i] = results[0];
presampletan[i] = results[1];
diff = presample[i] - prevsample;
overalllen += diff.length();
samplelen[i] = overalllen;
prevsample = presample[i];
}
// now as we have the
sampleu = 0;
for(long i=0;i<in_subdiv-1;i++,sampleu+=samplestep){
samplelen[i+1] = samplelen[i+1] / overalllen;
if(in_u>=samplelen[i] && in_u <= samplelen[i+1]){
in_u = (in_u - samplelen[i]) / (samplelen[i+1] - samplelen[i]);
bezierPos = linearInterpolate(presample[i],presample[i+1],in_u);
xAxis = linearInterpolate(presampletan[i],presampletan[i+1],in_u);
break;
}
}
}
// compute the scaling (straight interpolation!)
MVector scl1 = linearInterpolate(scl[index1temp], scl[index2temp],vtemp);
// compute the rotation!
MQuaternion q = slerp(rot[index1temp], rot[index2temp], vtemp);
yAxis = MVector(0,1,0);
yAxis = yAxis.rotateBy(q);
// use directly or project the roll values!
// print roll
double a = linearInterpolate(roll[index1temp], roll[index2temp], vtemp);
yAxis = yAxis.rotateBy( MQuaternion(xAxis.x * sin(a/2.0), xAxis.y * sin(a/2.0), xAxis.z * sin(a/2.0), cos(a/2.0)));
zAxis = xAxis ^ yAxis;
zAxis.normalize();
yAxis = zAxis ^ xAxis;
yAxis.normalize();
// Output -------------------------------------------
MTransformationMatrix result;
// translation
result.setTranslation(bezierPos, MSpace::kWorld);
// rotation
q = getQuaternionFromAxes(xAxis,yAxis,zAxis);
result.setRotationQuaternion(q.x, q.y, q.z, q.w);
// scaling
threeDoubles[0] = 1;
threeDoubles[0] = scl1.y;
threeDoubles[0] = scl1.z;
result.setScale(threeDoubles, MSpace::kWorld);
MDataHandle h = data.outputValue( output );
h.setMMatrix( result.asMatrix() * outputParent.inverse() );
data.setClean( plug );
return MS::kSuccess;
}
void Bras::in_state_func()
{
switch (state)
{
case RANGE_DEPART :
scr();
a0();
spb();
pf();
break;
case INT_RANGE :
//va bumper
scn();
a0();
spb();
pf();
break;
case INT2_RANGE :
set_time_out(400, trigger_to_be);
scn();
a0();
spb();
pf();
break;
case GO_ATTENTE :
scn();
a1();
spb();
pf();
break;
case ATTENTE_ACTIF :
scn();
a1();
spb();
pf();
break;
case DESCENTE_LAT :
scn();
a4();
spb();
pf();
break;
case DESCENTE_POMPE_LAT :
scn();
a4();
spb();
po();
break;
case PRISE_LAT :
scn();
a4();
spb();
po();
break;
case MONTE_VERT :
set_time_out(500, TIME_OUT);
scn();
a0();
spv();
po();
break;
case MONTE :
scn();
a0();
spb();
po();
break;
case RANGE_PRISE :
set_time_out(2000, TIME_OUT);
scl();
a0();
spb();
po();
break;
case LACHE :
set_time_out(300, TIME_OUT);
scl();
a0();
spb();
pf();
break;
case MONTE_ECH :
scn();
a3();
spb();
po();
break;
case MONTE_ECH_VERT :
sce();
a3();
spr();
po();
break;
case RETOURNE_ECH :
call_for_help();
sce();
a3();
spr();
po();
break;
case REPLACE_APRES_ECH :
set_time_out(300, TIME_OUT);
scn();
a3();
spb();
pf();
break;
case SEND_MINE :
scv();
a4();
spv();
pf();
break;
case SENDMASTER_PRET :
scv();
a4();
spv();
po();
break;
case PRISE_VERT :
scv();
a4();
spv();
po();
break;
case PRISE_COPAIN :
a2();
sce();
spb();
po();
break;
}
}
