void MidiSequencer::outputThreadStopped()
{
for (int channel = 0; channel < 16; ++channel) {
drumstick::ControllerEvent ev1(channel, MIDI_CTL_ALL_NOTES_OFF, 0);
ev1.setSource(m_outputPortId);
ev1.setSubscribers();
ev1.setDirect();
m_client->outputDirect(&ev1);
drumstick::ControllerEvent ev2(channel, MIDI_CTL_ALL_SOUNDS_OFF, 0);
ev2.setSource(m_outputPortId);
ev2.setSubscribers();
ev2.setDirect();
m_client->outputDirect(&ev2);
}
m_client->drainOutput();
}
main()
{
S2CINT *sp;
#if MAC_CLASSIC
STACKPTR( sp );
SetApplLimit( (char*)sp-57000 );
console_options.nrows = 30;
console_options.title = "\pScheme->C";
#endif
printf( "Embedded Scheme->C Test Bed\n0- " );
scheme2c( "(begin (set-stack-size! 57000) (set-time-slice! 100000))",
&status, &result, &error );
if (status != 0) {
printf( "Initialization failed!\n" );
exit( 1 );
}
while (gets( line ) != NULL) {
switch (s) {
case 0:
ev0();
break;
case 1:
ev1();
break;
case 2:
ev2();
break;
case 3:
ev3();
break;
}
s = (s + 1) & 3;
if (*result != 0) printf( "%s\n", result );
if (*error != 0) printf( "%s", error );
printf( "%d- ", status );
fflush( stdout );
}
printf( "\n" );
exit( 0 );
}
void Audio::processMidi()
{
midiBusy = true;
//
// TODO: syntis should directly write into recordEventList
//
for (iMidiDevice id = midiDevices.begin(); id != midiDevices.end(); ++id)
{
MidiDevice* md = *id;
MPEventList* playEvents = md->playEvents();
//
// erase already played events:
//
iMPEvent nextPlayEvent = md->nextPlayEvent();
playEvents->erase(playEvents->begin(), nextPlayEvent);
// klumsy hack for synti devices:
if (md->isSynti())
{
SynthI* s = (SynthI*) md;
while (s->eventsPending())
{
MidiRecordEvent ev = s->receiveEvent();
md->recordEvent(ev);
}
}
// Is it a Jack midi device?
//MidiJackDevice* mjd = dynamic_cast<MidiJackDevice*>(md);
//if(mjd)
// mjd->collectMidiEvents();
md->collectMidiEvents();
// Take snapshots of the current sizes of the recording fifos,
// because they may change while here in process, asynchronously.
md->beforeProcess();
}
MPEventList* playEvents = metronome->playEvents();
iMPEvent nextPlayEvent = metronome->nextPlayEvent();
playEvents->erase(playEvents->begin(), nextPlayEvent);
// p3.3.25
bool extsync = extSyncFlag.value();
for (iMidiTrack t = song->midis()->begin(); t != song->midis()->end(); ++t)
{
MidiTrack* track = *t;
int port = track->outPort();
MidiDevice* md = midiPorts[port].device();
// Changed by Tim. p3.3.8
//if(md == 0)
// continue;
//MPEventList* playEvents = md->playEvents();
//if (playEvents == 0)
// continue;
//if (!track->isMute())
MPEventList* playEvents = 0;
if (md)
{
playEvents = md->playEvents();
// only add track events if the track is unmuted
if (!track->isMute())
{
if (isPlaying() && (curTickPos < nextTickPos))
collectEvents(track, curTickPos, nextTickPos);
}
}
//
//----------midi recording
//
if (track->recordFlag())
{
//int portMask = track->inPortMask();
// p3.3.38 Removed
//unsigned int portMask = track->inPortMask();
//int channelMask = track->inChannelMask();
MPEventList* rl = track->mpevents();
MidiPort* tport = &midiPorts[port];
// p3.3.38
//for (iMidiDevice id = midiDevices.begin(); id != midiDevices.end(); ++id)
//{
RouteList* irl = track->inRoutes();
for (ciRoute r = irl->begin(); r != irl->end(); ++r)
{
//if(!r->isValid() || (r->type != Route::ALSA_MIDI_ROUTE && r->type != Route::JACK_MIDI_ROUTE))
//if(!r->isValid() || (r->type != Route::MIDI_DEVICE_ROUTE))
if (!r->isValid() || (r->type != Route::MIDI_PORT_ROUTE)) // p3.3.49
continue;
int devport = r->midiPort; // p3.3.49
if (devport == -1)
continue;
//MidiDevice* dev = *id;
//MidiDevice* dev = r->device;
MidiDevice* dev = midiPorts[devport].device(); // p3.3.49
if (!dev)
continue;
// p3.3.50 Removed
//int channel = r->channel;
// NOTE: TODO: Special for input device sysex 'channel' marked as -1, ** IF we end up going with that method **.
// This would mean having a separate 'System' channel listed in the routing popups.
// The other alternative is to accept sysex from a device as long as ANY regular channel is routed from it,
// this does not require a 'System' channel listed in the routing popups.
// But that requires more code below... Done.
//if(channel == -1)
//channel = MIDI_CHANNELS; // Special channel '17'
// continue;
//int devport = dev->midiPort();
// record only from ports marked in portMask:
//if (devport == -1 || !(portMask & (1 << devport)))
//if (devport == -1)
// continue;
//MREventList* el = dev->recordEvents();
//MidiFifo& rf = dev->recordEvents();
int channelMask = r->channel; // p3.3.50
if (channelMask == -1 || channelMask == 0)
continue;
for (int channel = 0; channel < MIDI_CHANNELS; ++channel) // p3.3.50
{
if (!(channelMask & (1 << channel)))
continue;
if (!dev->sysexFIFOProcessed())
{
// Set to the sysex fifo at first.
MidiFifo& rf = dev->recordEvents(MIDI_CHANNELS);
// Get the frozen snapshot of the size.
int count = dev->tmpRecordCount(MIDI_CHANNELS);
for (int i = 0; i < count; ++i)
{
MidiPlayEvent event(rf.peek(i));
//unsigned time = event.time() + segmentSize*(segmentCount-1);
//unsigned time = event.time() + (extsync ? config.division/24 : segmentSize*(segmentCount-1));
//unsigned time = extsync ? curTickPos : (event.time() + segmentSize*(segmentCount-1));
//event.setTime(time);
//if(!extsync)
// event.setTime(event.time() + segmentSize*(segmentCount-1));
event.setPort(port);
// dont't echo controller changes back to software
// synthesizer:
if (!dev->isSynti() && md && track->recEcho())
playEvents->add(event);
// If syncing externally the event time is already in units of ticks, set above.
if (!extsync)
{
//time = tempomap.frame2tick(event.time());
//event.setTime(time); // set tick time
event.setTime(tempomap.frame2tick(event.time())); // set tick time
}
if (recording)
rl->add(event);
}
dev->setSysexFIFOProcessed(true);
}
// Set to the sysex fifo at first.
///MidiFifo& rf = dev->recordEvents(MIDI_CHANNELS);
// Get the frozen snapshot of the size.
///int count = dev->tmpRecordCount(MIDI_CHANNELS);
// Iterate once for sysex fifo (if needed), once for channel fifos.
///for(int sei = 0; sei < 2; ++sei)
{
// If on first pass, do sysex fifo.
/*
if(sei == 0)
{
// Ignore any further channel routes on this device if already done here.
if(dev->sysexFIFOProcessed())
continue;
// Go ahead and set this now.
dev->setSysexFIFOProcessed(true);
// Allow it to fall through with the sysex fifo and count...
}
else
{
// We're on the second pass, do channel fifos.
rf = dev->recordEvents(channel);
// Get the frozen snapshot of the size.
count = dev->tmpRecordCount(channel);
}
*/
MidiFifo& rf = dev->recordEvents(channel);
int count = dev->tmpRecordCount(channel);
//for (iMREvent ie = el->begin(); ie != el->end(); ++ie)
for (int i = 0; i < count; ++i)
{
MidiPlayEvent event(rf.peek(i));
//int channel = ie->channel();
///int channel = event.channel();
int defaultPort = devport;
///if (!(channelMask & (1 << channel)))
///{
/// continue;
///}
//MidiPlayEvent event(*ie);
int drumRecPitch = 0; //prevent compiler warning: variable used without initialization
MidiController *mc = 0;
int ctl = 0;
//Hmmm, hehhh...
// TODO: Clean up a bit around here when it comes to separate events for rec & for playback.
// But not before 0.7 (ml)
int prePitch = 0, preVelo = 0;
event.setChannel(track->outChannel());
if (event.isNote() || event.isNoteOff())
{
//
// apply track values
//
//Apply drum inkey:
if (track->type() == Track::DRUM)
{
int pitch = event.dataA();
//Map note that is played according to drumInmap
drumRecPitch = drumMap[(unsigned int) drumInmap[pitch]].enote;
devport = drumMap[(unsigned int) drumInmap[pitch]].port;
event.setPort(devport);
channel = drumMap[(unsigned int) drumInmap[pitch]].channel;
event.setA(drumMap[(unsigned int) drumInmap[pitch]].anote);
event.setChannel(channel);
}
else
{ //Track transpose if non-drum
prePitch = event.dataA();
int pitch = prePitch + track->transposition;
if (pitch > 127)
pitch = 127;
if (pitch < 0)
pitch = 0;
event.setA(pitch);
}
if (!event.isNoteOff())
{
preVelo = event.dataB();
int velo = preVelo + track->velocity;
velo = (velo * track->compression) / 100;
if (velo > 127)
velo = 127;
if (velo < 1)
velo = 1;
event.setB(velo);
}
}
// Added by T356.
else if (event.type() == ME_CONTROLLER)
{
//printf("11111111111111111111111111111111111111111111111111111\n");
if (track->type() == Track::DRUM)
{
//printf("2222222222222222222222222222222222222222222222222222222222\n");
ctl = event.dataA();
// Regardless of what port the event came from, is it a drum controller event
// according to the track port's instrument?
mc = tport->drumController(ctl);
if (mc)
{
//printf("333333333333333333333333333333333333333333333333\n");
int pitch = ctl & 0x7f;
ctl &= ~0xff;
int dmindex = drumInmap[pitch] & 0x7f;
//Map note that is played according to drumInmap
drumRecPitch = drumMap[dmindex].enote;
devport = drumMap[dmindex].port;
event.setPort(devport);
channel = drumMap[dmindex].channel;
event.setA(ctl | drumMap[dmindex].anote);
event.setChannel(channel);
}
}
}
// p3.3.25
// OOMidi uses a fixed clocks per quarternote of 24.
// At standard 384 ticks per quarternote for example,
// 384/24=16 for a division of 16 sub-frames (16 OOMidi 'ticks').
// That is what we'll use if syncing externally.
//unsigned time = event.time() + segmentSize*(segmentCount-1);
//unsigned time = event.time() + (extsync ? config.division/24 : segmentSize*(segmentCount-1));
// p3.3.34
// Oops, use the current tick.
//unsigned time = extsync ? curTickPos : (event.time() + segmentSize*(segmentCount-1));
//event.setTime(time);
// p3.3.35
// If ext sync, events are now time-stamped with last tick in MidiDevice::recordEvent().
// TODO: Tested, but record resolution not so good. Switch to wall clock based separate list in MidiDevice.
// p3.3.36
//if(!extsync)
// event.setTime(event.time() + segmentSize*(segmentCount-1));
// dont't echo controller changes back to software
// synthesizer:
if (!dev->isSynti())
{
//printf("444444444444444444444444444444444444444444444444444444\n");
//Check if we're outputting to another port than default:
if (devport == defaultPort)
{
//printf("5555555555555555555555555555555555555555555\n");
event.setPort(port);
if (md && track->recEcho())
playEvents->add(event);
}
else
{
//printf("66666666666666666666666666666666666666\n");
// Hmm, this appears to work, but... Will this induce trouble with md->setNextPlayEvent??
MidiDevice* mdAlt = midiPorts[devport].device();
if (mdAlt && track->recEcho())
mdAlt->playEvents()->add(event);
}
// Shall we activate meters even while rec echo is off? Sure, why not...
if (event.isNote() && event.dataB() > track->activity())
track->setActivity(event.dataB());
}
// p3.3.25
// If syncing externally the event time is already in units of ticks, set above.
if (!extsync)
{
//printf("7777777777777777777777777777777777777777777\n");
// p3.3.35
//time = tempomap.frame2tick(event.time());
//event.setTime(time); // set tick time
event.setTime(tempomap.frame2tick(event.time())); // set tick time
}
// Special handling of events stored in rec-lists. a bit hACKish. TODO: Clean up (after 0.7)! :-/ (ml)
if (recording)
{
//printf("888888888888888888888888888888888888888888888888\n");
// In these next steps, it is essential to set the recorded event's port
// to the track port so buildMidiEventList will accept it. Even though
// the port may have no device "<none>".
//
if (track->type() == Track::DRUM)
{
//printf("99999999999999999999999999999999999999999999999999\n");
// Is it a drum controller event?
if (mc)
{
//printf("aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa\n");
MidiPlayEvent drumRecEvent = event;
drumRecEvent.setA(ctl | drumRecPitch);
// In this case, preVelo is simply the controller value.
drumRecEvent.setB(preVelo);
drumRecEvent.setPort(port); //rec-event to current port
drumRecEvent.setChannel(track->outChannel()); //rec-event to current channel
rl->add(drumRecEvent);
}
else
{
//printf("bbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbbb\n");
MidiPlayEvent drumRecEvent = event;
drumRecEvent.setA(drumRecPitch);
drumRecEvent.setB(preVelo);
// Changed by T356.
// Tested: Events were not being recorded for a drum map entry pointing to a
// different port. This must have been wrong - buildMidiEventList would ignore this.
//drumRecEvent.setPort(devport);
drumRecEvent.setPort(port); //rec-event to current port
drumRecEvent.setChannel(track->outChannel()); //rec-event to current channel
rl->add(drumRecEvent);
}
}
else
{
//printf("ccccccccccccccccccccccccccccccccccccccccccccc\n");
// Restore record-pitch to non-transposed value since we don't want the note transposed twice next
MidiPlayEvent recEvent = event;
if (prePitch)
recEvent.setA(prePitch);
if (preVelo)
recEvent.setB(preVelo);
recEvent.setPort(port);
recEvent.setChannel(track->outChannel());
rl->add(recEvent);
}
}
}
}
}
}
}
// Added by Tim. p3.3.8
if (md)
{
//printf("zzzzzzzzzzzzzzzzzzzzzzzzzzzzzzz\n");
md->setNextPlayEvent(playEvents->begin());
}
}
//
// clear all recorded events in midiDevices
// process stuck notes
//
for (iMidiDevice id = midiDevices.begin(); id != midiDevices.end(); ++id)
{
//printf("--------------------------aaaaaaaaaaaaaaaaaaaaaaaaaaaa\n");
MidiDevice* md = *id;
///md->recordEvents()->clear();
// By T356. Done processing this rec buffer, now flip to the other one.
///md->flipRecBuffer();
// We are done with the 'frozen' recording fifos, remove the events.
md->afterProcess();
MPEventList* stuckNotes = md->stuckNotes();
MPEventList* playEvents = md->playEvents();
iMPEvent k;
for (k = stuckNotes->begin(); k != stuckNotes->end(); ++k)
{
if (k->time() >= nextTickPos)
break;
MidiPlayEvent ev(*k);
// p3.3.25
//int frame = tempomap.tick2frame(k->time()) + frameOffset;
if (extsync)
{
ev.setTime(k->time());
}
else
{
int frame = tempomap.tick2frame(k->time()) + frameOffset;
ev.setTime(frame);
}
// p3.3.25
//ev.setTime(frame);
playEvents->add(ev);
}
stuckNotes->erase(stuckNotes->begin(), k);
md->setNextPlayEvent(playEvents->begin());
}
//---------------------------------------------------
// insert metronome clicks
//---------------------------------------------------
MidiDevice* md = 0;
if (midiClickFlag)
md = midiPorts[clickPort].device();
if (song->click() && (isPlaying() || state == PRECOUNT))
{
MPEventList* playEvents = 0;
MPEventList* stuckNotes = 0;
if (md)
{
playEvents = md->playEvents();
stuckNotes = md->stuckNotes();
}
int bar, beat;
unsigned tick;
bool isMeasure = false;
while (midiClick < nextTickPos)
{
if (isPlaying())
{
///sigmap.tickValues(midiClick, &bar, &beat, &tick);
AL::sigmap.tickValues(midiClick, &bar, &beat, &tick);
isMeasure = beat == 0;
}
else if (state == PRECOUNT)
{
isMeasure = (clickno % clicksMeasure) == 0;
}
// p3.3.25
//int frame = tempomap.tick2frame(midiClick) + frameOffset;
int evtime = extsync ? midiClick : tempomap.tick2frame(midiClick) + frameOffset;
// p3.3.25
//MidiPlayEvent ev(frame, clickPort, clickChan, ME_NOTEON,
MidiPlayEvent ev(evtime, clickPort, clickChan, ME_NOTEON,
beatClickNote, beatClickVelo);
if (md)
{
// p3.3.25
//MidiPlayEvent ev(frame, clickPort, clickChan, ME_NOTEON,
MidiPlayEvent ev(evtime, clickPort, clickChan, ME_NOTEON,
beatClickNote, beatClickVelo);
if (isMeasure)
{
ev.setA(measureClickNote);
ev.setB(measureClickVelo);
}
playEvents->add(ev);
}
if (audioClickFlag)
{
// p3.3.25
//MidiPlayEvent ev1(frame, 0, 0, ME_NOTEON, 0, 0);
MidiPlayEvent ev1(evtime, 0, 0, ME_NOTEON, 0, 0);
ev1.setA(isMeasure ? 0 : 1);
metronome->playEvents()->add(ev1);
}
if (md)
{
ev.setB(0);
// p3.3.25
// Removed. Why was this here?
//frame = tempomap.tick2frame(midiClick+20) + frameOffset;
//
// Does it mean this should be changed too?
// No, stuck notes are in units of ticks, not frames like (normal, non-external) play events...
ev.setTime(midiClick + 10);
if (md)
stuckNotes->add(ev);
}
if (isPlaying())
///midiClick = sigmap.bar2tick(bar, beat+1, 0);
midiClick = AL::sigmap.bar2tick(bar, beat + 1, 0);
else if (state == PRECOUNT)
{
midiClick += ticksBeat;
if (clickno)
--clickno;
else
state = START_PLAY;
}
}
if (md)
md->setNextPlayEvent(playEvents->begin());
if (audioClickFlag)
metronome->setNextPlayEvent(metronome->playEvents()->begin());
}
if (state == STOP)
{
//---------------------------------------------------
// end all notes
//---------------------------------------------------
for (iMidiDevice imd = midiDevices.begin(); imd != midiDevices.end(); ++imd)
{
MidiDevice* md = *imd;
MPEventList* playEvents = md->playEvents();
MPEventList* stuckNotes = md->stuckNotes();
for (iMPEvent k = stuckNotes->begin(); k != stuckNotes->end(); ++k)
{
MidiPlayEvent ev(*k);
ev.setTime(0); // play now
playEvents->add(ev);
}
stuckNotes->clear();
}
}
// p3.3.36
//int tickpos = audio->tickPos();
//bool extsync = extSyncFlag.value();
//
// Special for Jack midi devices: Play all Jack midi events up to curFrame.
//
for (iMidiDevice id = midiDevices.begin(); id != midiDevices.end(); ++id)
{
(*id)->processMidi();
/*
int port = md->midiPort();
MidiPort* mp = port != -1 ? &midiPorts[port] : 0;
MPEventList* el = md->playEvents();
if (el->empty())
continue;
iMPEvent i = md->nextPlayEvent();
for(; i != el->end(); ++i)
{
// If syncing to external midi sync, we cannot use the tempo map.
// Therefore we cannot get sub-tick resolution. Just use ticks instead of frames.
//if(i->time() > curFrame)
if(i->time() > (extsync ? tickpos : curFrame))
{
//printf(" curT %d frame %d\n", i->time(), curFrame);
break; // skip this event
}
if(mp)
{
if(mp->sendEvent(*i))
break;
}
else
{
if(md->putEvent(*i))
break;
}
}
md->setNextPlayEvent(i);
*/
}
midiBusy = false;
}
void CCmpPathfinder::ComputeShortPath(const IObstructionTestFilter& filter,
entity_pos_t x0, entity_pos_t z0, entity_pos_t r,
entity_pos_t range, const Goal& goal, pass_class_t passClass, Path& path)
{
UpdateGrid(); // TODO: only need to bother updating if the terrain changed
PROFILE("ComputeShortPath");
// ScopeTimer UID__(L"ComputeShortPath");
m_DebugOverlayShortPathLines.clear();
if (m_DebugOverlay)
{
// Render the goal shape
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = CColor(1, 0, 0, 1);
switch (goal.type)
{
case CCmpPathfinder::Goal::POINT:
{
SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), 0.2f, m_DebugOverlayShortPathLines.back(), true);
break;
}
case CCmpPathfinder::Goal::CIRCLE:
{
SimRender::ConstructCircleOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat(), m_DebugOverlayShortPathLines.back(), true);
break;
}
case CCmpPathfinder::Goal::SQUARE:
{
float a = atan2f(goal.v.X.ToFloat(), goal.v.Y.ToFloat());
SimRender::ConstructSquareOnGround(GetSimContext(), goal.x.ToFloat(), goal.z.ToFloat(), goal.hw.ToFloat()*2, goal.hh.ToFloat()*2, a, m_DebugOverlayShortPathLines.back(), true);
break;
}
}
}
// List of collision edges - paths must never cross these.
// (Edges are one-sided so intersections are fine in one direction, but not the other direction.)
std::vector<Edge> edges;
std::vector<Edge> edgesAA; // axis-aligned squares
// Create impassable edges at the max-range boundary, so we can't escape the region
// where we're meant to be searching
fixed rangeXMin = x0 - range;
fixed rangeXMax = x0 + range;
fixed rangeZMin = z0 - range;
fixed rangeZMax = z0 + range;
{
// (The edges are the opposite direction to usual, so it's an inside-out square)
Edge e0 = { CFixedVector2D(rangeXMin, rangeZMin), CFixedVector2D(rangeXMin, rangeZMax) };
Edge e1 = { CFixedVector2D(rangeXMin, rangeZMax), CFixedVector2D(rangeXMax, rangeZMax) };
Edge e2 = { CFixedVector2D(rangeXMax, rangeZMax), CFixedVector2D(rangeXMax, rangeZMin) };
Edge e3 = { CFixedVector2D(rangeXMax, rangeZMin), CFixedVector2D(rangeXMin, rangeZMin) };
edges.push_back(e0);
edges.push_back(e1);
edges.push_back(e2);
edges.push_back(e3);
}
// List of obstruction vertexes (plus start/end points); we'll try to find paths through
// the graph defined by these vertexes
std::vector<Vertex> vertexes;
// Add the start point to the graph
CFixedVector2D posStart(x0, z0);
fixed hStart = (posStart - NearestPointOnGoal(posStart, goal)).Length();
Vertex start = { posStart, fixed::Zero(), hStart, 0, Vertex::OPEN, QUADRANT_NONE, QUADRANT_ALL };
vertexes.push_back(start);
const size_t START_VERTEX_ID = 0;
// Add the goal vertex to the graph.
// Since the goal isn't always a point, this a special magic virtual vertex which moves around - whenever
// we look at it from another vertex, it is moved to be the closest point on the goal shape to that vertex.
Vertex end = { CFixedVector2D(goal.x, goal.z), fixed::Zero(), fixed::Zero(), 0, Vertex::UNEXPLORED, QUADRANT_NONE, QUADRANT_ALL };
vertexes.push_back(end);
const size_t GOAL_VERTEX_ID = 1;
// Add terrain obstructions
{
u16 i0, j0, i1, j1;
NearestTile(rangeXMin, rangeZMin, i0, j0);
NearestTile(rangeXMax, rangeZMax, i1, j1);
AddTerrainEdges(edgesAA, vertexes, i0, j0, i1, j1, r, passClass, *m_Grid);
}
// Find all the obstruction squares that might affect us
CmpPtr<ICmpObstructionManager> cmpObstructionManager(GetSimContext(), SYSTEM_ENTITY);
std::vector<ICmpObstructionManager::ObstructionSquare> squares;
cmpObstructionManager->GetObstructionsInRange(filter, rangeXMin - r, rangeZMin - r, rangeXMax + r, rangeZMax + r, squares);
// Resize arrays to reduce reallocations
vertexes.reserve(vertexes.size() + squares.size()*4);
edgesAA.reserve(edgesAA.size() + squares.size()); // (assume most squares are AA)
// Convert each obstruction square into collision edges and search graph vertexes
for (size_t i = 0; i < squares.size(); ++i)
{
CFixedVector2D center(squares[i].x, squares[i].z);
CFixedVector2D u = squares[i].u;
CFixedVector2D v = squares[i].v;
// Expand the vertexes by the moving unit's collision radius, to find the
// closest we can get to it
CFixedVector2D hd0(squares[i].hw + r + EDGE_EXPAND_DELTA, squares[i].hh + r + EDGE_EXPAND_DELTA);
CFixedVector2D hd1(squares[i].hw + r + EDGE_EXPAND_DELTA, -(squares[i].hh + r + EDGE_EXPAND_DELTA));
// Check whether this is an axis-aligned square
bool aa = (u.X == fixed::FromInt(1) && u.Y == fixed::Zero() && v.X == fixed::Zero() && v.Y == fixed::FromInt(1));
Vertex vert;
vert.status = Vertex::UNEXPLORED;
vert.quadInward = QUADRANT_NONE;
vert.quadOutward = QUADRANT_ALL;
vert.p.X = center.X - hd0.Dot(u); vert.p.Y = center.Y + hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_BR; vertexes.push_back(vert);
vert.p.X = center.X - hd1.Dot(u); vert.p.Y = center.Y + hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_TR; vertexes.push_back(vert);
vert.p.X = center.X + hd0.Dot(u); vert.p.Y = center.Y - hd0.Dot(v); if (aa) vert.quadInward = QUADRANT_TL; vertexes.push_back(vert);
vert.p.X = center.X + hd1.Dot(u); vert.p.Y = center.Y - hd1.Dot(v); if (aa) vert.quadInward = QUADRANT_BL; vertexes.push_back(vert);
// Add the edges:
CFixedVector2D h0(squares[i].hw + r, squares[i].hh + r);
CFixedVector2D h1(squares[i].hw + r, -(squares[i].hh + r));
CFixedVector2D ev0(center.X - h0.Dot(u), center.Y + h0.Dot(v));
CFixedVector2D ev1(center.X - h1.Dot(u), center.Y + h1.Dot(v));
CFixedVector2D ev2(center.X + h0.Dot(u), center.Y - h0.Dot(v));
CFixedVector2D ev3(center.X + h1.Dot(u), center.Y - h1.Dot(v));
if (aa)
{
Edge e = { ev1, ev3 };
edgesAA.push_back(e);
}
else
{
Edge e0 = { ev0, ev1 };
Edge e1 = { ev1, ev2 };
Edge e2 = { ev2, ev3 };
Edge e3 = { ev3, ev0 };
edges.push_back(e0);
edges.push_back(e1);
edges.push_back(e2);
edges.push_back(e3);
}
// TODO: should clip out vertexes and edges that are outside the range,
// to reduce the search space
}
ENSURE(vertexes.size() < 65536); // we store array indexes as u16
if (m_DebugOverlay)
{
// Render the obstruction edges
for (size_t i = 0; i < edges.size(); ++i)
{
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
std::vector<float> xz;
xz.push_back(edges[i].p0.X.ToFloat());
xz.push_back(edges[i].p0.Y.ToFloat());
xz.push_back(edges[i].p1.X.ToFloat());
xz.push_back(edges[i].p1.Y.ToFloat());
SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
}
for (size_t i = 0; i < edgesAA.size(); ++i)
{
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = CColor(0, 1, 1, 1);
std::vector<float> xz;
xz.push_back(edgesAA[i].p0.X.ToFloat());
xz.push_back(edgesAA[i].p0.Y.ToFloat());
xz.push_back(edgesAA[i].p0.X.ToFloat());
xz.push_back(edgesAA[i].p1.Y.ToFloat());
xz.push_back(edgesAA[i].p1.X.ToFloat());
xz.push_back(edgesAA[i].p1.Y.ToFloat());
xz.push_back(edgesAA[i].p1.X.ToFloat());
xz.push_back(edgesAA[i].p0.Y.ToFloat());
xz.push_back(edgesAA[i].p0.X.ToFloat());
xz.push_back(edgesAA[i].p0.Y.ToFloat());
SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), true);
}
}
// Do an A* search over the vertex/visibility graph:
// Since we are just measuring Euclidean distance the heuristic is admissible,
// so we never have to re-examine a node once it's been moved to the closed set.
// To save time in common cases, we don't precompute a graph of valid edges between vertexes;
// we do it lazily instead. When the search algorithm reaches a vertex, we examine every other
// vertex and see if we can reach it without hitting any collision edges, and ignore the ones
// we can't reach. Since the algorithm can only reach a vertex once (and then it'll be marked
// as closed), we won't be doing any redundant visibility computations.
PROFILE_START("A*");
PriorityQueue open;
PriorityQueue::Item qiStart = { START_VERTEX_ID, start.h };
open.push(qiStart);
u16 idBest = START_VERTEX_ID;
fixed hBest = start.h;
while (!open.empty())
{
// Move best tile from open to closed
PriorityQueue::Item curr = open.pop();
vertexes[curr.id].status = Vertex::CLOSED;
// If we've reached the destination, stop
if (curr.id == GOAL_VERTEX_ID)
{
idBest = curr.id;
break;
}
// Sort the edges so ones nearer this vertex are checked first by CheckVisibility,
// since they're more likely to block the rays
std::sort(edgesAA.begin(), edgesAA.end(), EdgeSort(vertexes[curr.id].p));
std::vector<EdgeAA> edgesLeft;
std::vector<EdgeAA> edgesRight;
std::vector<EdgeAA> edgesBottom;
std::vector<EdgeAA> edgesTop;
SplitAAEdges(vertexes[curr.id].p, edgesAA, edgesLeft, edgesRight, edgesBottom, edgesTop);
// Check the lines to every other vertex
for (size_t n = 0; n < vertexes.size(); ++n)
{
if (vertexes[n].status == Vertex::CLOSED)
continue;
// If this is the magical goal vertex, move it to near the current vertex
CFixedVector2D npos;
if (n == GOAL_VERTEX_ID)
{
npos = NearestPointOnGoal(vertexes[curr.id].p, goal);
// To prevent integer overflows later on, we need to ensure all vertexes are
// 'close' to the source. The goal might be far away (not a good idea but
// sometimes it happens), so clamp it to the current search range
npos.X = clamp(npos.X, rangeXMin, rangeXMax);
npos.Y = clamp(npos.Y, rangeZMin, rangeZMax);
}
else
{
npos = vertexes[n].p;
}
// Work out which quadrant(s) we're approaching the new vertex from
u8 quad = 0;
if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BL;
if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TR;
if (vertexes[curr.id].p.X <= npos.X && vertexes[curr.id].p.Y >= npos.Y) quad |= QUADRANT_TL;
if (vertexes[curr.id].p.X >= npos.X && vertexes[curr.id].p.Y <= npos.Y) quad |= QUADRANT_BR;
// Check that the new vertex is in the right quadrant for the old vertex
if (!(vertexes[curr.id].quadOutward & quad))
{
// Hack: Always head towards the goal if possible, to avoid missing it if it's
// inside another unit
if (n != GOAL_VERTEX_ID)
{
continue;
}
}
bool visible =
CheckVisibilityLeft(vertexes[curr.id].p, npos, edgesLeft) &&
CheckVisibilityRight(vertexes[curr.id].p, npos, edgesRight) &&
CheckVisibilityBottom(vertexes[curr.id].p, npos, edgesBottom) &&
CheckVisibilityTop(vertexes[curr.id].p, npos, edgesTop) &&
CheckVisibility(vertexes[curr.id].p, npos, edges);
/*
// Render the edges that we examine
m_DebugOverlayShortPathLines.push_back(SOverlayLine());
m_DebugOverlayShortPathLines.back().m_Color = visible ? CColor(0, 1, 0, 0.5) : CColor(1, 0, 0, 0.5);
std::vector<float> xz;
xz.push_back(vertexes[curr.id].p.X.ToFloat());
xz.push_back(vertexes[curr.id].p.Y.ToFloat());
xz.push_back(npos.X.ToFloat());
xz.push_back(npos.Y.ToFloat());
SimRender::ConstructLineOnGround(GetSimContext(), xz, m_DebugOverlayShortPathLines.back(), false);
//*/
if (visible)
{
fixed g = vertexes[curr.id].g + (vertexes[curr.id].p - npos).Length();
// If this is a new tile, compute the heuristic distance
if (vertexes[n].status == Vertex::UNEXPLORED)
{
// Add it to the open list:
vertexes[n].status = Vertex::OPEN;
vertexes[n].g = g;
vertexes[n].h = DistanceToGoal(npos, goal);
vertexes[n].pred = curr.id;
// If this is an axis-aligned shape, the path must continue in the same quadrant
// direction (but not go into the inside of the shape).
// Hack: If we started *inside* a shape then perhaps headed to its corner (e.g. the unit
// was very near another unit), don't restrict further pathing.
if (vertexes[n].quadInward && !(curr.id == START_VERTEX_ID && g < fixed::FromInt(8)))
vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF;
if (n == GOAL_VERTEX_ID)
vertexes[n].p = npos; // remember the new best goal position
PriorityQueue::Item t = { (u16)n, g + vertexes[n].h };
open.push(t);
// Remember the heuristically best vertex we've seen so far, in case we never actually reach the target
if (vertexes[n].h < hBest)
{
idBest = (u16)n;
hBest = vertexes[n].h;
}
}
else // must be OPEN
{
// If we've already seen this tile, and the new path to this tile does not have a
// better cost, then stop now
if (g >= vertexes[n].g)
continue;
// Otherwise, we have a better path, so replace the old one with the new cost/parent
vertexes[n].g = g;
vertexes[n].pred = curr.id;
// If this is an axis-aligned shape, the path must continue in the same quadrant
// direction (but not go into the inside of the shape).
if (vertexes[n].quadInward)
vertexes[n].quadOutward = ((~vertexes[n].quadInward) & quad) & 0xF;
if (n == GOAL_VERTEX_ID)
vertexes[n].p = npos; // remember the new best goal position
open.promote((u16)n, g + vertexes[n].h);
}
}
}
}
// Reconstruct the path (in reverse)
for (u16 id = idBest; id != START_VERTEX_ID; id = vertexes[id].pred)
{
Waypoint w = { vertexes[id].p.X, vertexes[id].p.Y };
path.m_Waypoints.push_back(w);
}
PROFILE_END("A*");
}
int main(void) {
gslpp::complex zi = gslpp::complex::i();
std::vector<double> sd = {10., 5., 1.};
std::vector<gslpp::complex> sc = {30. + zi, 2. + 3. * zi, 1. + 4. * zi};
gslpp::matrix<double> md1(2, 2);
md1(0, 0) = 10.;
md1(1, 0) = 20.;
md1(0, 1) = 5.;
md1(1, 1) = 1.;
gslpp::matrix<double> md2(2, 2);
md2(0, 0) = -3.;
md2(1, 0) = 30.;
md2(0, 1) = -5.;
md2(1, 1) = 4.;
gslpp::matrix<gslpp::complex> mc1(2, 2);
mc1.assign(0, 0, 9. + 2 * zi);
mc1.assign(1, 0, 19. - 4 * zi);
mc1.assign(0, 1, 4. - 6 * zi);
mc1.assign(1, 1, 3. + 2 * zi);
gslpp::matrix<gslpp::complex> mc2(2, 2);
mc2.assign(0, 0, -8. + 3 * zi);
mc2.assign(1, 0, 11. - 5 * zi);
mc2.assign(0, 1, 2. + 5 * zi);
mc2.assign(1, 1, -3. + 4 * zi);
gslpp::vector<double> vd1(2);
gslpp::vector<double> vd2(2);
vd1(0) = 14.;
vd1(1) = -5;
vd2(0) = -9.;
vd2(1) = 3;
gslpp::vector<gslpp::complex> vc1(2);
gslpp::vector<gslpp::complex> vc2(2);
vc1.assign(0, 4. - 2. * zi);
vc1.assign(1, 6. - 8. * zi);
vc2.assign(0, 5. + 3. * zi);
vc2.assign(1, 4. - 12. * zi);
Expanded<double> esd(sd);
Expanded<gslpp::complex> esc(sc);
std::vector<gslpp::matrix<double> > mdv = {md1, md2};
Expanded<gslpp::matrix<double> > emd(mdv);
std::vector<gslpp::matrix<gslpp::complex> > mcv = {mc1, mc2};
Expanded<gslpp::matrix<gslpp::complex> > emc(mcv);
std::vector<gslpp::vector<double> > vdv = {vd1, vd2};
Expanded<gslpp::vector<double> > evd(vdv);
std::vector<gslpp::vector<gslpp::complex> > vcv = {vc1, vc2};
Expanded<gslpp::vector<gslpp::complex> > evc(vcv);
// Print Input
std::cout << std::endl;
std::cout << "esd " << esd << std::endl;
std::cout << "esc " << esc << std::endl;
std::cout << "evd " << evd << std::endl;
std::cout << "evc " << evc << std::endl;
std::cout << "emd " << emd << std::endl;
std::cout << "emc " << emc << std::endl;
std::cout << "-------------" << std::endl;
std::cout << "-sd " << -esd << std::endl;
std::cout << "-sc " << -esc << std::endl;
std::cout << "-vd " << -evd << std::endl;
std::cout << "-vc " << -evc << std::endl;
std::cout << "-md " << -emd << std::endl;
std::cout << "-mc " << -emc << std::endl;
std::cout << "-------------" << std::endl;
// Expanded * Expanded
std::cout << "sd*sd " << esd * esd << std::endl;
std::cout << "sd*sc " << esd * esc << std::endl;
std::cout << "sd*vd " << esd * evd << std::endl;
std::cout << "sd*vc " << esd * evc << std::endl;
std::cout << "sd*md " << esd * emd << std::endl;
std::cout << "sd*mc " << esd * emc << std::endl;
std::cout << "sc*sd " << esc * esd << std::endl;
std::cout << "sc*sc " << esc * esc << std::endl;
std::cout << "sc*vd " << esc * evd << std::endl;
std::cout << "sc*vc " << esc * evc << std::endl;
std::cout << "sc*md " << esc * emd << std::endl;
std::cout << "sc*mc " << esc * emc << std::endl;
std::cout << "vd*sd " << evd * esd << std::endl;
std::cout << "vd*sc " << evd * esc << std::endl;
std::cout << "vd*vd " << evd * evd << std::endl;
std::cout << "vd*vc " << evd * evc << std::endl;
std::cout << "vd*md " << evd * emd << std::endl;
std::cout << "vd*mc " << evd * emc << std::endl;
std::cout << "vc*sd " << evc * esd << std::endl;
std::cout << "vc*sc " << evc * esc << std::endl;
std::cout << "vc*vd " << evc * evd << std::endl;
std::cout << "vc*vc " << evc * evc << std::endl;
std::cout << "vc*md " << evc * emd << std::endl;
std::cout << "vc*mc " << evc * emc << std::endl;
std::cout << "md*sd " << emd * esd << std::endl;
std::cout << "md*sc " << emd * esc << std::endl;
std::cout << "md*vd " << emd * evd << std::endl;
std::cout << "md*vc " << emd * evc << std::endl;
std::cout << "md*md " << emd * emd << std::endl;
std::cout << "md*mc " << emd * emc << std::endl;
std::cout << "mc*sd " << emc * esd << std::endl;
std::cout << "mc*sc " << emc * esc << std::endl;
std::cout << "mc*vd " << emc * evd << std::endl;
std::cout << "mc*vc " << emc * evc << std::endl;
std::cout << "mc*md " << emc * emd << std::endl;
std::cout << "mc*mc " << emc * emc << std::endl;
std::cout << "-------------" << std::endl;
// Expanded + Expanded
std::cout << "sd + sd " << esd + esd << std::endl;
std::cout << "sd + sc " << esd + esc << std::endl;
std::cout << "sc + sd " << esc + esd << std::endl;
std::cout << "sc + sc " << esc + esc << std::endl;
std::cout << "vd + vd " << evd + evd << std::endl;
std::cout << "vd + vc " << evd + evc << std::endl;
std::cout << "vc + vd " << evc + evd << std::endl;
std::cout << "vc + vc " << evc + evc << std::endl;
std::cout << "md + md " << emd + emd << std::endl;
std::cout << "md + mc " << emd + emc << std::endl;
std::cout << "mc + md " << emc + emd << std::endl;
std::cout << "mc + mc " << emc + emc << std::endl;
//
std::cout << "-------------" << std::endl;
// Expanded - Expanded
std::cout << "sd - sd " << esd - esd << std::endl;
std::cout << "sd - sc " << esd - esc << std::endl;
std::cout << "sc - sd " << esc - esd << std::endl;
std::cout << "sc - sc " << esc - esc << std::endl;
std::cout << "vd - vd " << evd - evd << std::endl;
std::cout << "vd - vc " << evd - evc << std::endl;
std::cout << "vc - vd " << evc - evd << std::endl;
std::cout << "vc - vc " << evc - evc << std::endl;
std::cout << "md - md " << emd - emd << std::endl;
std::cout << "md - mc " << emd - emc << std::endl;
std::cout << "mc - md " << emc - emd << std::endl;
std::cout << "mc - mc " << emc - emc << std::endl;
std::cout << "-------------" << std::endl;
// Expanded * UnExpanded
std::cout << "esd*5 = " << esd * 5. << std::endl;
std::cout << "esd*(4-2I) = " << esd * (4. - 2. * zi) << std::endl;
std::cout << "esd*vd1 = " << esd * vd1 << std::endl;
std::cout << "esd*vc1 = " << esd * vc1 << std::endl;
std::cout << "esd*md1 = " << esd * md1 << std::endl;
std::cout << "esd*mc1 = " << esd * mc1 << std::endl;
//
std::cout << "esc*5 = " << esc * 5. << std::endl;
std::cout << "esc*(4-2I) = " << esc * (4. - 2. * zi) << std::endl;
std::cout << "esc*vd1 = " << esc * vd1 << std::endl;
std::cout << "esc*vc1 = " << esc * vc1 << std::endl;
std::cout << "esc*md1 = " << esc * md1 << std::endl;
std::cout << "esc*mc1 = " << esc * mc1 << std::endl;
//
std::cout << "evd*5 = " << evd * 5. << std::endl;
std::cout << "evd*(4-2I) = " << evd * (4. - 2. * zi) << std::endl;
std::cout << "evd*vd1 = " << evd * vd1 << std::endl;
std::cout << "evd*vc1 = " << evd * vc1 << std::endl;
std::cout << "evd*md1 = " << evd * md1 << std::endl;
std::cout << "evd*mc1 = " << evd * mc1 << std::endl;
//
std::cout << "evc*5 = " << evc * 5. << std::endl;
std::cout << "evc*(4-2I) = " << evc * (4. - 2. * zi) << std::endl;
std::cout << "evc*vd1 = " << evc * vd1 << std::endl;
std::cout << "evc*vc1 = " << evc * vc1 << std::endl;
std::cout << "evc*md1 = " << evc * md1 << std::endl;
std::cout << "evc*mc1 = " << evc * mc1 << std::endl;
//
std::cout << "emd*5 = " << emd * 5. << std::endl;
std::cout << "emd*(4-2I) = " << emd * (4. - 2. * zi) << std::endl;
std::cout << "emd*vd1 = " << emd * vd1 << std::endl;
std::cout << "emd*vc1 = " << emd * vc1 << std::endl;
std::cout << "emd*md1 = " << emd * md1 << std::endl;
std::cout << "emd*mc1 = " << emd * mc1 << std::endl;
//
std::cout << "emc*5 = " << emc * 5. << std::endl;
std::cout << "emc*(4-2I) = " << emc * (4. - 2. * zi) << std::endl;
std::cout << "emc*vd1 = " << emc * vd1 << std::endl;
std::cout << "emc*vc1 = " << emc * vc1 << std::endl;
std::cout << "emc*md1 = " << emc * md1 << std::endl;
std::cout << "emc*mc1 = " << emc * mc1 << std::endl;
std::cout << "-------------" << std::endl;
// // Expanded / UnExpanded-Scalar
std::cout << "esd/5 = " << esd / 5. << std::endl;
std::cout << "esd/(4-2I) = " << esd / (4. - 2. * zi) << std::endl;
std::cout << "esc/5 = " << esc / 5. << std::endl;
std::cout << "esc/(4-2I) = " << esc / (4. - 2. * zi) << std::endl;
std::cout << "evd/5 = " << evd / 5. << std::endl;
std::cout << "evd/(4-2I) = " << evd / (4. - 2. * zi) << std::endl;
std::cout << "evc/5 = " << evc / 5. << std::endl;
std::cout << "evc/(4-2I) = " << evc / (4. - 2. * zi) << std::endl;
std::cout << "emd/5 = " << emd / 5. << std::endl;
std::cout << "emd/(4-2I) = " << emd / (4. - 2. * zi) << std::endl;
std::cout << "emc/5 = " << emc / 5. << std::endl;
std::cout << "emc/(4-2I) = " << emc / (4. - 2. * zi) << std::endl;
std::cout << "-------------" << std::endl;
// UnExpanded * Expanded easy-check (must be 0)
std::cout << "zero1 = " << 5. * esd - esd * 5. << std::endl;
std::cout << "zero1 = " << 5. * esc - esc * 5. << std::endl;
std::cout << "zero1 = " << 5. * evd - evd * 5. << std::endl;
std::cout << "zero1 = " << 5. * evc - evc * 5. << std::endl;
std::cout << "zero1 = " << 5. * emd - emd * 5. << std::endl;
std::cout << "zero1 =" << 5. * emc - emc * 5. << std::endl;
//
std::cout << "zero2 = " << (4 - 2 * zi) * esd - esd * (4 - 2 * zi) << std::endl;
std::cout << "zero2 = " << (4 - 2 * zi) * esc - esc * (4 - 2 * zi) << std::endl;
std::cout << "zero2 = " << (4 - 2 * zi) * evd - evd * (4 - 2 * zi) << std::endl;
std::cout << "zero2 = " << (4 - 2 * zi) * evc - evc * (4 - 2 * zi) << std::endl;
std::cout << "zero2 = " << (4 - 2 * zi) * emd - emd * (4 - 2 * zi) << std::endl;
std::cout << "zero2 = " << (4 - 2 * zi) * emc - emc * (4 - 2 * zi) << std::endl;
std::cout << "zero3 = " << vd1 * esd - esd * vd1 << std::endl;
std::cout << "zero3 = " << vd1 * esc - esc * vd1 << std::endl;
std::cout << "zero3 = " << vd1 * evd - evd * vd1 << std::endl;
std::cout << "zero3 = " << vd1 * evc - evc * vd1 << std::endl;
std::cout << "zero3 = " << vd1 * emd - emd.transpose() * vd1 << std::endl;
std::cout << "zero3 = " << vd1 * emc - emc.transpose() * vd1 << std::endl;
//
std::cout << "zero4 = " << vc1 * esd - esd * vc1 << std::endl;
std::cout << "zero4 = " << vc1 * esc - esc * vc1 << std::endl;
std::cout << "zero4 = " << vc1 * evd - evd * vc1 << std::endl;
std::cout << "zero4 = " << vc1 * evc - evc * vc1 << std::endl;
std::cout << "zero4 = " << vc1 * emd - emd.transpose() * vc1 << std::endl;
std::cout << "zero4 = " << vc1 * emc - emc.transpose() * vc1 << std::endl;
//
std::cout << "zero5 = " << md1 * esd - esd * md1 << std::endl;
std::cout << "zero5 = " << md1 * esc - esc * md1 << std::endl;
std::cout << "zero5 = " << md1 * evd - evd * md1.transpose() << std::endl;
std::cout << "zero5 = " << md1 * evc - evc * md1.transpose() << std::endl;
std::cout << "zero5 = " << md1 * emd - (emd.transpose() * md1.transpose()).transpose() << std::endl;
std::cout << "zero5 = " << md1 * emc - (emc.transpose() * md1.transpose()).transpose() << std::endl;
//
std::cout << "zero6 = " << mc1 * esd - esd * mc1 << std::endl;
std::cout << "zero6 = " << mc1 * esc - esc * mc1 << std::endl;
std::cout << "zero6 = " << mc1 * evd - evd * mc1.transpose() << std::endl;
std::cout << "zero6 = " << mc1 * evc - evc * mc1.transpose() << std::endl;
std::cout << "zero6 = " << mc1 * emd - (emd.transpose() * mc1.transpose()).transpose() << std::endl;
std::cout << "zero6 = " << mc1 * emc - (emc.transpose() * mc1.transpose()).transpose() << std::endl;
std::cout << "true = " << (esd == esd) << std::endl;
std::cout << "true = " << (esc == esc) << std::endl;
std::cout << "true = " << (evd == evd) << std::endl;
std::cout << "true = " << (evc == evc) << std::endl;
std::cout << "true = " << (emd == emd) << std::endl;
std::cout << "true = " << (emc == emc) << std::endl;
std::cout << "false = " << (esd == esd * esd) << std::endl;
std::cout << "false = " << (esc == esc * esc) << std::endl;
std::cout << "false = " << (evd == 5 * evd) << std::endl;
std::cout << "false = " << (evc == 7 * evc) << std::endl;
std::cout << "false = " << (emd == emd * emd) << std::endl;
std::cout << "false = " << (emc == emc * emc) << std::endl;
Expanded<double> esd1(sd, 2);
std::vector<double> sdx = {10., 5., 1.,-7};
Expanded<double> esd2(sdx, 1);
std::cout << "esd1 * esd2 = " << esd1*esd2 << std::endl;
std::cout << "esd1 + esd2 = " << esd1+esd2 << std::endl;
std::cout << "esd1 - esd2 = " << esd1-esd2 << std::endl;
std::vector<double> q1={1.,0.};
std::vector<double> q2={5.,7.};
std::vector<std::vector<double> > v1 = {q1,q2};
std::vector<double> w1={3.,4.};
std::vector<double> w2={8.};
std::vector<std::vector<double> > v2 = {w1,w2};
Expanded<double> ev1(v1, 1, 0);
Expanded<double> ev2(v2);
std::cout << "ev1 = " << ev1 << std::endl;
std::cout << "ev2 = " << ev2 << std::endl;
std::cout << "ev1 * ev2 = " << ev1*ev2 << std::endl;
std::cout << "ev1 + ev2 = " << ev1+ev2 << std::endl;
std::cout << "1/ev1 = " << ev1.inverse() << std::endl;
std::cout << "1/ev2 = " << ev2.inverse() << std::endl;
complex ii(0, 1);
std::vector<complex> r1={1.+2.*ii, 3.+4.*ii};
std::vector<complex> r2={5. + 6.*ii, 7.+8.*ii, 9.+10.*ii};
std::vector<std::vector<complex> > vv1 = {r1,r2};
Expanded<complex> evv1(vv1, 1, 1);
std::cout << "evv1 = " << evv1 << std::endl;
std::cout << "1/evv1 = " << evv1.inverse() << std::endl;
std::cout << "ev1/evv1 = " << ev1/evv1 << std::endl;
std::cout << "evv1/ev1 = " << evv1/ev1 << std::endl;
Expanded<complex> tmp(evv1.truncate(std::vector<int>(2,2),2));
std::cout << "evv1 trunc at (std::vector<int>(2,2),2)" << tmp << std::endl;
std::cout << "evv1 series = " << evv1.Series(std::vector<int>(2,2),2) << std::endl;
Expanded<double> pp;
return (0);
}
