public override void OnEnable()
{
useFormulae = false;
arb = target as Arbiter;
teamScroll = Vector2.zero;
base.OnEnable();
name = "Arbiter";
}
public vector.VectorState get()
{
vector.VectorState ret = null;
Arbiter.ExecuteToCompletion(DssEnvironment.TaskQueue,
Arbiter.Choice <vector.VectorState, Fault>(
opPort.Get(),
delegate(vector.VectorState state)
{
ret = state;
},
delegate(Fault failure)
{
throw new AdapterOperationException(failure);
}));
return(ret);
}
public void ArbiterRemoved(Arbiter arbiter)
{
arbiter.body1.arbiters.Remove(arbiter);
arbiter.body2.arbiters.Remove(arbiter);
if (arbiter.body1.island != null)
{
arbiter.body1.island.arbiter.Remove(arbiter);
}
else if (arbiter.body2.island != null)
{
arbiter.body2.island.arbiter.Remove(arbiter);
}
RemoveConnection(arbiter.body1, arbiter.body2);
}
IEnumerator <ITask> ReliableSubscribeHandler(ReliableSubscribe subscribe)
{
yield return(Arbiter.Choice(
SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
if (_state != null &&
_state.DistanceMeasurements != null)
{
_subMgrPort.Post(new submgr.Submit(
subscribe.Body.Subscriber, DsspActions.ReplaceRequest, _state, null));
}
},
null
));
}
public IEnumerator <ITask> ReliableSubscribeHandler(drive.ReliableSubscribe subscribe)
{
yield return(Arbiter.Choice(
SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
_subMgrPort.Post(new submgr.Submit(
subscribe.Body.Subscriber, DsspActions.UpdateRequest, _state, null));
},
delegate(Exception ex)
{
LogError(ex);
throw ex;
}
));
}
/// <summary>
/// Start the control window
/// </summary>
private void SetupWindowControl()
{
// TODO Arbiter.Interleave?
// Start handlers handling the different window events allowing the communication between simulator and window
Activate <ITask>(
Arbiter.ReceiveWithIterator <OnLoad>(true, _windowControlEventsPort, OnWindowControlLoadHandler),
Arbiter.Receive <OnClosed>(true, _windowControlEventsPort, OnWindowControlClosedHandler),
Arbiter.Receive <WindowControl.OnVirtualJoystickChanged>(true, _windowControlEventsPort, OnWindowControlVirtualJoystickHandler),
Arbiter.Receive <WindowControl.OnAction>(true, _windowControlEventsPort, OnWindowControlActionHandler),
Arbiter.Receive <WindowControl.OnValueChanged>(true, _windowControlEventsPort, OnWindowControlValueChangedHandler),
Arbiter.Receive <WindowControl.OnUpdateDisplay>(true, _windowControlEventsPort, OnWindowControlUpdateDisplayHandler)
);
// Start the control window
WinFormsServicePort.Post(new RunForm(CreateWindowControl));
}
/// <summary>
/// Service Start
/// </summary>
protected override void Start()
{
InitializeState();
base.Start();
base.MainPortInterleave.CombineWith(new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive <brick.LegoSensorUpdate>(true, _legoBrickNotificationPort, NotificationHandler)
),
new ConcurrentReceiverGroup()));
// Set up the reliable port using DSS forwarder to ensure exception and timeout conversion to fault.
_reliableMainPort = ServiceForwarder <ColorSensorOperations>(this.ServiceInfo.Service);
_reliableMainPort.ConnectToBrick(_state);
}
/// <summary>
/// Service Start
/// </summary>
protected override void Start()
{
base.Start();
// setup internal notifications when we receive a OnWiimoteChanged event
Activate(Arbiter.Receive(true, _internalStateChangedPort, InternalWiimoteChangedHandler));
// hookup event handlers
_wm.WiimoteChanged += new EventHandler <WiimoteChangedEventArgs>(_wm_WiimoteChanged);
_wm.WiimoteExtensionChanged += new EventHandler <WiimoteExtensionChangedEventArgs>(_wm_WiimoteExtensionChanged);
// connect to wiimote and setup standard params
_wm.Connect();
_wm.SetReportType(InputReport.IRAccel, true);
_wm.SetLEDs(false, true, false, true);
}
public void ArbiterCreated(Arbiter arbiter)
{
AddConnection(arbiter.body1, arbiter.body2);
arbiter.body1.arbiters.Add(arbiter);
arbiter.body2.arbiters.Add(arbiter);
if (arbiter.body1.island != null)
{
arbiter.body1.island.arbiter.Add(arbiter);
}
else if (arbiter.body2.island != null)
{
arbiter.body2.island.arbiter.Add(arbiter);
}
}
// Send an update message to ourself at an interval specified by the camera entity.
// If the interval is 0, check once per second to see if the interval has changed but
// don't do an update.
void CheckForUpdate(DateTime time)
{
if (_entity == null)
{
return; // the entity went away, do no more updates
}
if (_entity.UpdateInterval != 0)
{
PostUpdateFrameIfNeeded();
Activate(Arbiter.Receive(false, TimeoutPort(_entity.UpdateInterval), CheckForUpdate));
}
else
{
Activate(Arbiter.Receive(false, TimeoutPort(1000), CheckForUpdate));
}
}
// Runs down the timer then reports the score to
// Arbiter if this is not a practice round
////////////////////////////////////////////////
void Update()
{
if (timeRemaining > 0)
{
RunTimer();
}
else
{
gameOver = true;
if (gameState.practiceMode == false && scoreReported == false)
{
scoreReported = true;
Arbiter.ReportScoreForChallenge(gameState.challenge.Id, score.ToString(), OnReportScoreSuccess, OnReportScoreError);
}
}
}
protected override void Start()
{
Port <bool> successPort = new Port <bool>();
SpawnIterator(successPort, StartIterator);
Activate(Arbiter.Receive(false, successPort,
delegate(bool success)
{
if (!success)
{
LogError("Service failed to start. Shutting down.");
Shutdown();
}
}
));
}
/// <summary>
/// Service Start
/// </summary>
protected override void Start()
{
#region CODECLIP 01-2
// The initial state is optional, so we must be prepared to
// create a new state if there is none
if (_state == null)
{
_state = new ServiceTutorial3State();
}
#endregion
base.Start();
_timerPort.Post(DateTime.Now);
Activate(Arbiter.Receive(true, _timerPort, TimerHandler));
}
private void FindTarget()
{
if (HasBeenInitialized)
{
var query = new VisualEntity();
query.State.Name = _targetName;
Activate(
Arbiter.Choice(
SimulationEngine.GlobalInstancePort.Query(query),
success => Target = success.Entity,
CcrServiceBase.EmptyHandler
)
);
}
}
/// <summary>
/// Shutdown or kill process
/// </summary>
/// <returns>ITask enumerator</returns>
private IEnumerator <ITask> ShutdownSuccessfullyOrKillProcess()
{
var shutdownOrTimedOut = new SuccessFailurePort();
Activate(TimeoutPort(DefaultProcessTimeout).Receive(dateTime => shutdownOrTimedOut.Post(new Exception())));
Activate(shutdownOrTimedOut.Choice(
success => { }, /*cleanly shutdown*/
failure => /*timed out*/ System.Diagnostics.Process.GetCurrentProcess().Kill()));
ControlPanelPort.Post(new DsspDefaultDrop());
Activate(Arbiter.ReceiveWithIterator <DsspDefaultDrop>(false, this.mainPort, this.DropHandler));
shutdownOrTimedOut.Post(SuccessResult.Instance);
yield break;
}
public drive.DriveDifferentialTwoWheelState get()
{
drive.DriveDifferentialTwoWheelState ret = null;
Arbiter.ExecuteToCompletion(DssEnvironment.TaskQueue,
Arbiter.Choice <drive.DriveDifferentialTwoWheelState, Fault>(
drivePort.Get(),
delegate(drive.DriveDifferentialTwoWheelState state)
{
ret = state;
},
delegate(Fault failure)
{
throw new AdapterOperationException(failure);
}));
return(ret);
}
void ActivateHandler()
{
Activate(
Arbiter.Interleave(
new TeardownReceiverGroup(
Arbiter.Receive <Shutdown>(false, _port, ShutdownHandler)
),
new ExclusiveReceiverGroup(
Arbiter.Receive(true, _timePort, TimeHandler)
),
new ConcurrentReceiverGroup()
)
);
TimeHandler(DateTime.Now);
}
////20101209
//void ThreadHeartBeatProc()
//{
// SpawnIterator(SendHeartBeatToClient);
//}
//20101209
public IEnumerator <ITask> SendHeartBeatToClient()
{
client.HeartBeat heartBeat = new client.HeartBeat();
heartBeat.Body.HeartBeat = "heart broken...";
//对连接上的客户端进行心跳检测，但是心跳检测的最大队伍数即比赛类型所允许的参赛队伍数 modified 20110218
for (int i = 0; i < Math.Min(_listClientUris.Count, MyMission.Instance().ParasRef.TeamCount); i++)
{
ServiceForwarder <client.ClientBaseOperations>(_listClientUris[i]).Post(heartBeat);
yield return(Arbiter.Choice(
heartBeat.ResponsePort,
delegate(client.HeartBeatResponse rsp)
{
Console.WriteLine("Successful");
},
delegate(Fault f)
{
Console.WriteLine("Team{0} failed", i + 1);
WinFormsServicePort.FormInvoke(delegate()
{
_serverControlBoard.ShowDialogInfo(string.Format("Team{0} Connection Failed", i + 1));
});
MyMission myMission = MyMission.Instance();
InitMission(myMission.ParasRef.Name, myMission.ParasRef.TotalSeconds / 60);
//// 发送比赛类型改变事件消息，以当前选中比赛类型名称和比赛时间分钟数作为参数
//FromServerUiEvents.Instance().Post(new FromServerUiMsg(FromServerUiMsg.MsgEnum.COMPETITION_ITEM_CHANGED,
// new string[] { myMission.ParasRef.Name, (myMission.ParasRef.TotalSeconds / 60).ToString() }));
//CompetitionControlButton announceCompetitionControlButton = new CompetitionControlButton();
//announceCompetitionControlButton.Body = new CompetitionControlButtonRequest("ID Changed");
//_mainPort.Post(announceCompetitionControlButton); //将第i只队伍断线的信息发送给其他客户端
RemoveUnresponsiveClient(ref i);
}));
}
if (_listClientUris.Count != 0)
{
Activate(Arbiter.Receive(false, TimeoutPort(20 * (MyMission.Instance().ParasRef.MsPerCycle)), SendHeartBeat));
}
yield break;
}
static void ParallelMul()
{
// создание массива объектов для хранения параметров
InputData[] ClArr = new InputData[nc];
for (int i = 0; i < nc; i++)
{
ClArr[i] = new InputData();
}
//Далее, задаются исходные данные для каждого экземпляра
//вычислительного метода:
// делим количество строк в матрице на nc частей
int step = (Int32)(m / nc);
// заполняем массив параметров
int c = -1;
for (int i = 0; i < nc; i++)
{
ClArr[i].start = c + 1;
ClArr[i].stop = c + step;
c = c + step;
}
//Создаётся диспетчер с пулом из двух потоков:
Dispatcher d = new Dispatcher(nc, "Test Pool");
DispatcherQueue dq = new DispatcherQueue("Test Queue", d);
//Описывается порт, в который каждый экземпляр метода Mul()
//отправляет сообщение после завершения вычислений:
Port <int> p = new Port <int>();
//Метод Arbiter.Activate помещает в очередь диспетчера две задачи(два
//экземпляра метода Mul):
for (int i = 0; i < nc; i++)
{
Arbiter.Activate(dq, new Task <InputData, Port <int> >(ClArr[i], p, Mul));
}
//Первый параметр метода Arbiter.Activate – очередь диспетчера,
//который будет управлять выполнением задачи, второй параметр –
//запускаемая задача.
//С помощью метода Arbiter.MultipleItemReceive запускается задача
//(приёмник), которая обрабатывает получение двух сообщений портом p:
Arbiter.Activate(dq, Arbiter.MultipleItemReceive(true, p, nc, delegate(int[] array) {
dispResult();
Console.WriteLine("Вычисления завершены");
Console.ReadKey(true);
Environment.Exit(0);
}));
}
protected override void Start()
{
//configure initial state
if (_state == null)
{
LogInfo("TrackRoamerBumper:Start(): _state == null - initializing...");
_state = new pxbumper.ContactSensorArrayState();
_state.Sensors = new List<pxbumper.ContactSensor>();
pxbumper.ContactSensor leftBumper = new pxbumper.ContactSensor();
leftBumper.HardwareIdentifier = 101;
leftBumper.Name = "Front Whisker Left";
_state.Sensors.Add(leftBumper);
pxbumper.ContactSensor rightBumper = new pxbumper.ContactSensor();
rightBumper.HardwareIdentifier = 201;
rightBumper.Name = "Front Whisker Right";
_state.Sensors.Add(rightBumper);
SaveState(_state);
}
else
{
LogInfo("TrackRoamerBumper:Start(): _state is supplied by file: " + _configFile);
}
base.Start();
MainPortInterleave.CombineWith(
new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive<powerbrick.UpdateWhiskers>(true, notificationPortWhiskers, WhiskersNotificationHandler)
),
new ConcurrentReceiverGroup()
)
);
// display HTTP service Uri
LogInfo("TrackRoamerBumper:Start() Service URI=" + ServiceInfo.HttpUri());
// Subscribe to the Hardware Controller for bumper notifications
SubscribeToTrackRoamerBot();
}
/// <summary>
/// Initializes this instance.
/// </summary>
/// <returns>Standard ccr iterator</returns>
private IEnumerator <ITask> Initialize()
{
var located = new PortSet <VisualEntity, Fault>();
SpawnIterator(located, this.LocateCameraEntity);
yield return(located.Choice());
var entity = (VisualEntity)located;
if (entity == null)
{
LogError("Kinect entity not found");
StartFailed();
yield break;
}
this.kinectEntity = (KinectEntity)entity;
if (this.state == null)
{
this.state = new kinect.KinectState
{
DepthImageFormat = DepthImageFormat.Resolution320x240Fps30,
FrameRate = MaxFramesPerSec,
IsDepthServiceUpdateEnabled = true,
IsWebCamServiceUpdateEnabled = true,
UseColor = true,
UseDepth = true,
UseSkeletalTracking = false,
ColorImageFormat = ColorImageFormat.RgbResolution640x480Fps30
};
SaveState(this.state);
}
this.panTiltState = InitialPanTiltState();
base.Start();
MainPortInterleave.CombineWith(
new Interleave(
new ExclusiveReceiverGroup(
Arbiter.ReceiveWithIterator(true, this.pollPort, this.DrainPendingRequests)),
new ConcurrentReceiverGroup()));
this.kinectOps.Post(new kinect.SetFrameRate(new kinect.SetFrameRateRequest(this.state.FrameRate)));
}
/// <summary>
/// Convert xml files to binary
/// </summary>
/// <param name="logFile">File to be converted</param>
/// <param name="newLogFile">New file name</param>
/// <param name="resultPort">DSS result port</param>
/// <returns>ITask enumerator</returns>
private IEnumerator <ITask> ConvertXmlLogToBinary(string logFile, string newLogFile, Port <LogFileResult> resultPort)
{
var envelopes = new List <Envelope>(DefaultInitialEnvelopeCapacity);
using (var fs = new FileStream(logFile, FileMode.Open, FileAccess.Read, FileShare.None, 1))
{
if (fs.Length != 0)
{
using (var newFs = new FileStream(newLogFile, FileMode.Create, FileAccess.Write, FileShare.None, 1))
{
using (var bw = new BinaryWriter(newFs))
{
var reader = XmlReader.Create(fs);
while (ReadToNextEnvelopeNode(reader))
{
var deserHeader = new Deserialize(reader);
SerializerPort.Post(deserHeader);
DeserializeResult headerDeserResult = null;
yield return(Arbiter.Choice(
deserHeader.ResultPort,
w3cHeader => headerDeserResult = w3cHeader,
failure => LogError(failure)));
if (headerDeserResult != null && ReadToNextBodyNode(reader))
{
var deserBody = new Deserialize(reader);
SerializerPort.Post(deserBody);
DeserializeResult bodyDeserResult = null;
yield return(Arbiter.Choice(
deserBody.ResultPort,
bodyContents => bodyDeserResult = bodyContents,
failure => LogError(failure)));
if (bodyDeserResult != null)
{
this.SerializeToBinary(headerDeserResult, bodyDeserResult, bw);
}
}
}
}
}
}
}
}
/// <summary>
/// Service Start
/// </summary>
protected override void Start()
{
// Alternate Port Operations handlers
Activate(Arbiter.ReceiveWithIterator <led.Get>(true, _alternatePort, AlternateGetHandler));
// Alternate Port Operations handlers
Activate(Arbiter.ReceiveWithIterator <led.SetSingle>(true, _alternatePort, AlternateSetSingleHandler));
// Alternate Port Operations handlers
Activate(Arbiter.ReceiveWithIterator <led.SetVector>(true, _alternatePort, AlternateSetVectorHandler));
led.LEDVector ledvec = new led.LEDVector();
List <led.LEDVector> leds = new List <led.LEDVector>(1);
//configure default state
if (_alternatestate == null)
{
ledvec.LEDVec = new List <led.LED>(3);
ledvec.LEDVec.Add(new led.LED());
ledvec.LEDVec.Add(new led.LED());
ledvec.LEDVec.Add(new led.LED());
_state = new ScribblerLEDArrayState();
_state.LEDs = leds;
_state.LEDs.Add(ledvec);
//set hardware identifier equat to array index
for (int i = 0; i < _state.LEDs.Count; i++)
{
_state.LEDs[0].LEDVec[i].HardwareIdentifier = i;
}
}
//configure alternate state
if (_alternatestate == null)
{
_alternatestate = new led.LedarrayState();
_alternatestate.LEDs = leds;
}
// Listen on the main port for requests and call the appropriate handler.
ActivateDsspOperationHandlers();
// Publish the service to the local Node Directory
DirectoryInsert();
// display HTTP service Uri
LogInfo(LogGroups.Console, "Service uri: ");
}
public IEnumerator <ITask> SetMotorPowerHandler(motor.SetMotorPower setMotorPower)
{
// Requests come too fast, so dump ones that come in too fast.
if (RequestPending > 0)
{
setMotorPower.ResponsePort.Post(new DefaultUpdateResponseType());
yield break;
}
//flip direction if necessary
double revPow = setMotorPower.Body.TargetPower;
if (_state.ReversePolarity)
{
revPow *= -1.0;
}
//update state
_state.CurrentPower = revPow;
//convert to native units
revPow *= 100;
revPow += 100;
int power = (int)Math.Round(revPow);
//send hardware specific motor data
brick.SetMotorBody motordata = new brick.SetMotorBody();
motordata.Motor = _state.Name;
motordata.Speed = power;
RequestPending++;
Activate(Arbiter.Choice(_scribblerPort.SetMotor(motordata),
delegate(DefaultUpdateResponseType status)
{
setMotorPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
RequestPending--;
},
delegate(soap.Fault failure)
{
setMotorPower.ResponsePort.Post(failure);
RequestPending--;
}
));
yield break;
}
IEnumerator <ITask> OnConnectMotorHandler(OnConnectMotor onConnectMotor)
{
if (onConnectMotor.DriveControl == _driveControl)
{
drive.EnableDriveRequest request = new drive.EnableDriveRequest(false);
if (_drivePort != null)
{
yield return(Arbiter.Choice(
_drivePort.EnableDrive(request),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault f)
{
LogError(f);
}
));
if (_motorShutdown != null)
{
PerformShutdown(ref _motorShutdown);
}
}
_drivePort = ServiceForwarder <drive.DriveOperations>(onConnectMotor.Service);
_motorShutdown = new Port <Shutdown>();
drive.ReliableSubscribe subscribe = new drive.ReliableSubscribe(
new ReliableSubscribeRequestType(10)
);
subscribe.NotificationPort = _driveNotify;
subscribe.NotificationShutdownPort = _motorShutdown;
_drivePort.Post(subscribe);
yield return(Arbiter.Choice(
subscribe.ResponsePort,
delegate(SubscribeResponseType response)
{
LogInfo("Subscribed to " + onConnectMotor.Service);
},
delegate(Fault fault)
{
_motorShutdown = null;
LogError(fault);
}
));
}
}
public virtual IEnumerator <ITask> AlertHandler(Alert alert)
{
SuccessFailurePort result = new SuccessFailurePort();
AlertForm form = null;
RunForm runForm = new RunForm(
delegate()
{
form = new AlertForm(result);
form.Message = alert.Body.Message;
form.Countdown = _defaultTimeout;
return(form);
}
);
WinFormsServicePort.Post(runForm);
yield return(Arbiter.Choice(
runForm.pResult,
delegate(SuccessResult success){},
delegate(Exception e)
{
result.Post(e);
}
));
yield return(Arbiter.Choice(
result,
delegate(SuccessResult success)
{
alert.ResponsePort.Post(DefaultSubmitResponseType.Instance);
if (form.Timeout)
{
LogWarning("Alert dialog timed out.");
}
},
delegate(Exception e)
{
Fault fault = Fault.FromException(e);
LogError(null, "Error in Alert Handler", fault);
alert.ResponsePort.Post(fault);
}
));
}
//Метод, осуществляющий запуск параллельного алгоритма сортировки слиянием.
//Результат - время подготовки необходимого для запуска асинхронных
//вычислений и разбиения задачи на подзадачи.
public long workAsync()
{
Stopwatch watcher = new Stopwatch(); // вычисление полного времени вычислений
watcher.Start();
InputData[] ClArr = new InputData[processCount]; // Создал хранилище начальных данных для подзадач
for (int i = 0; i < processCount; i++)
{
ClArr[i] = new InputData();
}
int step = (Int32)(itemCount / processCount); // Размер данных
int c = -1;
for (int i = 0; i < processCount; i++) // Заполнил хранилище исходными данными
{
ClArr[i].start = c + 1;
ClArr[i].stop = c + step;
c += step;
}
// Создадим диспетчер задач
Dispatcher d = new Dispatcher(processCount, "Test Pool");
DispatcherQueue dq = new DispatcherQueue("Test Queue", d);
// Определим экземпляр порта
Port <int> port = new Port <int>();
// Заполняем диспетчер задачами
for (int i = 0; i < processCount; i++)
{
Arbiter.Activate(dq, new Task <InputData, Port <int> >(ClArr[i], port, sort));
}
// Зададим задачу, которая будет выполнена после выполнения всех остальных задач
Arbiter.Activate(dq, Arbiter.MultipleItemReceive(true, port, processCount, delegate(int[] array)
{
// Проведем окончательно слияние 2 отсортированных подмасива
MainMerge(raw, 0, (itemCount - 1) / 2 + 1, itemCount - 1);
watcher.Stop();
// Выведем сообщение об времени выполнения алгоритма
Console.WriteLine("Общее время выполнения: " + watcher.ElapsedMilliseconds);
}
));
return(watcher.ElapsedMilliseconds);
}
//private IEnumerator<ITask> ConfigureSensorHandler(ConfigureSensor command)
//{
// if (command.Body == null)
// {
// command.ResponsePort.Post(new Fault());
// yield break;
// }
// if (command.Body.Sensor == null || command.Body.Configuration == null)
// {
// command.ResponsePort.Post(new Fault());
// yield break;
// }
// switch (command.Body.Sensor.ToUpper())
// {
// case "LEFT":
// _state.LightLeftConfig = command.Body.Configuration;
// break;
// case "CENTER":
// _state.LightCenterConfig = command.Body.Configuration;
// break;
// case "RIGHT":
// _state.LightRightConfig = command.Body.Configuration;
// break;
// }
// command.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// yield break;
//}
/// <summary>
/// Send a command to the Scribbler and wait for a response.
/// </summary>
/// <param name="ready"></param>
/// <param name="legoCommand"></param>
/// <returns></returns>
private IEnumerator <ITask> SendScribblerCommandHandler(SendScribblerCommand command)
{
// Send command to robot and wait for echo and response
ScribblerResponse validResponse = _scribblerCom.SendCommand(command.Body);
if (validResponse == null)
{
LogError(LogGroups.Console, "Send Scribbler Command null response");
command.ResponsePort.Post(new Fault());
}
//else if (validResponse.GetType() == typeof(LegoResponseException))
//{
// // Pull exception text from response
// string errorMessage = "LEGO command: " + command.Body.LegoCommandCode.ToString() + " response generated an error: " + ((LegoResponseException)validResponse).ErrorMessage;
// command.ResponsePort.Post(new SoapFaultContext(new InvalidOperationException(errorMessage)).SoapFault);
//}
else
{
// Check to see if we need to update state
// based on the response received from LEGO.
// PortSet<DefaultUpdateResponseType, Fault> responsePort = UpdateCurrentState(validResponse);
// if (responsePort != null)
// {
// yield return Arbiter.Choice(responsePort,
// delegate(DefaultUpdateResponseType response) { },
// delegate(Fault fault)
// {
// LogError(LogGroups.Console, "Failed to update LEGO NXT service state", fault);
// });
// }
// PostCommandProcessing(validResponse);
//reset timer
PollTimer.Enabled = false;
PollTimer.Enabled = true;
//Update our state with the scribbler's response
UpdateState(validResponse);
command.ResponsePort.Post(validResponse);
}
// Ready to process another command
Activate(Arbiter.ReceiveWithIterator <SendScribblerCommand>(false, _scribblerComPort, SendScribblerCommandHandler));
yield break;
}
protected override void Start()
{
if (_state == null)
{
_state = new ColorSegmentState();
}
else
{
for (int set = 0; set < _state.Colors.Count; set++)
{
ColorSet colorSet = _state.Colors[set];
for (int index = 0; index < colorSet.Colors.Count; index++)
{
ColorDefinition color = colorSet.Colors[index];
if (!color.Validate())
{
colorSet.Colors.RemoveAt(index);
index--;
}
}
if (colorSet.Colors.Count == 0)
{
_state.Colors.RemoveAt(set);
set--;
}
}
_state.UpdateColorSetMap();
_state.ImageSource = null;
_state.Processing = false;
_state.FrameCount = 0;
_state.DroppedFrames = 0;
_state.FoundColorAreas = null;
}
_utilitiesPort = DsspHttpUtilitiesService.Create(Environment);
_fwdPort = ServiceForwarder <ColorSegmentOperations>(ServiceInfo.Service);
base.Start();
Activate <ITask>(
Arbiter.Receive <webcam.UpdateFrame>(true, _webcamNotify, OnWebcamUpdateFrame)
);
_webcamPort.Subscribe(_webcamNotify, typeof(webcam.UpdateFrame));
}
private void UpdateContacts()
{
foreach (Arbiter arbiter in arbiterMap.Arbiters)
{
UpdateArbiterContacts(arbiter);
}
while (removedArbiterStack.Count > 0)
{
Arbiter arbiter = removedArbiterStack.Pop();
Arbiter.Pool.GiveBack(arbiter);
arbiterMap.Remove(arbiter);
removedArbiterQueue.Enqueue(arbiter);
events.RaiseBodiesEndCollide(arbiter.body1, arbiter.body2);
}
}
/// <summary>
/// Called the first time the simulation engine tells us about our entity. Activate
/// the other handlers and insert ourselves into the service directory.
/// </summary>
/// <param name="ins"></param>
void InsertEntityNotificationHandlerFirstTime(simengine.InsertSimulationEntity ins)
{
InsertEntityNotificationHandler(ins);
base.Start();
MainPortInterleave.CombineWith(
new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive <simengine.InsertSimulationEntity>(true, _notificationTarget, InsertEntityNotificationHandler),
Arbiter.Receive <simengine.DeleteSimulationEntity>(true, _notificationTarget, DeleteEntityNotificationHandler)
),
new ConcurrentReceiverGroup()
)
);
}
public Testbench()
{
_clkgen = new Clock(ClockCycle)
{
Clk = _clk
};
_arbiter = new Arbiter(NumMasters)
{
Clk = _clk,
Request = _request,
Grant = _grant
};
_masters = new ComponentCollection(CreateBusMasters());
}
public void DoBroadPhaseCollision()
{
RigidBody rigidBodyA;
RigidBody rigidBodyB;
for (int i = 0; i < _rigidBodyList.Count - 1; i++)
{
for (int j = i + 1; j < _rigidBodyList.Count; j++)
{
rigidBodyA = _rigidBodyList[i];
rigidBodyB = _rigidBodyList[j];
//possible early exit
if((rigidBodyA.CollisionGroup == rigidBodyB.CollisionGroup) && rigidBodyA.CollisionGroup !=0 && rigidBodyB.CollisionGroup !=0){
continue;
}
if (rigidBodyA.IsStatic && rigidBodyB.IsStatic) { //don't collide two static bodies
continue;
}
if (AABB.Intersect(rigidBodyA.Geometry.AABB, rigidBodyB.Geometry.AABB)) {
if (rigidBodyA._collisionListner != null) { rigidBodyA._collisionListner.objectCollisionOccured(rigidBodyB, rigidBodyA._actionCommand); }
if (rigidBodyB._collisionListner != null) { rigidBodyB._collisionListner.objectCollisionOccured(rigidBodyA, rigidBodyB._actionCommand); }
Arbiter arbiter = new Arbiter(rigidBodyA, rigidBodyB, _allowedPenetration, _biasFactor, _maxContactsPerRigidBodyPair);
if (!_arbiterList.Contains(arbiter)) {
_arbiterList.Add(arbiter);
}
}
}
}
}
protected internal override bool TryGetIntersection(TimeStep step, Body first, Body second, out IContact contact)
{
long id = PairID.GetId(first.ID, second.ID);
Arbiter arbiter;
if (arbiters.TryGetValue(id, out arbiter))
{
arbiter.Update(step);
/* if (!arbiter.Collided)
{
arbiter.OnRemoved();
arbiters.Remove(id);
}*/
}
else
{
arbiter = new Arbiter(this, first, second);
arbiter.Update(step);
//if (arbiter.Collided)
//{
arbiters.Add(id, arbiter);
//}
}
contact = arbiter;
return arbiter.Collided;
}
protected internal override bool HandleCollision(Scalar dt, Body first, Body second)
{
Body entity1 = first;
Body entity2 = second;
long id = PairID.GetId(entity1.ID, entity2.ID);
Arbiter arbiter;
if (arbiters.TryGetValue(id, out arbiter))
{
arbiter.Update();
if (arbiter.Count == 0)
{
arbiters.Remove(id);
}
}
else
{
arbiter = new Arbiter(entity1, entity2, biasPreservesMomentum, biasFactor, allowedPenetration);
arbiter.Update();
if (!entity1.IgnoresCollisionResponse &&
!entity2.IgnoresCollisionResponse &&
arbiter.Count > 0)
{
arbiters.Add(id, arbiter);
}
}
bool rv = arbiter.Count > 0;
if (rv)
{
((SITag)first.SolverTag).CollisionCount++;
((SITag)second.SolverTag).CollisionCount++;
}
return rv;
}
private void UpdateArbiterContacts(Arbiter arbiter)
{
if (arbiter.contactList.Count == 0)
{
lock (removedArbiterStack) { removedArbiterStack.Push(arbiter); }
return;
}
for (int i = arbiter.contactList.Count - 1; i >= 0; i--)
{
Contact c = arbiter.contactList[i];
c.UpdatePosition();
if (c.penetration < -contactSettings.breakThreshold)
{
Contact.Pool.GiveBack(c);
arbiter.contactList.RemoveAt(i);
continue;
}
else
{
JVector diff; JVector.Subtract(ref c.p1, ref c.p2, out diff);
float distance = JVector.Dot(ref diff, ref c.normal);
diff = diff - distance * c.normal;
distance = diff.LengthSquared();
// hack (multiplication by factor 100) in the
// following line.
if (distance > contactSettings.breakThreshold * contactSettings.breakThreshold * 100)
{
Contact.Pool.GiveBack(c);
arbiter.contactList.RemoveAt(i);
continue;
}
}
}
}
protected internal override void Solve(Scalar dt)
{
Scalar dtInv = (dt > 0.0f) ? (1.0f / dt) : (0.0f);
foreach (Arbiter arb in arbiters.Values)
{
arb.Updated = false;
}
Detect(dt);
RemoveEmpty();
this.Engine.RunLogic(dt);
foreach (Body item in this.Bodies)
{
item.UpdateVelocity(dt);
}
int ArbCount = arbiters.Count;
Arbiter[] arbs = new Arbiter[ArbCount];
arbiters.Values.CopyTo(arbs, 0);
for (int index = 0; index < ArbCount; ++index)
{
arbs[index].PreApply(dtInv);
}
foreach (ISequentialImpulsesJoint joint in siJoints)
{
joint.PreApply(dtInv);
}
for (int i = 0; i < iterations; ++i)
{
for (int index = 0; index < ArbCount; ++index)
{
arbs[index].Apply();
}
foreach (ISequentialImpulsesJoint joint in siJoints)
{
joint.Apply();
}
}
foreach (Body item in this.Bodies)
{
if (biasPreservesMomentum)
{
item.UpdatePosition(dt, ref item.State.SolverVelocity);
item.State.SolverVelocity = ALVector2D.Zero;
}
else
{
item.UpdatePosition(dt);
}
item.ClearForces();
((SITag)item.SolverTag).CollisionCount = 0;
}
}
