void Triangulator::triangulateFromUpVp(cv::Mat &up, cv::Mat &vp, cv::Mat &xyz){
std::cerr << "WARNING! NOT FULLY IMPLEMENTED!" << std::endl;
int N = up.rows * up.cols;
cv::Mat projPointsCam(2, N, CV_32F);
uc.reshape(0,1).copyTo(projPointsCam.row(0));
vc.reshape(0,1).copyTo(projPointsCam.row(1));
cv::Mat projPointsProj(2, N, CV_32F);
up.reshape(0,1).copyTo(projPointsProj.row(0));
vp.reshape(0,1).copyTo(projPointsProj.row(1));
cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));
cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
Pp = cv::Mat(calibration.Kp) * temp;
cv::Mat xyzw;
cv::triangulatePoints(Pc, Pp, projPointsCam, projPointsProj, xyzw);
xyz.create(3, N, CV_32F);
for(int i=0; i<N; i++){
xyz.at<float>(0,i) = xyzw.at<float>(0,i)/xyzw.at<float>(3,i);
xyz.at<float>(1,i) = xyzw.at<float>(1,i)/xyzw.at<float>(3,i);
xyz.at<float>(2,i) = xyzw.at<float>(2,i)/xyzw.at<float>(3,i);
}
xyz = xyz.t();
xyz = xyz.reshape(3, up.rows);
}
point triangle::calc_center( triangle* t )
{
arma::vec Pa(3);
Pa(0) = (t->m_vertex_list[0].x);
Pa(1) = (t->m_vertex_list[0].y);
Pa(2) = 0;
arma::vec Pb(3);
Pb(0) = (t->m_vertex_list[1].x);
Pb(1) = (t->m_vertex_list[1].y);
Pb(2) = 0;
arma::vec Pc(3);
Pc(0) = (t->m_vertex_list[2].x);
Pc(1) = (t->m_vertex_list[2].y);
Pc(2) = 0;
arma::vec AB = Pb - Pa;
arma::vec AC = Pc - Pa;
arma::vec BC = Pc - Pb;
//circumcenter
// arma::vec N = arma::cross(AC,AB);
// arma::vec L1 = arma::cross(AB,N);
// arma::vec L2 = arma::cross(BC,N);
// arma::vec P21 = (Pc - Pa)/2;
// arma::vec P1 = (Pa + Pb)/2;
//incenter
arma::vec uab = AB / arma::norm(AB,1);
arma::vec uac = AC / arma::norm(AC,1);
arma::vec ubc = BC / arma::norm(BC,1);
arma::vec uba = -uab;
arma::vec L1 = uab + uac;
arma::vec L2 = uba + ubc;
arma::vec P21 = Pb-Pa;
arma::vec P1 = Pa;
arma::mat ML1(L1);
arma::mat ML2(L2);
arma::mat ML = arma::join_rows(ML1,-ML2);
arma::vec lambda = arma::solve(ML,P21);
arma::vec pos = P1+lambda(0)*L1;
point p;
p.x = pos(0);
p.y = pos(1);
return p;
}
Triangulator::Triangulator(CalibrationData _calibration) : calibration(_calibration){
// Precompute uc, vc maps
uc.create(calibration.frameHeight, calibration.frameWidth, CV_32F);
vc.create(calibration.frameHeight, calibration.frameWidth, CV_32F);
for(unsigned int row=0; row<calibration.frameHeight; row++){
for(unsigned int col=0; col<calibration.frameWidth; col++){
uc.at<float>(row, col) = col;
vc.at<float>(row, col) = row;
}
}
// Precompute determinant tensor
cv::Mat Pc(3,4,CV_32F,cv::Scalar(0.0));
cv::Mat(calibration.Kc).copyTo(Pc(cv::Range(0,3), cv::Range(0,3)));
cv::Mat Pp(3,4,CV_32F), temp(3,4,CV_32F);
cv::Mat(calibration.Rp).copyTo(temp(cv::Range(0,3), cv::Range(0,3)));
cv::Mat(calibration.Tp).copyTo(temp(cv::Range(0,3), cv::Range(3,4)));
Pp = cv::Mat(calibration.Kp) * temp;
cv::Mat e = cv::Mat::eye(4, 4, CV_32F);
int sz[] = {4, 3, 3, 3};
cv::Mat C(4, sz, CV_32F, cv::Scalar::all(0));
for(int k=0; k<4; k++){
for(int i=0; i<3; i++){
for(int j=0; j<3; j++){
for(int l=0; l<3; l++){
cv::Mat op(4, 4, CV_32F);
Pc.row(i).copyTo(op.row(0));
Pc.row(j).copyTo(op.row(1));
Pp.row(l).copyTo(op.row(2));
e.row(k).copyTo(op.row(3));
C.at<float>(cv::Vec4i(k,i,j,l)) = cv::determinant(op.t());
}
}
}
}
determinantTensor = C;
// Precompute lens correction maps
cv::Mat eye = cv::Mat::eye(3, 3, CV_32F);
cv::initUndistortRectifyMap(calibration.Kc, calibration.kc, eye, calibration.Kc, cv::Size(calibration.frameWidth, calibration.frameHeight), CV_32FC1, lensMap1, lensMap2);
//cv::Mat map1, map2;
//cv::normalize(lensMap1, map1, 0, 255, cv::NORM_MINMAX, CV_8U);
//cv::normalize(lensMap2, map2, 0, 255, cv::NORM_MINMAX, CV_8U);
//cv::imwrite("map1.png", map1);
//cv::imwrite("map2.png", map2);
}
//---------------------------------------------------------
inline TSG_Point CSG_Direct_Georeferencer::Image_to_World(double x_i, double y_i, double z_w)
{
double k;
TSG_Point p;
CSG_Vector Pc(3), Pw;
Pc[0] = (m_O[0] - x_i) * m_s;
Pc[1] = (m_O[1] - y_i) * m_s;
Pc[2] = m_f;
Pw = m_R * Pc;
k = (z_w - m_T[2]) / Pw[2];
p.x = m_T[0] + k * Pw[0];
p.y = m_T[1] + k * Pw[1];
return( p );
}
/**
* @function setGlCamera
* Set OpenGL camera parameters.
* The off-axis projection is what gives
* the feeling of augmented reality.
*/
void setGlCamera()
{
if(bProjectionMode)
{
/* SKEWED FRUSTRUM / OFF-AXIS PROJECTION
** My implementation is based on the following paper:
** Name: Generalized Perspective Projection
** Author: Robert Kooima
** Date: August 2008, revised June 2009
*/
//-- space corners coordinates
float pa[3]={-cx,-cy,0};
float pb[3]={cx,-cy,0};
float pc[3]={-cx,cy,0};
float pe[3]={glCamX,glCamY,glCamZ};
//-- space points
cv::Vec3f Pa(pa);
cv::Vec3f Pb(pb);
cv::Vec3f Pc(pc);
cv::Vec3f Pe(pe);
//-- Compute an orthonormal basis for the screen.
cv::Vec3f Vr = Pb-Pa;
Vr /= cv::norm(Vr);
cv::Vec3f Vu = Pc-Pa;
Vu /= cv::norm(Vu);
cv::Vec3f Vn = Vr.cross(Vu);
Vn /= cv::norm(Vn);
//-- Compute the screen corner vectors.
cv::Vec3f Va = Pa-Pe;
cv::Vec3f Vb = Pb-Pe;
cv::Vec3f Vc = Pc-Pe;
//-- Find the distance from the eye to screen plane.
float d = -Va.dot(Vn);
//-- Find the extent of the perpendicular projection.
float l = Va.dot(Vr) * near / d;
float r = Vr.dot(Vb) * near / d;
float b = Vu.dot(Va) * near / d;
float t = Vu.dot(Vc) * near / d;
//-- Load the perpendicular projection.
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glFrustum(l, r, b, t, near, far+d);
//-- Rotate the projection to be non-perpendicular.
float M[16];
memset(M, 0, 16 * sizeof (float));
M[0] = Vr[0]; M[1] = Vu[0]; M[2] = Vn[0];
M[4] = Vr[1]; M[5] = Vu[1]; M[6] = Vn[1];
M[8] = Vr[2]; M[9] = Vu[2]; M[10] = Vn[2];
M[15] = 1.0f;
glMultMatrixf(M);
//-- Move the apex of the frustum to the origin.
glTranslatef(-pe[0], -pe[1], -pe[2]);
//-- Reset modelview matrix
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
}
else
{
//-- intrinsic camera params
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(60, (float)windowWidth/(float)windowHeight, 1, 250);
//-- extrinsic camera params
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
gluLookAt(glCamX, glCamY, glCamZ, 0, 0, 0, 0, 1, 0);
}
}
void POS::UndoMove(int move, UNDO *u) {
int sd = Opp(side);
int op = side;
int fsq = Fsq(move);
int tsq = Tsq(move);
int ftp = Tp(pc[tsq]); // moving piece
int ttp = u->ttp;
castle_flags = u->castle_flags;
ep_sq = u->ep_sq;
rev_moves = u->rev_moves;
pawn_key = u->pawn_key;
hash_key = u->hash_key;
head--;
pc[fsq] = Pc(sd, ftp);
pc[tsq] = NO_PC;
cl_bb[sd] ^= SqBb(fsq) | SqBb(tsq);
tp_bb[ftp] ^= SqBb(fsq) | SqBb(tsq);
#ifndef LEAF_PST
mg_pst[sd] += Param.mg_pst_data[sd][ftp][fsq] - Param.mg_pst_data[sd][ftp][tsq];
eg_pst[sd] += Param.eg_pst_data[sd][ftp][fsq] - Param.eg_pst_data[sd][ftp][tsq];
#endif
// Update king location
if (ftp == K) king_sq[sd] = fsq;
// Undo capture
if (ttp != NO_TP) {
pc[tsq] = Pc(op, ttp);
cl_bb[op] ^= SqBb(tsq);
tp_bb[ttp] ^= SqBb(tsq);
phase += phase_value[ttp];
#ifndef LEAF_PST
mg_pst[op] += Param.mg_pst_data[op][ttp][tsq];
eg_pst[op] += Param.eg_pst_data[op][ttp][tsq];
#endif
cnt[op][ttp]++;
}
switch (MoveType(move)) {
case NORMAL:
break;
case CASTLE:
// define complementary rook move
switch (tsq) {
case C1: { fsq = A1; tsq = D1; break; }
case G1: { fsq = H1; tsq = F1; break; }
case C8: { fsq = A8; tsq = D8; break; }
case G8: { fsq = H8; tsq = F8; break; }
}
pc[tsq] = NO_PC;
pc[fsq] = Pc(sd, R);
cl_bb[sd] ^= SqBb(fsq) | SqBb(tsq);
tp_bb[R] ^= SqBb(fsq) | SqBb(tsq);
#ifndef LEAF_PST
mg_pst[sd] += Param.mg_pst_data[sd][R][fsq] - Param.mg_pst_data[sd][R][tsq];
eg_pst[sd] += Param.eg_pst_data[sd][R][fsq] - Param.eg_pst_data[sd][R][tsq];
#endif
break;
case EP_CAP:
tsq ^= 8;
pc[tsq] = Pc(op, P);
cl_bb[op] ^= SqBb(tsq);
tp_bb[P] ^= SqBb(tsq);
phase += phase_value[P];
#ifndef LEAF_PST
mg_pst[op] += Param.mg_pst_data[op][P][tsq];
eg_pst[op] += Param.eg_pst_data[op][P][tsq];
#endif
cnt[op][P]++;
break;
case EP_SET:
break;
case N_PROM: case B_PROM: case R_PROM: case Q_PROM:
pc[fsq] = Pc(sd, P);
tp_bb[P] ^= SqBb(fsq);
tp_bb[ftp] ^= SqBb(fsq);
phase += phase_value[P] - phase_value[ftp];
#ifndef LEAF_PST
mg_pst[sd] += Param.mg_pst_data[sd][P][fsq] - Param.mg_pst_data[sd][ftp][fsq];
eg_pst[sd] += Param.eg_pst_data[sd][P][fsq] - Param.eg_pst_data[sd][ftp][fsq];
#endif
cnt[sd][P]++;
cnt[sd][ftp]--;
break;
}
side ^= 1;
}
void caml_debugger(enum event_kind event)
{
int frame_number;
value * frame;
intnat i, pos;
value val;
if (dbg_socket == -1) return; /* Not connected to a debugger. */
/* Reset current frame */
frame_number = 0;
frame = caml_extern_sp + 1;
/* Report the event to the debugger */
switch(event) {
case PROGRAM_START: /* Nothing to report */
goto command_loop;
case EVENT_COUNT:
putch(dbg_out, REP_EVENT);
break;
case BREAKPOINT:
putch(dbg_out, REP_BREAKPOINT);
break;
case PROGRAM_EXIT:
putch(dbg_out, REP_EXITED);
break;
case TRAP_BARRIER:
putch(dbg_out, REP_TRAP);
break;
case UNCAUGHT_EXC:
putch(dbg_out, REP_UNCAUGHT_EXC);
break;
}
caml_putword(dbg_out, caml_event_count);
if (event == EVENT_COUNT || event == BREAKPOINT) {
caml_putword(dbg_out, caml_stack_high - frame);
caml_putword(dbg_out, (Pc(frame) - caml_start_code) * sizeof(opcode_t));
} else {
/* No PC and no stack frame associated with other events */
caml_putword(dbg_out, 0);
caml_putword(dbg_out, 0);
}
caml_flush(dbg_out);
command_loop:
/* Read and execute the commands sent by the debugger */
while(1) {
switch(getch(dbg_in)) {
case REQ_SET_EVENT:
pos = caml_getword(dbg_in);
Assert (pos >= 0);
Assert (pos < caml_code_size);
caml_set_instruction(caml_start_code + pos / sizeof(opcode_t), EVENT);
break;
case REQ_SET_BREAKPOINT:
pos = caml_getword(dbg_in);
Assert (pos >= 0);
Assert (pos < caml_code_size);
caml_set_instruction(caml_start_code + pos / sizeof(opcode_t), BREAK);
break;
case REQ_RESET_INSTR:
pos = caml_getword(dbg_in);
Assert (pos >= 0);
Assert (pos < caml_code_size);
pos = pos / sizeof(opcode_t);
caml_set_instruction(caml_start_code + pos, caml_saved_code[pos]);
break;
case REQ_CHECKPOINT:
#ifndef _WIN32
i = fork();
if (i == 0) {
close_connection(); /* Close parent connection. */
open_connection(); /* Open new connection with debugger */
} else {
caml_putword(dbg_out, i);
caml_flush(dbg_out);
}
#else
caml_fatal_error("error: REQ_CHECKPOINT command");
exit(-1);
#endif
break;
case REQ_GO:
caml_event_count = caml_getword(dbg_in);
return;
case REQ_STOP:
exit(0);
break;
case REQ_WAIT:
#ifndef _WIN32
wait(NULL);
#else
caml_fatal_error("Fatal error: REQ_WAIT command");
exit(-1);
#endif
break;
case REQ_INITIAL_FRAME:
frame = caml_extern_sp + 1;
/* Fall through */
case REQ_GET_FRAME:
caml_putword(dbg_out, caml_stack_high - frame);
if (frame < caml_stack_high){
caml_putword(dbg_out, (Pc(frame) - caml_start_code) * sizeof(opcode_t));
}else{
caml_putword (dbg_out, 0);
}
caml_flush(dbg_out);
break;
case REQ_SET_FRAME:
i = caml_getword(dbg_in);
frame = caml_stack_high - i;
break;
case REQ_UP_FRAME:
i = caml_getword(dbg_in);
if (frame + Extra_args(frame) + i + 3 >= caml_stack_high) {
caml_putword(dbg_out, -1);
} else {
frame += Extra_args(frame) + i + 3;
caml_putword(dbg_out, caml_stack_high - frame);
caml_putword(dbg_out, (Pc(frame) - caml_start_code) * sizeof(opcode_t));
}
caml_flush(dbg_out);
break;
case REQ_SET_TRAP_BARRIER:
i = caml_getword(dbg_in);
caml_trap_barrier = caml_stack_high - i;
break;
case REQ_GET_LOCAL:
i = caml_getword(dbg_in);
putval(dbg_out, Locals(frame)[i]);
caml_flush(dbg_out);
break;
case REQ_GET_ENVIRONMENT:
i = caml_getword(dbg_in);
putval(dbg_out, Field(Env(frame), i));
caml_flush(dbg_out);
break;
case REQ_GET_GLOBAL:
i = caml_getword(dbg_in);
putval(dbg_out, Field(caml_global_data, i));
caml_flush(dbg_out);
break;
case REQ_GET_ACCU:
putval(dbg_out, *caml_extern_sp);
caml_flush(dbg_out);
break;
case REQ_GET_HEADER:
val = getval(dbg_in);
caml_putword(dbg_out, Hd_val(val));
caml_flush(dbg_out);
break;
case REQ_GET_FIELD:
val = getval(dbg_in);
i = caml_getword(dbg_in);
if (Tag_val(val) != Double_array_tag) {
putch(dbg_out, 0);
putval(dbg_out, Field(val, i));
} else {
double d = Double_field(val, i);
putch(dbg_out, 1);
caml_really_putblock(dbg_out, (char *) &d, 8);
}
caml_flush(dbg_out);
break;
case REQ_MARSHAL_OBJ:
val = getval(dbg_in);
safe_output_value(dbg_out, val);
caml_flush(dbg_out);
break;
case REQ_GET_CLOSURE_CODE:
val = getval(dbg_in);
caml_putword(dbg_out, (Code_val(val)-caml_start_code) * sizeof(opcode_t));
caml_flush(dbg_out);
break;
case REQ_SET_FORK_MODE:
caml_debugger_fork_mode = caml_getword(dbg_in);
break;
}
}
}
void sManipulator::DoMove(sPosition *p, int move, UNDO *u)
{
int side = p->side; // moving side
int fsq = Fsq(move); // start square
int tsq = Tsq(move); // target square
int ftp = TpOnSq(p, fsq); // moving piece
int ttp = TpOnSq(p, tsq); // captured piece
U64 bbMove = SqBb(fsq) | SqBb(tsq); // optimization from Stockfish
// save data for undoing a move
u->ttp = ttp;
u->castleFlags = p->castleFlags;
u->epSquare = p->epSquare;
u->reversibleMoves = p->reversibleMoves;
u->hashKey = p->hashKey;
u->pawnKey = p->pawnKey;
p->repetitionList[p->head++] = p->hashKey;
// update reversible move counter (zeroing is done on captures and pawn moves)
p->reversibleMoves++;
p->hashKey ^= zobCastle[p->castleFlags];
p->castleFlags &= castleMask[fsq] & castleMask[tsq];
p->hashKey ^= zobCastle[p->castleFlags];
// clear en passant square
if (p->epSquare != NO_SQ) {
p->hashKey ^= zobEp[File(p->epSquare)];
p->epSquare = NO_SQ;
}
// move a piece from start square
p->pc[fsq] = NO_PC;
p->pc[tsq] = Pc(side, ftp);
p->hashKey ^= zobPiece[Pc(side, ftp)][fsq] ^ zobPiece[Pc(side, ftp)][tsq];
if (ftp == P) {
p->reversibleMoves = 0;
p->pawnKey ^= zobPiece[Pc(side, ftp)][fsq] ^ zobPiece[Pc(side, ftp)][tsq];
}
p->bbCl[side] ^= bbMove;
p->bbTp[ftp] ^= bbMove;
p->pstMg[side] += Data.pstMg[side][ftp][tsq] - Data.pstMg[side][ftp][fsq];
p->pstEg[side] += Data.pstEg[side][ftp][tsq] - Data.pstEg[side][ftp][fsq];
// on a king move update king location data
if (ftp == K) p->kingSquare[side] = tsq;
// capture
if (ttp != NO_TP) {
p->reversibleMoves = 0;
p->hashKey ^= zobPiece[Pc(Opp(side), ttp)][tsq];
if (ttp == P)
p->pawnKey ^= zobPiece[Pc(Opp(side), ttp)][tsq];
p->bbCl[Opp(side)] ^= SqBb(tsq);
p->bbTp[ttp] ^= SqBb(tsq);
p->pcCount[Opp(side)][ttp]--;
p->pieceMat[Opp(side)] -= Data.matValue[ttp];
p->phase -= Data.phaseValue[ttp];
p->pstMg[Opp(side)] -= Data.pstMg[Opp(side)][ttp][tsq];
p->pstEg[Opp(side)] -= Data.pstEg[Opp(side)][ttp][tsq];
}
switch (MoveType(move)) {
case NORMAL:
break;
case CASTLE:
if (tsq > fsq) {
fsq += 3;
tsq -= 1;
} else {
fsq -= 4;
tsq += 1;
}
p->pc[fsq] = NO_PC;
p->pc[tsq] = Pc(side, R);
p->hashKey ^= zobPiece[Pc(side, R)][fsq] ^ zobPiece[Pc(side, R)][tsq];
p->bbCl[side] ^= SqBb(fsq) | SqBb(tsq);
p->bbTp[R] ^= SqBb(fsq) | SqBb(tsq);
p->pstMg[side] += Data.pstMg[side][R][tsq] - Data.pstMg[side][R][fsq];
p->pstEg[side] += Data.pstEg[side][R][tsq] - Data.pstEg[side][R][fsq];
break;
case EP_CAP:
tsq ^= 8;
p->pc[tsq] = NO_PC;
p->hashKey ^= zobPiece[Pc(Opp(side), P)][tsq];
p->pawnKey ^= zobPiece[Pc(Opp(side), P)][tsq];
p->bbCl[Opp(side)] ^= SqBb(tsq);
p->bbTp[P] ^= SqBb(tsq);
p->pcCount[Opp(side)][P]--;
p->phase -= Data.phaseValue[P];
p->pstMg[Opp(side)] -= Data.pstMg[Opp(side)][P][tsq];
p->pstEg[Opp(side)] -= Data.pstEg[Opp(side)][P][tsq];
break;
case EP_SET:
tsq ^= 8;
if (bbPawnAttacks[side][tsq] & bbPc(p, Opp(side), P)) {
p->epSquare = tsq;
p->hashKey ^= zobEp[File(tsq)];
}
break;
// promotion: (1) add promoted piece and add values associated with it
case N_PROM: // (2) remove promoted pawn and substract values associated with it
case B_PROM:
case R_PROM:
case Q_PROM:
ftp = PromType(move);
p->pc[tsq] = Pc(side, ftp);
p->hashKey ^= zobPiece[Pc(side, P)][tsq] ^ zobPiece[Pc(side, ftp)][tsq];
p->pawnKey ^= zobPiece[Pc(side, P)][tsq];
p->bbTp[P] ^= SqBb(tsq);
p->bbTp[ftp]^= SqBb(tsq);
p->pcCount[side][ftp]++;
p->pcCount[side][P]--;
p->pieceMat[side] += Data.matValue[ftp];
p->phase += Data.phaseValue[ftp] - Data.phaseValue[P];
p->pstMg[side] += Data.pstMg[side][ftp][tsq] - Data.pstMg[side][P][tsq];
p->pstEg[side] += Data.pstEg[side][ftp][tsq] - Data.pstEg[side][P][tsq];
break;
}
p->side ^= 1;
p->hashKey ^= SIDE_RANDOM;
}
int Data_path (void)
{
register struct mem_wrd *memaddr;
register struct IR_FIELDS *ir;
register struct SIM_FLAGS *f;
int retval,retval_cmmu; /* return value */
unsigned int issue_latency = 0; /* max time to issue instr(s) */
if (ckbrkpts(IP, BRK_EXEC)) /* Check for breakpoint */
{
retval = -1;
return(retval);
}
retval_cmmu = 0;
if(usecmmu && (retval = cmmu_function1()))
return(retval);
if ((memaddr = getmemptr (IP, M_INSTR)) == 0)
return (exception(E_TCACC, "Code Access Bus Error"));
ir = &memaddr -> opcode;
f = ((ir->p) ? &ir->p->flgs: &simdata.flgs);
/* see if we can issue the instruction in this clock */
if ( (retval = test_issue( ir, f )) == -1 )
{
return ( retval );
}
else
{
Statistics (ir);
/* Issue the instruction to the appropriate FU */
do_issue();
issue_latency = f->is_latency;
issue_latency += retval;
prev_extime = issue_latency;
if(debugflag)
PPrintf(" after chk_SB : Dcmmutime = %d \n", Dcmmutime);
if (usecmmu)
cmmu_function2(ir);
/*
* The data from the source 1 register is put onto the
* source 1 bus, as an input to the ALU.
*/
m88000.S1bus = m88000.Regs[ir -> src1];
/*
* The data from the source 2 register, or an immediate
* value, is put on the source 2 bus, which is also an
* input to the ALU.
*/
if (!f -> imm_flags) /* if not immediate */
m88000.S2bus = m88000.Regs[ir -> src2];
else if (f -> imm_flags == i26bit)
m88000.S2bus = sext (opword (IP), 0, 26);
else if ((f -> imm_flags == i16bit) &&
((ir -> op < (unsigned)JSR) || (ir -> op > (unsigned)BCND)))
m88000.S2bus = uext (opword (IP), 0, 16);
else if ((f -> imm_flags == i16bit) &&
((ir -> op >= (unsigned)JSR) && (ir -> op <= (unsigned)BCND)))
m88000.S2bus = sext (opword (IP), 0, 16);
else if (f -> imm_flags == i10bit)
m88000.S2bus = uext (opword (IP), 0, 10);
else
{
Eprintf ("SYSTEM ERROR in dpath, funky sized immediate\n");
return(-1);
}
/*
* The instruction has been issued and the busses have
* been driven. Now execute the instruction.
*/
if( retval = execute(ir, f, memaddr) ) return(retval);
if (usecmmu && debugflag)
PPrintf(" Dcmmutime (total after store) = %d \n",Dcmmutime);
/*
* Adjust the program counter
*/
killtime ( issue_latency );
if ( retval = Pc(memaddr, ir, f) )
return ( retval );
}
return ( retval );
}