SpatialMotionVectorRaw PositionRaw::changePointOf(const SpatialMotionVectorRaw & other) const
{
SpatialMotionVectorRaw result;
Eigen::Map<const Eigen::Vector3d> thisPos(this->data());
Eigen::Map<const Vector6d> otherTwist(other.data());
Eigen::Map<Vector6d> resTwist(result.data());
resTwist.segment<3>(0) = otherTwist.segment<3>(0)+thisPos.cross(otherTwist.segment<3>(3));
resTwist.segment<3>(3) = otherTwist.segment<3>(3);
return result;
}
SpatialForceVectorRaw PositionRaw::changePointOf(const SpatialForceVectorRaw & other) const
{
SpatialForceVectorRaw result;
Eigen::Map<const Eigen::Vector3d> thisPos(this->data());
Eigen::Map<const Vector6d> otherWrench(other.data());
Eigen::Map<Vector6d> resWrench(result.data());
resWrench.segment<3>(0) = otherWrench.segment<3>(0);
resWrench.segment<3>(3) = thisPos.cross(otherWrench.segment<3>(0))+otherWrench.segment<3>(3);
return result;
}
SpatialMotionVector PositionRaw::changePointOf(const SpatialMotionVector & other) const
{
SpatialMotionVector result;
Eigen::Map<const Eigen::Vector3d> thisPos(this->data());
Eigen::Map<const Eigen::Vector3d> otherLinear(other.getLinearVec3().data());
Eigen::Map<const Eigen::Vector3d> otherAngular(other.getAngularVec3().data());
Eigen::Map<Eigen::Vector3d> resLinear(result.getLinearVec3().data());
Eigen::Map<Eigen::Vector3d> resAngular(result.getAngularVec3().data());
resLinear = otherLinear + thisPos.cross(otherAngular);
resAngular = otherAngular;
return result;
}
void NavigationPath::UpdatePath(bool force)
{
if (m_navMesh == nullptr)
return;
if (m_query == nullptr)
{
m_query.reset(new dtNavMeshQuery);
m_query->init(m_navMesh, 10000 /* MAX_NODES */);
}
PSPAWNINFO me = GetCharInfo()->pSpawn;
if (me == nullptr)
return;
float startOffset[3] = { me->X, me->Feet, me->Y };
float endOffset[3] = { m_destination.x, m_destination.z, m_destination.y };
float spos[3];
float epos[3];
glm::vec3 thisPos(startOffset[0], startOffset[1], startOffset[2]);
if (thisPos == m_lastPos && !force)
return;
m_lastPos = thisPos;
m_currentPathCursor = 0;
m_currentPathSize = 0;
dtPolyRef startRef, endRef;
m_query->findNearestPoly(startOffset, m_extents, &m_filter, &startRef, spos);
if (!startRef)
{
WriteChatf(PLUGIN_MSG "No start reference");
return;
}
dtPolyRef polys[MAX_POLYS];
int numPolys = 0;
if (!m_corridor)
{
if (m_useCorridor)
{
// initialize planning
m_corridor.reset(new dtPathCorridor);
m_corridor->init(MAX_PATH_SIZE);
m_corridor->reset(startRef, startOffset);
}
m_query->findNearestPoly(endOffset, m_extents, &m_filter, &endRef, epos);
if (!endRef)
{
WriteChatf(PLUGIN_MSG "No end reference");
return;
}
dtStatus status = m_query->findPath(startRef, endRef, spos, epos, &m_filter, polys, &numPolys, MAX_POLYS);
if (status & DT_OUT_OF_NODES)
DebugSpewAlways("findPath from %.2f,%.2f,%.2f to %.2f,%.2f,%.2f failed: out of nodes",
startOffset[0], startOffset[1], startOffset[2],
endOffset[0], endOffset[1], endOffset[2]);
if (status & DT_PARTIAL_RESULT)
DebugSpewAlways("findPath from %.2f,%.2f,%.2f to %.2f,%.2f,%.2f returned a partial result.",
startOffset[0], startOffset[1], startOffset[2],
endOffset[0], endOffset[1], endOffset[2]);
if (m_corridor)
{
m_corridor->setCorridor(endOffset, polys, numPolys);
}
}
else
{
// this is an update
m_corridor->movePosition(startOffset, m_query.get(), &m_filter);
}
if (m_corridor)
{
m_corridor->optimizePathTopology(m_query.get(), &m_filter);
m_currentPathSize = m_corridor->findCorners(m_currentPath,
m_cornerFlags, polys, 10, m_query.get(), &m_filter);
}
if (m_debugDrawGrp)
m_debugDrawGrp->Reset();
if (numPolys > 0)
{
m_query->findStraightPath(spos, epos, polys, numPolys, m_currentPath,
0, 0, &m_currentPathSize, MAX_POLYS, 0);
// The 0th index is the starting point. Begin by trying to reach the
// 2nd point...
if (m_currentPathSize > 1)
m_currentPathCursor = 1;
if (m_debugDrawGrp && mq2nav::GetSettings().debug_render_pathing)
{
DebugDrawDX dd(m_debugDrawGrp.get());
// draw current position
duDebugDrawCross(&dd, me->X, me->Feet, me->Y, 0.5, DXColor(51, 255, 255), 1);
// Draw the waypoints. Green is next point
for (int i = 0; i < m_currentPathSize; ++i)
{
int color = DXColor(255, 255, 255);
if (i < m_currentPathCursor)
color = DXColor(0, 102, 204);
if (i == m_currentPathCursor)
color = DXColor(0, 255, 0);
if (i == m_currentPathSize - 1)
color = DXColor(255, 0, 0);
duDebugDrawCross(&dd, m_currentPath[i * 3],
m_currentPath[i * 3 + 1],
m_currentPath[i * 3 + 2],
0.5, color, 1);
}
}
}
if (m_line && mq2nav::GetSettings().show_nav_path)
{
m_line->Update();
}
}
void GCode::draw(const Settings &settings, int layer,
bool liveprinting, int linewidth)
{
/*--------------- Drawing -----------------*/
//cerr << "gc draw "<<layer << " - " <<liveprinting << endl;
Vector3d thisPos(0,0,0);
uint start = 0, end = 0;
uint n_cmds = commands.size();
bool arrows = true;
if (layerchanges.size()>0) {
// have recorded layerchange indices -> draw whole layers
if (layer>-1) {
if (layer != 0)
start = layerchanges[layer];
if (layer < (int)layerchanges.size()-1)
end = layerchanges[layer+1];
else
end = commands.size();
}
else {
int n_changes = layerchanges.size();
int sind = 0;
int eind = 0;
if (n_changes > 0) {
sind = (uint)ceil(settings.Display.GCodeDrawStart*(n_changes-1)/Max.z());
eind = (uint)ceil(settings.Display.GCodeDrawEnd *(n_changes-1)/Max.z());
}
if (sind>=eind) {
eind = MIN(sind+1, n_changes-1);
}
else arrows = false; // arrows only for single layers
sind = CLAMP(sind, 0, n_changes-1);
eind = CLAMP(eind, 0, n_changes-1);
if (sind == 0) start = 0;
else start = layerchanges[sind];
//if (start>0) start-=1; // get one command before layer
end = layerchanges[eind];
if (sind == n_changes-1) end = commands.size(); // get last layer
if (eind == n_changes-1) end = commands.size(); // get last layer
}
}
else {
if (n_cmds > 0) {
start = (uint)(settings.Display.GCodeDrawStart*(n_cmds)/Max.z());
end = (uint)(settings.Display.GCodeDrawEnd *(n_cmds)/Max.z());
}
}
drawCommands(settings, start, end, liveprinting, linewidth,
arrows && settings.Display.DisplayGCodeArrows,
!liveprinting && settings.Display.DisplayGCodeBorders);
if (currentCursorWhere!=Vector3d::ZERO) {
glDisable(GL_DEPTH_TEST);
// glPointSize(10);
// glLineWidth(5);
// glColor4f(1.f,0.f,1.f,1.f);
// glBegin(GL_POINTS);
// glVertex3dv(currentCursorWhere);
// glEnd();
// glBegin(GL_LINES);
// glVertex3dv(currentCursorFrom);
// glVertex3dv(currentCursorWhere);
// glEnd();
const Vector3d offset =
Vector3d(settings.Extruders[currentCursorCommand.extruder_no].OffsetX,
settings.Extruders[currentCursorCommand.extruder_no].OffsetY, 0.);
currentCursorCommand.draw(currentCursorFrom, offset, 7,
Vector4f(1.f,0.f,1.f,1.f),
0., true, false);
}
}
std::string MapEditor::blockType(Terrain* terrain){
bool leftOccupied(false);
bool rightOccupied(false);
bool topOccupied(false);
bool botOccupied(false);
bool topLeftOccupied(true);
bool topRightOccupied(true);
bool botLeftOccupied(true);
bool botRightOccupied(true);
sf::Vector2f thisPos(terrain->getPos());
std::string returnString;
Terrains relevantTerrains;
for (Terrains::size_type i = 0; i < mTerrains.size(); i++){
if (mTerrains[i]->getPos().x < thisPos.x + 200 && mTerrains[i]->getPos().x > thisPos.x - 200)
if(mTerrains[i]->getType() == Terrain::BLOCK0 ||
mTerrains[i]->getType() == Terrain::BLOCK0ICY ||
mTerrains[i]->getType() == Terrain::BLOCK0WALLJUMP)
relevantTerrains.push_back(mTerrains[i]);
}
sf::Vector2f leftBorder(thisPos.x - 1,
thisPos.y + (terrain->getHeight()/ 2));
sf::Vector2f rightBorder(thisPos.x + terrain->getWidth() + 1,
thisPos.y + (terrain->getHeight() / 2));
sf::Vector2f topBorder(thisPos.x + (terrain->getWidth() / 2),
thisPos.y - 1);
sf::Vector2f botBorder(thisPos.x + (terrain->getWidth() / 2),
thisPos.y + (terrain->getHeight() + 1));
sf::Vector2f topLeftBorder(terrain->getPos());
topLeftBorder.x -= 1;
topLeftBorder.y -= 1;
sf::Vector2f topRightBorder(terrain->getPos());
topRightBorder.x += terrain->getWidth() + 1;
topRightBorder.y -= 1;
sf::Vector2f botLeftBorder(terrain->getPos());
botLeftBorder.x -= 1;
botLeftBorder.y += terrain->getHeight() + 1;
sf::Vector2f botRightBorder(terrain->getPos());
botRightBorder.x += terrain->getWidth() + 1;
botRightBorder.y += terrain->getHeight() + 1;
for (Terrains::size_type i = 0; i < relevantTerrains.size(); i++){
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &leftBorder))
leftOccupied = true;
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &rightBorder))
rightOccupied = true;
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &topBorder))
topOccupied = true;
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &botBorder))
botOccupied = true;
}
if (leftOccupied && rightOccupied && topOccupied && botOccupied)
returnString.push_back('a');
if (leftOccupied && rightOccupied && topOccupied && !botOccupied)
returnString.push_back('b');
if (leftOccupied && rightOccupied && !topOccupied && botOccupied)
returnString.push_back('c');
if (leftOccupied && !rightOccupied && topOccupied && botOccupied)
returnString.push_back('d');
if (!leftOccupied && rightOccupied && topOccupied && botOccupied)
returnString.push_back('e');
if (leftOccupied && rightOccupied && !topOccupied && !botOccupied)
returnString.push_back('f');
if (leftOccupied && !rightOccupied && topOccupied && !botOccupied)
returnString.push_back('g');
if (!leftOccupied && rightOccupied && topOccupied && !botOccupied)
returnString.push_back('h');
if (leftOccupied && !rightOccupied && !topOccupied && botOccupied)
returnString.push_back('i');
if (!leftOccupied && rightOccupied && !topOccupied && botOccupied)
returnString.push_back('j');
if (!leftOccupied && !rightOccupied && topOccupied && botOccupied)
returnString.push_back('k');
if (leftOccupied && !rightOccupied && !topOccupied && !botOccupied)
returnString.push_back('l');
if (!leftOccupied && rightOccupied && !topOccupied && !botOccupied)
returnString.push_back('m');
if (!leftOccupied && !rightOccupied && topOccupied && !botOccupied)
returnString.push_back('n');
if (!leftOccupied && !rightOccupied && !topOccupied && botOccupied)
returnString.push_back('o');
if (!leftOccupied && !rightOccupied && !topOccupied && !botOccupied)
returnString.push_back('p');
bool tileDecNeeded(false);
if (returnString[0] == 'a') {
topLeftOccupied = false;
topRightOccupied = false;
botLeftOccupied = false;
botRightOccupied = false;
for (Terrains::size_type i = 0; i < relevantTerrains.size(); i++) {
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &topLeftBorder))
topLeftOccupied = true;
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &topRightBorder))
topRightOccupied = true;
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &botLeftBorder))
botLeftOccupied = true;
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &botRightBorder))
botRightOccupied = true;
}
tileDecNeeded = true;
}
if (returnString[0] == 'b') {
topLeftOccupied = false;
topRightOccupied = false;
for (Terrains::size_type i = 0; i < relevantTerrains.size(); i++) {
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &topLeftBorder))
topLeftOccupied = true;
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &topRightBorder))
topRightOccupied = true;
}
tileDecNeeded = true;
}
if (returnString[0] == 'c') {
botLeftOccupied = false;
botRightOccupied = false;
for (Terrains::size_type i = 0; i < relevantTerrains.size(); i++) {
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &botLeftBorder))
botLeftOccupied = true;
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &botRightBorder))
botRightOccupied = true;
}
tileDecNeeded = true;
}
if (returnString[0] == 'd') {
topLeftOccupied = false;
botLeftOccupied = false;
for (Terrains::size_type i = 0; i < relevantTerrains.size(); i++) {
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &topLeftBorder))
topLeftOccupied = true;
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &botLeftBorder))
botLeftOccupied = true;
}
tileDecNeeded = true;
}
if (returnString[0] == 'e') {
topRightOccupied = false;
botRightOccupied = false;
for (Terrains::size_type i = 0; i < relevantTerrains.size(); i++) {
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &topRightBorder))
topRightOccupied = true;
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &botRightBorder))
botRightOccupied = true;
}
tileDecNeeded = true;
}
if (returnString[0] == 'g') {
topLeftOccupied = false;
for (Terrains::size_type i = 0; i < relevantTerrains.size(); i++) {
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &topLeftBorder))
topLeftOccupied = true;
}
tileDecNeeded = true;
}
if (returnString[0] == 'h') {
topRightOccupied = false;
for (Terrains::size_type i = 0; i < relevantTerrains.size(); i++) {
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &topRightBorder))
topRightOccupied = true;
}
tileDecNeeded = true;
}
if (returnString[0] == 'i') {
botLeftOccupied = false;
for (Terrains::size_type i = 0; i < relevantTerrains.size(); i++) {
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &botLeftBorder))
botLeftOccupied = true;
}
tileDecNeeded = true;
}
if (returnString[0] == 'j') {
botRightOccupied = false;
for (Terrains::size_type i = 0; i < relevantTerrains.size(); i++) {
if (MapEditor::isSpriteClicked(relevantTerrains[i]->getSprite(), &botRightBorder))
botRightOccupied = true;
}
tileDecNeeded = true;
}
if (tileDecNeeded) {
if (!topLeftOccupied && !topRightOccupied && !botRightOccupied && !botLeftOccupied)
returnString.push_back('a');
if (topLeftOccupied && topRightOccupied && !botRightOccupied && !botLeftOccupied)
returnString.push_back('b');
if (!topLeftOccupied && topRightOccupied && botRightOccupied && !botLeftOccupied)
returnString.push_back('c');
if (topLeftOccupied && !topRightOccupied && !botRightOccupied && botLeftOccupied)
returnString.push_back('d');
if (!topLeftOccupied && !topRightOccupied && botRightOccupied && botLeftOccupied)
returnString.push_back('e');
if (!topLeftOccupied && topRightOccupied && botRightOccupied && botLeftOccupied)
returnString.push_back('f');
if (topLeftOccupied && !topRightOccupied && botRightOccupied && botLeftOccupied)
returnString.push_back('g');
if (topLeftOccupied && topRightOccupied && !botRightOccupied && botLeftOccupied)
returnString.push_back('h');
if (topLeftOccupied && topRightOccupied && botRightOccupied && !botLeftOccupied)
returnString.push_back('i');
if (topLeftOccupied && !topRightOccupied && !botRightOccupied && !botLeftOccupied)
returnString.push_back('j');
if (!topLeftOccupied && topRightOccupied && !botRightOccupied && !botLeftOccupied)
returnString.push_back('k');
if (!topLeftOccupied && !topRightOccupied && botRightOccupied && !botLeftOccupied)
returnString.push_back('l');
if (!topLeftOccupied && !topRightOccupied && !botRightOccupied && botLeftOccupied)
returnString.push_back('m');
}
return returnString;
}
