gettorqu.m

来自「多智能体工具包」· M 代码 · 共 30 行

function [ThisTorque,TheSign]=gettorqu(DeltaAngle,WidthAngle)
% GETTORQU	Calculate the torque by the force of a single robot.

%     Copyright (c) 1998-2000 Jiming Liu and Jianbing Wu

global ObjectSize;

SquareWidth=ObjectSize;
%SquareWidth=3;

PositiveToqueAngle=-DeltaAngle;
if PositiveToqueAngle<0
  PositiveToqueAngle=PositiveToqueAngle+2*pi;
end;
if (PositiveToqueAngle>=0) & (PositiveToqueAngle<pi)
  ToqueRadius=SquareWidth*sin(PositiveToqueAngle)/(2*sin(WidthAngle));
  ThisTorque=abs(ToqueRadius);
  TheSign=1;
end;

NegativeToqueAngle=DeltaAngle;
if NegativeToqueAngle<0
  NegativeToqueAngle=NegativeToqueAngle+2*pi;
end;
if (NegativeToqueAngle>0) & (NegativeToqueAngle<pi)
  ToqueRadius=SquareWidth*sin(NegativeToqueAngle)/(2*sin(WidthAngle));
  ThisTorque=abs(ToqueRadius);
  TheSign=-1;
end;