__asm void PendSV_Handler ()
{
PRESERVE8
IMPORT g_thread_current
IMPORT g_thread_next
switch_start
; Backup next thread address to R12
LDR R0, =g_thread_current ; Get pointer (current)
LDR R1, =g_thread_next ; Get pointer (next)
LDR R1, [R1] ; Load next thread address
MOV R12, R1 ; Save to R12
LDR R2, [R0] ; Load current thread address
STR R1, [R0] ; g_thread_current = g_thread_next
TST R2, R2
BEQ switch_next
; Save registers to stack, the registers in stack would be
; (H->L)
; xPSR, ReturnAddress(), LR(R14), R12, R3, R2, R1, R0
; R7, R6, R5, R4,
; R11, R10, R9, R8,
; Current LR(R14)
MRS R0, PSP
SUBS R0, #4 * 9
STR R0, [R2] ; Save PSP to thread handler
MOV R3, R9
MOV R2, R8
MOV R1, LR
STM R0!, {R1 - R3}
/* delete sessid from AVLTree */
AVLTree_Node * deletion(AVLTree_Node *T,uint16_t x)
{ AVLTree_Node *p;
if(T==NULL)
{
return NULL;
}
else
if(x > T->data) // insert in right subtree
{
T->right=deletion(T->right,x);
if(BF(T)==2)
if(BF(T->left)>=0)
T=LL(T);
else
T=LR(T);
}
else
if(x<T->data)
{
T->left=deletion(T->left,x);
if(BF(T)==-2)//Rebalance during windup
if(BF(T->right)<=0)
T=RR(T);
else
T=RL(T);
}
else
{
//data to be deleted is found
if(T->right !=NULL)
{ //delete its inorder succesor
p=T->right;
while(p->left != NULL)
p=p->left;
T->data=p->data;
T->head=p->head;
T->right=deletion(T->right,p->data);
if(BF(T)==2)//Rebalance during windup
if(BF(T->left)>=0)
T=LL(T);
else
T=LR(T);
}
else
return(T->left);
}
T->ht=height(T);
return(T);
}
//left right situation
static TLDNode *LR(TLDNode *node){
TLDNode *temp;
temp=node->left;
//perform rotation
node->left=RR(temp);
return LR(node);
}
node *insertNode(node *root, int cont){
if(root==NULL){
return createNode(cont);
}
else
if(cont>root->data) {
root->right=insertNode(root->right,cont);
if(BF(root)==-2){
if(cont>root->right->data)
root=RR(root);
else
root=RL(root);
}
}
else
if(cont<root->data) {
root->left=insertNode(root->left,cont);
if(BF(root)==2){
if(cont < root->left->data)
root=LL(root);
else
root=LR(root);
}
}
/*else{
if(cont==root->data) printf("the node %d already exists in the tree",cont);
return root;
}*/
root->ht=height(root);
return(root);
}
std::vector<int> min_for_window(std::vector<int> arr, int w){
int size = arr.size();
int n_windows = size - w+1;
std::vector<int> LR(size);
std::vector<int> RL(size);
std::vector<int> res(n_windows);
for(int i = 0; i < size; ++i){ // filling out LR
if(!(i % w)){
LR[i] = arr[i];
} else{
LR[i] = std::min(LR[i-1], arr[i]);
}
}
std::cout << "Printing Left-Right array: " << std::endl;
print_vector(LR);
for(int i = size-1; i >= 0; --i){ // filling out RL
if(i == size-1 || i % w == w-1){
RL[i] = arr[i];
} else{
RL[i] = std::min(RL[i+1], arr[i]);
}
}
std::cout << "Printing Right-Left array: " << std::endl;
print_vector(RL);
for(int i = 0; i < n_windows; ++i){ // filling out result array
res[i] = std::min(RL[i], LR[i+w-1]);
}
return res;
}
void GEMENI_charsent() {
unsigned long r = s->log[s->lc];
if (UCA0IFG & UCTXIFG) {
s->nch++;
switch (s->nch) {
case 1:
UCA0TXBUF = SL(r) << 5 |TL(r)<<4|KL(r)<<3|PL(r)<<2|WL(r)<<1|HL(r);
break;
case 2:
UCA0TXBUF = RL(r)<<6 | AL(r)<<5 | OL(r)<<4 | STAR(r)<<3;
break;
case 3:
UCA0TXBUF = ER(r)<<3 | UR(r)<<2 | FR(r)<<1 | RR(r);
break;
case 4:
UCA0TXBUF = PR(r)<<6 | BR(r)<<5 | LR(r)<<4 | GR(r)<<3 | TR(r)<<2 | SR(r)<<1 | DRS(r);
break;
case 5:
UCA0TXBUF = POUND(r)<<1 | ZRS(r);
break;
default:
s->lc++;
if (s->lc != s->nc-1) {
s->nch = 0;
UCA0TXBUF = 1 << 7; // first packet, no fn or '#'
} else {
s->flags &= ~CSEND;
}
}
}
}
/* Insert sessid and socket pair in AVL Tree */
AVLTree_Node* insertion(AVLTree_Node *T,uint16_t x, struct node *head)
{
if(T==NULL)
{
T=(AVLTree_Node*)malloc(sizeof(AVLTree_Node));
T->data=x;
T->head = head;
T->left=NULL;
T->right=NULL;
}
else
if(x > T->data) // insert in right subtree
{
T->right=insertion(T->right,x,head);
if(BF(T)==-2)
if(x>T->right->data)
T=RR(T);
else
T=RL(T);
}
else
if(x<T->data)
{
T->left=insertion(T->left,x,head);
if(BF(T)==2)
if(x < T->left->data)
T=LL(T);
else
T=LR(T);
}
T->ht=height(T);
//root=T;
return(T);
}
int main(/* int argc, char * argv[] */)
{
{
::libmaus::network::LogReceiver::unique_ptr_type LR(new ::libmaus::network::LogReceiver("logpre",4444,1024));
::libmaus::network::StringRecDispatchCallback srdc;
::libmaus::network::LogReceiverTestProcess::unique_ptr_type TP0 = UNIQUE_PTR_MOVE(LR->constructLogReceiverTestProcess(0 /* id */,&srdc));
::libmaus::network::LogReceiverTestProcess::unique_ptr_type TP1 = UNIQUE_PTR_MOVE(LR->constructLogReceiverTestProcess(1 /* id */,&srdc));
::libmaus::network::LogReceiverTestProcess::unique_ptr_type TP2 = UNIQUE_PTR_MOVE(LR->constructLogReceiverTestProcess(2 /* id */,&srdc));
std::map<uint64_t, ::libmaus::network::SocketBase::shared_ptr_type > controlsocks;
sleep(2);
while ( LR->controlDescriptorPending() )
{
::libmaus::network::LogReceiver::ControlDescriptor P = LR->getControlDescriptor();
controlsocks [ P.id ] = P.controlsock;
std::cerr << "Got control socket for " << P.id << std::endl;
switch ( P.id )
{
case 0: controlsocks.find(P.id)->second->writeString("Coke"); break;
case 1: controlsocks.find(P.id)->second->writeString("Pepsi"); break;
case 2: controlsocks.find(P.id)->second->writeString("Juice"); break;
}
}
LR.reset();
return 0;
}
}
avltree insert(avltree root,Elemtype data)
{
if(!root)
{
root=(pnode)malloc(sizeof(node));
root->data=data;
root->left=root->right=0;
root->height=0;
}
else if(data < root->data) //插到左边
{
root->left=insert(root->left,data);
if(HEIGHT(root->left)-HEIGHT(root->right)==2) //失去平衡
{
if(data < root->left->data)
root=LL(root);
else
root=LR(root);
}
}
else if(data >root->data) //插到右边
{
root->right=insert(root->right,data);
if(HEIGHT(root->right)-HEIGHT(root->left)==2) //失去平衡
{
if(data > root->right->data)
root=RR(root);
else
root=RL(root);
}
}
//data在树中已存在时什么事也不做
root->height=MAX(HEIGHT(root->left),HEIGHT(root->right))+1;
return root;
}
static Node NodeDelete(Node rootnode , Node delnode){
if (delnode == NULL || rootnode == NULL){
return NULL;
}
//node in the left
if (delnode -> data < rootnode -> data){
rootnode -> left = NodeDelete(rootnode -> left , delnode);
//lose balance
if (Height(rootnode -> right) == Height(rootnode -> right) + 2){
Node right = rootnode -> right;
if (Height(right -> left) > Height(right -> right)){
rootnode = RL(rootnode);
}else{
rootnode = RR(rootnode);
}
}
}
//node in the right
if (delnode -> data > rootnode -> data){
rootnode -> right = NodeDelete(rootnode -> right , delnode);
//lose balance
if (Height(rootnode -> left) == Height(rootnode -> right) + 2){
Node left = rootnode -> left;
if (Height(left -> right) > Height(left -> left)){
rootnode = LR(rootnode);
}else{
rootnode = LL(rootnode);
}
}
}
//delnode is rootnode
if (delnode -> data == rootnode -> data){
if ((rootnode -> left != NULL) && (rootnode -> right != NULL)){
//left is lager than right
if (Height(rootnode -> left) > Height(rootnode -> right)){
Node max = FindMax(rootnode -> left);
rootnode -> data = max -> data;
rootnode -> left = NodeDelete(rootnode -> left , max);
}else{
//right is lager than left
Node min = FindMin(rootnode -> right);
rootnode -> data = min -> data;
rootnode -> right = NodeDelete(rootnode -> right , min);
}
}else{
//left || right in null
Node tmp = rootnode;
if (rootnode -> left != NULL){
rootnode = rootnode -> left;
}else{
rootnode = rootnode -> right;
}
free(tmp);
}
}
return rootnode;
}
void insert(RBT * T,int x)
{
RNode * y = new RNode;
y->val = x;
y->L = y->R = y->F = T->nil;
_insert(T,T->root,y);
y->color = RED;
while((y != T->root) && (y->F->color == RED))
{
if(y->F == y->F->F->L)
{
RNode * u = y->F->F->R;
if(u->color == RED)
{
y->F->color = u->color = BLACK;
u->F->color = RED;
y = u->F;
}
else
{
if(y == y->F->R) y=y->F,LR(T,y); // 定义|x|为x的黑高度,则|y->F->L| == |y->F->R| => |y->F->L| = |y->L| =>足够对于y->F左旋
//假设修改,并且|y->F->F->R| == |y->R| 足够使y->F->F右旋
y->F->color = BLACK;
y->F->F->color = RED;
RR(T,y->F->F);
}
}else{
RNode * u = y->F->F->L;
if(u->color == RED)
{
y->F->color = u->color = BLACK;
u->F->color = RED;
y = u->F;
}else {
if(y == y->F->L) y = y->F,RR(T,y);
y->F->color = BLACK;
y->F->F->color = RED;
LR(T,y->F->F);
}
}
}
T->root->color = BLACK;
}
R _parse_list(T& t, R r, type_list<LL, LR>)
{
if (_check(r, srange(LL()())))
return _parse(t, r, srange(LL()()) );
return _parse_list( t, r, LR() );
/*R income = r;
r = _parse(t, r, srange(LL()()) );
if ( r == income )
return _parse_list( t, r, LR() );
return r;
*/
}
int
widget_memory_draw( void *data )
{
int x, y;
char pbuf[36];
widget_rectangle( LC(0), LR(0), 40 * 8, 16 * 8 + 4, 1 );
widget_rectangle( LC(0), LR(16) + 2, 320, 1, 7 );
for( y = 0; y < 16; ++y ) {
libspectrum_word addr = memaddr + y * 8;
sprintf( pbuf, "%04X:", addr );
widget_printstring_right( LC(5) - 4, LR(y), 5, pbuf );
for( x = 0; x < 8; ++x ) {
libspectrum_byte b = readbyte_internal( addr + x );
widget_printchar_fixed( LC(x + 29) / 8, LR(y) / 8, 7 - (y & 1), b );
sprintf( pbuf + x * 3, "%02X ", b );
}
widget_printstring_fixed( LC(5) / 8, LR(y) / 8, 7 - (y & 1), pbuf );
}
widget_display_lines( LR(0) / 8, 17 );
return 0;
}
/**
Função recursiva de inserção na árvore AVL.
*/
int insertAVL_TreeR(struct AVL_TreeNode **node, void *elem, int *h) {
if (!*node) {//se o nó não existir, é criado um novo no
AVL_TreeNode *novo = (AVL_TreeNode*) malloc(sizeof (AVL_TreeNode));
novo->elem = elem;
novo->fb = 0;
novo->left = NULL;
novo->right = NULL;
*node = novo;
*h = 1; //haverá mudança na altura
return 1;
} else {
if (*(int*) (*node)->elem == *(int*) elem) {
*h = 0;
return 0;
}
if (*(int*) elem < *(int*) (*node)->elem) {
insertAVL_TreeR(&((*node)->left), elem, h);
if (*h == 1) { //se h = 1 então houve mudança na altura da arvore então terã de recalcular o FB e talvez rebalancear.
//Se rebalancear então não haverá mais mudança na altura.
if ((*node)->fb == 0) { //Se fb era 0 então estava balanceada,
(*node)->fb--; //inserindo alguém a esquerda, diminui o fb de 1
*h = 1; //e haverá mudança na altura
} else
if ((*node)->fb == 1) {//se o fb era 1 então a direita era mais pesada
(*node)->fb--; //Inserindo alguém na esquerda o fb diminuirá de 1
*h = 0; //e não haverá mudança na altura
} else
if ((*node)->fb == -1) {//se o fb era -1 então a esquerda é mais pesada
(*node)->fb = 0; //insserindo alguém na esquerda o fb irá a dois e deverá ser rebalanceada, voltando a 0
RR(&(*node), h); //caso1
}
}
} else
if (*(int*) elem > *(int*) (*node)->elem) {
insertAVL_TreeR(&((*node)->right), elem, h);
if (*h == 1) {
if ((*node)->fb == 1) {//Se fb era 1 então a direita ja tinha alguem, faz uma rotação para esquerda e a altura não muda
LR(&(*node), h);
*h = 0;
} else
if ((*node)->fb == 0) {
(*node)->fb++;
*h = 1;
} else
if ((*node)->fb == -1) { //Se fb era -1 então a esquerda era maior, como foi inserido na direita, a altura não muda.
(*node)->fb++;
*h = 0;
}
}
}
}
}
NODE* node_insert(NODE* node, SYMBOL *symbol)
{
NODE *root;
int balance;
int cmp, cmpchild;
root = node;
if (node == NULL)
return node_new(symbol);
cmp = strcmp(symbol->id, node->symbol->id);
if (cmp < 0)
{
/* Left */
node->left = node_insert(node->left, symbol);
balance = node_height(node->left) - node_height(node->right);
if (balance == 2)
{
cmpchild = strcmp(symbol->id, node->left->symbol->id);
if (cmpchild < 0)
root = LL(node);
else if (cmpchild > 0)
root = LR(node);
}
} else if (cmp > 0)
{
/* Right */
node->right = node_insert(node->right, symbol);
balance = node_height(node->right) - node_height(node->left);
if (balance == 2)
{
cmpchild = strcmp(symbol->id, node->right->symbol->id);
if (cmpchild < 0)
root = RL(node);
else if (cmpchild > 0)
root = RR(node);
}
}/* else
{
the implementation doesn't allow duplicates, since a lookup is ALWAYS performed first
} */
node->height = max(node_height(node->left), node_height(node->right))+1;
return root;
}
void adjust(struct node **p) {
int balance;
balance = height((*p)->left) - height((*p)->right);
if(balance == 2) {
if(height((*p)->left->left) > height((*p)->left->right))
LL(p);
else
LR(p);
} else if(balance == -2) {
if(height((*p)->right->right) > height((*p)->right->left))
RR(p);
else
RL(p);
}
}
void World::RenderGroup(SceneGroup *i, mat4 t)
{
int ii, time = 0;
// sphere vars
double r;
MaterialInfo m;
// light vars
LightInfo l;
// camera vars
CameraInfo f;
// if this is a terminal node
if (i->getChildCount() == 0) {
// if this node is a sphere
if (i->computeSphere(r,m,time)) {
_spheres.push_back(Sphere(vec4(0,0,0,1), r, m, t));
} else if (i->computeLight(l)) {
if (l.type == LIGHT_POINT) {
_lights[l.type].push_back(Light(vec3(t*vec4(0,0,0,1),VW),0, l));
}else if (l.type == LIGHT_DIRECTIONAL){
_lights[l.type].push_back(Light(0,vec3(t*vec4(0,0,-1,0),VW), l));
}else if (l.type == LIGHT_AMBIENT)
_ambientLight = l.color;
} else if (i->computeCamera(f)) {
vec4 eye(0.0, 0.0, 0.0, 1.0);
vec4 LL(f.sides[FRUS_LEFT], f.sides[FRUS_BOTTOM], -1*f.sides[FRUS_NEAR], 1.0);
vec4 UL(f.sides[FRUS_LEFT], f.sides[FRUS_TOP], -1*f.sides[FRUS_NEAR], 1.0);
vec4 LR(f.sides[FRUS_RIGHT], f.sides[FRUS_BOTTOM], -1*f.sides[FRUS_NEAR], 1.0);
vec4 UR(f.sides[FRUS_RIGHT], f.sides[FRUS_TOP], -1*f.sides[FRUS_NEAR], 1.0);
_view = Viewport(eye, LL, UL, LR, UR, IMAGE_WIDTH, IMAGE_HEIGHT);
}
} else {
// expand and traverse this node
for(ii=0; ii<i->getChildCount();ii++)
RenderInstance(i->getChild(ii), t);
}
}
Node NodeInsert(Node rootnode , int data){
if (rootnode == NULL){
rootnode = CreatNode(NULL , NULL , data);
return rootnode;
}
if (data == rootnode -> data){
printf("Insert Node %d Failed !\n" , data);
}
if (data < rootnode -> data){
rootnode -> left = NodeInsert(rootnode -> left , data);
rootnode -> height = MAX(Height(rootnode -> left) , Height(rootnode -> right)) + 1;
//lose balance
int rightheight = 0;
if (rootnode -> right != NULL){
rightheight = rootnode -> right ->height;
}
if (Height(rootnode -> left) >= rightheight + 2){
if (data < rootnode -> left -> data){
rootnode = LL(rootnode);
}else{
rootnode = LR(rootnode);
}
}
}else{
if (data > rootnode -> data){
rootnode -> right = NodeInsert(rootnode -> right , data);
rootnode -> height = MAX(Height(rootnode -> left) , Height(rootnode -> right)) + 1;
//lose balance
int leftheight = 0;
if (rootnode -> left != NULL){
leftheight = rootnode -> left -> height;
}
if (Height(rootnode -> right) >= leftheight + 2){
if (data > rootnode -> right -> data){
rootnode = RR(rootnode);
}else{
rootnode = RL(rootnode);
}
}
}
}
rootnode -> height = MAX(Height(rootnode -> left) , Height(rootnode -> right)) + 1;
return rootnode;
}
Node* AvlTree::nodeInsert(Node* &rootNode , int data){
if (rootNode == NULL){
rootNode = new Node(NULL, NULL, data);
return rootNode;
}
if (data == rootNode->data){
return NULL;
}
if (data < rootNode -> data){
rootNode -> left = nodeInsert(rootNode->left, data);
rootNode -> height = MAX(Height(rootNode->left) , Height(rootNode -> right)) + 1;
int rightHeight = 0;
if (rootNode -> right != NULL){
rightHeight = rootNode -> right -> height;
}
if (Height(rootNode -> left) >= rightHeight + 2){
if (data < rootNode -> left -> data){
rootNode = LL(rootNode);
}else{
rootNode = LR(rootNode);
}
}
}else{
if (data > rootNode -> data){
rootNode -> right = nodeInsert(rootNode -> right, data);
rootNode -> height = 1 + MAX(Height(rootNode -> left) , Height(rootNode -> right));
int leftHeight = 0;
if (rootNode -> left != NULL){
leftHeight = rootNode -> left -> height;
}
if (Height(rootNode -> right) >= leftHeight + 2){
if (data > rootNode -> right -> data){
rootNode = RR(rootNode);
}else{
rootNode = RL(rootNode);
}
}
}
}
rootNode -> height = MAX(Height(rootNode -> left) , Height(rootNode -> right)) + 1;
return rootNode;
}
static TLDNode *balance(TLDNode *node) {
int nodeDiff;
nodeDiff=checkDiff(node);
if (nodeDiff>1){
int checkLeft=checkDiff(node->left);
if (checkLeft>0){
node=LL(node);
}else{
node=LR(node);
}
}else if(nodeDiff<-1){
int checkRight=checkDiff(node->right);
if (checkRight>0){
node=RL(node);
}else{
node=RR(node);
}
};
return node;
}
int main(int argc, char ** argv)
{
try
{
::libmaus2::util::ArgInfo const arginfo(argc,argv);
::libmaus2::lsf::LSF::init(arginfo.progname);
testFarmerWorker(arginfo);
// testBunch(arginfo);
#if 0
::libmaus2::network::LogReceiver::unique_ptr_type LR(new ::libmaus2::network::LogReceiver("testlsf",4444,1024));
std::vector < DispatchedLsfProcess::shared_ptr_type > procs =
DispatchedLsfProcess::start(arginfo,LR->sid,LR->hostname,LR->port,8/*id*/,"testlsfdispatcher","testlsfclient",1/* numthreads*/,100/*mem*/,0/*hosts*/,0/*cwd*/,0/*tmpspace*/,false/*valgrind*/);
DispatchedLsfProcess::join(procs);
#if 0
DispatchedLsfProcess proc(arginfo,LR->sid,LR->hostname,LR->port,0/*id*/,"testlsfdispatcher","testlsfclient",1/* numthreads*/,100/*mem*/,0/*hosts*/,0/*cwd*/,0/*tmpspace*/,false/*valgrind*/);
proc.waitKnown();
while ( proc.isUnfinished() )
{
std::cerr << "Waiting for process to finish." << std::endl;
sleep(1);
}
#endif
#endif
}
catch(std::exception const & ex)
{
std::cerr << ex.what() << std::endl;
}
}
void UNavMeshRenderingComponent::GatherData(struct FNavMeshSceneProxyData* CurrentData) const
{
#if WITH_RECAST
const ARecastNavMesh* NavMesh = Cast<ARecastNavMesh>(GetOwner());
CurrentData->Reset();
CurrentData->bEnableDrawing = NavMesh->bEnableDrawing;
CurrentData->bNeedsNewData = false;
if (CurrentData && NavMesh && NavMesh->bEnableDrawing)
{
FHitProxyId HitProxyId = FHitProxyId();
CurrentData->bDrawPathCollidingGeometry = NavMesh->bDrawPathCollidingGeometry;
CurrentData->NavMeshDrawOffset.Z = NavMesh->DrawOffset;
CurrentData->NavMeshGeometry.bGatherPolyEdges = NavMesh->bDrawPolyEdges;
CurrentData->NavMeshGeometry.bGatherNavMeshEdges = NavMesh->bDrawNavMeshEdges;
const FNavDataConfig& NavConfig = NavMesh->GetConfig();
CurrentData->NavMeshColors[RECAST_DEFAULT_AREA] = NavConfig.Color.DWColor() > 0 ? NavConfig.Color : NavMeshRenderColor_RecastMesh;
for (uint8 i = 0; i < RECAST_DEFAULT_AREA; ++i)
{
CurrentData->NavMeshColors[i] = NavMesh->GetAreaIDColor(i);
}
// just a little trick to make sure navmeshes with different sized are not drawn with same offset
CurrentData->NavMeshDrawOffset.Z += NavMesh->GetConfig().AgentRadius / 10.f;
NavMesh->BeginBatchQuery();
if (NavMesh->bDrawOctree)
{
const UNavigationSystem* NavSys = UNavigationSystem::GetCurrent(GetWorld());
const FNavigationOctree* NavOctree = NavSys ? NavSys->GetNavOctree() : NULL;
if (NavOctree)
{
for (FNavigationOctree::TConstIterator<> NodeIt(*NavOctree); NodeIt.HasPendingNodes(); NodeIt.Advance())
{
const FNavigationOctree::FNode& CurrentNode = NodeIt.GetCurrentNode();
const FOctreeNodeContext& CorrentContext = NodeIt.GetCurrentContext();
CurrentData->OctreeBounds.Add(CorrentContext.Bounds);
FOREACH_OCTREE_CHILD_NODE(ChildRef)
{
if (CurrentNode.HasChild(ChildRef))
{
NodeIt.PushChild(ChildRef);
}
}
}
}
}
NavMesh->GetDebugGeometry(CurrentData->NavMeshGeometry);
const TArray<FVector>& MeshVerts = CurrentData->NavMeshGeometry.MeshVerts;
// @fixme, this is going to double up on lots of interior lines
if (NavMesh->bDrawTriangleEdges)
{
for (int32 AreaIdx = 0; AreaIdx < RECAST_MAX_AREAS; ++AreaIdx)
{
const TArray<int32>& MeshIndices = CurrentData->NavMeshGeometry.AreaIndices[AreaIdx];
for (int32 Idx=0; Idx<MeshIndices.Num(); Idx += 3)
{
CurrentData->ThickLineItems.Add(FNavMeshSceneProxyData::FDebugThickLine(MeshVerts[MeshIndices[Idx + 0]] + CurrentData->NavMeshDrawOffset, MeshVerts[MeshIndices[Idx + 1]] + CurrentData->NavMeshDrawOffset, NavMeshRenderColor_Recast_TriangleEdges, DefaultEdges_LineThickness));
CurrentData->ThickLineItems.Add(FNavMeshSceneProxyData::FDebugThickLine(MeshVerts[MeshIndices[Idx + 1]] + CurrentData->NavMeshDrawOffset, MeshVerts[MeshIndices[Idx + 2]] + CurrentData->NavMeshDrawOffset, NavMeshRenderColor_Recast_TriangleEdges, DefaultEdges_LineThickness));
CurrentData->ThickLineItems.Add(FNavMeshSceneProxyData::FDebugThickLine(MeshVerts[MeshIndices[Idx + 2]] + CurrentData->NavMeshDrawOffset, MeshVerts[MeshIndices[Idx + 0]] + CurrentData->NavMeshDrawOffset, NavMeshRenderColor_Recast_TriangleEdges, DefaultEdges_LineThickness));
}
}
}
// make lines for tile edges
if (NavMesh->bDrawPolyEdges)
{
const TArray<FVector>& TileEdgeVerts = CurrentData->NavMeshGeometry.PolyEdges;
for (int32 Idx=0; Idx < TileEdgeVerts.Num(); Idx += 2)
{
CurrentData->TileEdgeLines.Add( FDebugRenderSceneProxy::FDebugLine(TileEdgeVerts[Idx] + CurrentData->NavMeshDrawOffset, TileEdgeVerts[Idx+1] + CurrentData->NavMeshDrawOffset, NavMeshRenderColor_Recast_TileEdges));
}
}
// make lines for navmesh edges
if (NavMesh->bDrawNavMeshEdges)
{
const FColor EdgesColor = DarkenColor(CurrentData->NavMeshColors[RECAST_DEFAULT_AREA]);
const TArray<FVector>& NavMeshEdgeVerts = CurrentData->NavMeshGeometry.NavMeshEdges;
for (int32 Idx=0; Idx < NavMeshEdgeVerts.Num(); Idx += 2)
{
CurrentData->NavMeshEdgeLines.Add( FDebugRenderSceneProxy::FDebugLine(NavMeshEdgeVerts[Idx] + CurrentData->NavMeshDrawOffset, NavMeshEdgeVerts[Idx+1] + CurrentData->NavMeshDrawOffset, EdgesColor));
}
}
// offset all navigation-link positions
if (!NavMesh->bDrawClusters)
{
for (int32 OffMeshLineIndex = 0; OffMeshLineIndex < CurrentData->NavMeshGeometry.OffMeshLinks.Num(); ++OffMeshLineIndex)
{
FRecastDebugGeometry::FOffMeshLink& Link = CurrentData->NavMeshGeometry.OffMeshLinks[OffMeshLineIndex];
const bool bLinkValid = (Link.ValidEnds & FRecastDebugGeometry::OMLE_Left) && (Link.ValidEnds & FRecastDebugGeometry::OMLE_Right);
if (NavMesh->bDrawFailedNavLinks || (NavMesh->bDrawNavLinks && bLinkValid))
{
const FVector V0 = Link.Left + CurrentData->NavMeshDrawOffset;
const FVector V1 = Link.Right + CurrentData->NavMeshDrawOffset;
const FColor LinkColor = ((Link.Direction && Link.ValidEnds) || (Link.ValidEnds & FRecastDebugGeometry::OMLE_Left)) ? DarkenColor(CurrentData->NavMeshColors[Link.AreaID]) : NavMeshRenderColor_OffMeshConnectionInvalid;
CacheArc(CurrentData->NavLinkLines, V0, V1, 0.4f, 4, LinkColor, LinkLines_LineThickness);
const FVector VOffset(0, 0, FVector::Dist(V0, V1) * 1.333f);
CacheArrowHead(CurrentData->NavLinkLines, V1, V0+VOffset, 30.f, LinkColor, LinkLines_LineThickness);
if (Link.Direction)
{
CacheArrowHead(CurrentData->NavLinkLines, V0, V1+VOffset, 30.f, LinkColor, LinkLines_LineThickness);
}
// if the connection as a whole is valid check if there are any of ends is invalid
if (LinkColor != NavMeshRenderColor_OffMeshConnectionInvalid)
{
if (Link.Direction && (Link.ValidEnds & FRecastDebugGeometry::OMLE_Left) == 0)
{
// left end invalid - mark it
DrawWireCylinder(CurrentData->NavLinkLines, V0, FVector(1,0,0), FVector(0,1,0), FVector(0,0,1), NavMeshRenderColor_OffMeshConnectionInvalid, Link.Radius, NavMesh->AgentMaxStepHeight, 16, 0, DefaultEdges_LineThickness);
}
if ((Link.ValidEnds & FRecastDebugGeometry::OMLE_Right) == 0)
{
DrawWireCylinder(CurrentData->NavLinkLines, V1, FVector(1,0,0), FVector(0,1,0), FVector(0,0,1), NavMeshRenderColor_OffMeshConnectionInvalid, Link.Radius, NavMesh->AgentMaxStepHeight, 16, 0, DefaultEdges_LineThickness);
}
}
}
}
}
if (NavMesh->bDrawTileLabels || NavMesh->bDrawPolygonLabels || NavMesh->bDrawDefaultPolygonCost || NavMesh->bDrawTileBounds)
{
// calculate appropriate points for displaying debug labels
const int32 TilesCount = NavMesh->GetNavMeshTilesCount();
CurrentData->DebugLabels.Reserve(TilesCount);
for (int32 TileIndex = 0; TileIndex < TilesCount; ++TileIndex)
{
int32 X, Y, Layer;
if (NavMesh->GetNavMeshTileXY(TileIndex, X, Y, Layer))
{
const FBox TileBoundingBox = NavMesh->GetNavMeshTileBounds(TileIndex);
FVector TileLabelLocation = TileBoundingBox.GetCenter();
TileLabelLocation.Z = TileBoundingBox.Max.Z;
FNavLocation NavLocation(TileLabelLocation);
if (!NavMesh->ProjectPoint(TileLabelLocation, NavLocation, FVector(NavMesh->TileSizeUU/100, NavMesh->TileSizeUU/100, TileBoundingBox.Max.Z-TileBoundingBox.Min.Z)))
{
NavMesh->ProjectPoint(TileLabelLocation, NavLocation, FVector(NavMesh->TileSizeUU/2, NavMesh->TileSizeUU/2, TileBoundingBox.Max.Z-TileBoundingBox.Min.Z));
}
if (NavMesh->bDrawTileLabels)
{
CurrentData->DebugLabels.Add(FNavMeshSceneProxyData::FDebugText(
/*Location*/NavLocation.Location + CurrentData->NavMeshDrawOffset
, /*Text*/FString::Printf(TEXT("(%d,%d:%d)"), X, Y, Layer)
));
}
if (NavMesh->bDrawPolygonLabels || NavMesh->bDrawDefaultPolygonCost)
{
TArray<FNavPoly> Polys;
NavMesh->GetPolysInTile(TileIndex, Polys);
if (NavMesh->bDrawDefaultPolygonCost)
{
float DefaultCosts[RECAST_MAX_AREAS];
float FixedCosts[RECAST_MAX_AREAS];
NavMesh->GetDefaultQueryFilter()->GetAllAreaCosts(DefaultCosts, FixedCosts, RECAST_MAX_AREAS);
for(int k = 0; k < Polys.Num(); ++k)
{
uint32 AreaID = NavMesh->GetPolyAreaID(Polys[k].Ref);
CurrentData->DebugLabels.Add(FNavMeshSceneProxyData::FDebugText(
/*Location*/Polys[k].Center + CurrentData->NavMeshDrawOffset
, /*Text*/FString::Printf(TEXT("\\%.3f; %.3f\\"), DefaultCosts[AreaID], FixedCosts[AreaID])
));
}
}
else
{
for(int k = 0; k < Polys.Num(); ++k)
{
uint32 NavPolyIndex = 0;
uint32 NavTileIndex = 0;
NavMesh->GetPolyTileIndex(Polys[k].Ref, NavPolyIndex, NavTileIndex);
CurrentData->DebugLabels.Add(FNavMeshSceneProxyData::FDebugText(
/*Location*/Polys[k].Center + CurrentData->NavMeshDrawOffset
, /*Text*/FString::Printf(TEXT("[%X:%X]"), NavTileIndex, NavPolyIndex)
));
}
}
}
if (NavMesh->bDrawTileBounds)
{
FBox TileBox = NavMesh->GetNavMeshTileBounds(TileIndex);
float DrawZ = (TileBox.Min.Z + TileBox.Max.Z) * 0.5f; // @hack average
FVector LL(TileBox.Min.X, TileBox.Min.Y, DrawZ);
FVector UR(TileBox.Max.X, TileBox.Max.Y, DrawZ);
FVector UL(LL.X, UR.Y, DrawZ);
FVector LR(UR.X, LL.Y, DrawZ);
CurrentData->ThickLineItems.Add(FNavMeshSceneProxyData::FDebugThickLine(LL, UL, NavMeshRenderColor_TileBounds, DefaultEdges_LineThickness));
CurrentData->ThickLineItems.Add(FNavMeshSceneProxyData::FDebugThickLine(UL, UR, NavMeshRenderColor_TileBounds, DefaultEdges_LineThickness));
CurrentData->ThickLineItems.Add(FNavMeshSceneProxyData::FDebugThickLine(UR, LR, NavMeshRenderColor_TileBounds, DefaultEdges_LineThickness));
CurrentData->ThickLineItems.Add(FNavMeshSceneProxyData::FDebugThickLine(LR, LL, NavMeshRenderColor_TileBounds, DefaultEdges_LineThickness));
}
}
}
}
CurrentData->bSkipDistanceCheck = GIsEditor && (GEngine->GetDebugLocalPlayer() == NULL);
CurrentData->bDrawClusters = NavMesh->bDrawClusters;
NavMesh->FinishBatchQuery();
// Draw Mesh
if (NavMesh->bDrawClusters)
{
for (int32 Idx = 0; Idx < CurrentData->NavMeshGeometry.Clusters.Num(); ++Idx)
{
const TArray<int32>& MeshIndices = CurrentData->NavMeshGeometry.Clusters[Idx].MeshIndices;
if (MeshIndices.Num() == 0)
{
continue;
}
FNavMeshSceneProxyData::FDebugMeshData DebugMeshData;
DebugMeshData.ClusterColor = GetClusterColor(Idx);
for (int32 VertIdx=0; VertIdx < MeshVerts.Num(); ++VertIdx)
{
AddVertexHelper(DebugMeshData, MeshVerts[VertIdx] + CurrentData->NavMeshDrawOffset, DebugMeshData.ClusterColor);
}
for (int32 TriIdx=0; TriIdx < MeshIndices.Num(); TriIdx+=3)
{
AddTriangleHelper(DebugMeshData, MeshIndices[TriIdx], MeshIndices[TriIdx+1], MeshIndices[TriIdx+2]);
}
CurrentData->MeshBuilders.Add(DebugMeshData);
}
}
else if (NavMesh->bDrawNavMesh)
{
for (int32 AreaType = 0; AreaType < RECAST_MAX_AREAS; ++AreaType)
{
const TArray<int32>& MeshIndices = CurrentData->NavMeshGeometry.AreaIndices[AreaType];
if (MeshIndices.Num() == 0)
{
continue;
}
FNavMeshSceneProxyData::FDebugMeshData DebugMeshData;
for (int32 VertIdx=0; VertIdx < MeshVerts.Num(); ++VertIdx)
{
AddVertexHelper(DebugMeshData, MeshVerts[VertIdx] + CurrentData->NavMeshDrawOffset, CurrentData->NavMeshColors[AreaType]);
}
for (int32 TriIdx=0; TriIdx < MeshIndices.Num(); TriIdx+=3)
{
AddTriangleHelper(DebugMeshData, MeshIndices[TriIdx], MeshIndices[TriIdx+1], MeshIndices[TriIdx+2]);
}
DebugMeshData.ClusterColor = CurrentData->NavMeshColors[AreaType];
CurrentData->MeshBuilders.Add(DebugMeshData);
}
}
if (NavMesh->bDrawPathCollidingGeometry)
{
// draw all geometry gathered in navoctree
const FNavigationOctree* NavOctree = NavMesh->GetWorld()->GetNavigationSystem()->GetNavOctree();
TArray<FVector> PathCollidingGeomVerts;
TArray <int32> PathCollidingGeomIndices;
for (FNavigationOctree::TConstIterator<> It(*NavOctree); It.HasPendingNodes(); It.Advance())
{
const FNavigationOctree::FNode& Node = It.GetCurrentNode();
for (FNavigationOctree::ElementConstIt ElementIt(Node.GetElementIt()); ElementIt; ElementIt++)
{
const FNavigationOctreeElement& Element = *ElementIt;
if (Element.ShouldUseGeometry(&NavMesh->NavDataConfig) && Element.Data.CollisionData.Num())
{
const FRecastGeometryCache CachedGeometry(Element.Data.CollisionData.GetData());
AppendGeometry(PathCollidingGeomVerts, PathCollidingGeomIndices, CachedGeometry.Verts, CachedGeometry.Header.NumVerts, CachedGeometry.Indices, CachedGeometry.Header.NumFaces);
}
}
FOREACH_OCTREE_CHILD_NODE(ChildRef)
{
if (Node.HasChild(ChildRef))
{
It.PushChild(ChildRef);
}
}
}
CurrentData->PathCollidingGeomIndices = PathCollidingGeomIndices;
for (const auto& Vertex : PathCollidingGeomVerts)
{
CurrentData->PathCollidingGeomVerts.Add(FDynamicMeshVertex(Vertex));
}
}
if (CurrentData->NavMeshGeometry.BuiltMeshIndices.Num() > 0)
{
FNavMeshSceneProxyData::FDebugMeshData DebugMeshData;
for (int32 VertIdx=0; VertIdx < MeshVerts.Num(); ++VertIdx)
{
AddVertexHelper(DebugMeshData, MeshVerts[VertIdx] + CurrentData->NavMeshDrawOffset, NavMeshRenderColor_RecastTileBeingRebuilt);
}
DebugMeshData.Indices.Append(CurrentData->NavMeshGeometry.BuiltMeshIndices);
DebugMeshData.ClusterColor = NavMeshRenderColor_RecastTileBeingRebuilt;
CurrentData->MeshBuilders.Add(DebugMeshData);
// updates should be requested by FRecastNavMeshGenerator::TickAsyncBuild after tiles were refreshed
}
if (NavMesh->bDrawClusters)
{
for (int i = 0; i < CurrentData->NavMeshGeometry.ClusterLinks.Num(); i++)
{
const FRecastDebugGeometry::FClusterLink& CLink = CurrentData->NavMeshGeometry.ClusterLinks[i];
const FVector V0 = CLink.FromCluster + CurrentData->NavMeshDrawOffset;
const FVector V1 = CLink.ToCluster + CurrentData->NavMeshDrawOffset + FVector(0,0,20.0f);
CacheArc(CurrentData->ClusterLinkLines, V0, V1, 0.4f, 4, FColor::Black, ClusterLinkLines_LineThickness);
const FVector VOffset(0, 0, FVector::Dist(V0, V1) * 1.333f);
CacheArrowHead(CurrentData->ClusterLinkLines, V1, V0+VOffset, 30.f, FColor::Black, ClusterLinkLines_LineThickness);
}
}
// cache segment links
if (NavMesh->bDrawNavLinks)
{
for (int32 iArea = 0; iArea < RECAST_MAX_AREAS; iArea++)
{
const TArray<int32>& Indices = CurrentData->NavMeshGeometry.OffMeshSegmentAreas[iArea];
FNavMeshSceneProxyData::FDebugMeshData DebugMeshData;
int32 VertBase = 0;
for (int32 i = 0; i < Indices.Num(); i++)
{
FRecastDebugGeometry::FOffMeshSegment& SegInfo = CurrentData->NavMeshGeometry.OffMeshSegments[Indices[i]];
const FVector A0 = SegInfo.LeftStart + CurrentData->NavMeshDrawOffset;
const FVector A1 = SegInfo.LeftEnd + CurrentData->NavMeshDrawOffset;
const FVector B0 = SegInfo.RightStart + CurrentData->NavMeshDrawOffset;
const FVector B1 = SegInfo.RightEnd + CurrentData->NavMeshDrawOffset;
const FVector Edge0 = B0 - A0;
const FVector Edge1 = B1 - A1;
const float Len0 = Edge0.Size();
const float Len1 = Edge1.Size();
const FColor SegColor = DarkenColor(CurrentData->NavMeshColors[SegInfo.AreaID]);
const FColor ColA = (SegInfo.ValidEnds & FRecastDebugGeometry::OMLE_Left) ? FColor::White : FColor::Black;
const FColor ColB = (SegInfo.ValidEnds & FRecastDebugGeometry::OMLE_Right) ? FColor::White : FColor::Black;
const int32 NumArcPoints = 8;
const float ArcPtsScale = 1.0f / NumArcPoints;
FVector Prev0 = EvalArc(A0, Edge0, Len0*0.25f, 0);
FVector Prev1 = EvalArc(A1, Edge1, Len1*0.25f, 0);
AddVertexHelper(DebugMeshData, Prev0, ColA);
AddVertexHelper(DebugMeshData, Prev1, ColA);
for (int32 j = 1; j <= NumArcPoints; j++)
{
const float u = j * ArcPtsScale;
FVector Pt0 = EvalArc(A0, Edge0, Len0*0.25f, u);
FVector Pt1 = EvalArc(A1, Edge1, Len1*0.25f, u);
AddVertexHelper(DebugMeshData, Pt0, (j == NumArcPoints) ? ColB : FColor::White);
AddVertexHelper(DebugMeshData, Pt1, (j == NumArcPoints) ? ColB : FColor::White);
AddTriangleHelper(DebugMeshData, VertBase+0, VertBase+2, VertBase+1);
AddTriangleHelper(DebugMeshData, VertBase+2, VertBase+3, VertBase+1);
AddTriangleHelper(DebugMeshData, VertBase+0, VertBase+1, VertBase+2);
AddTriangleHelper(DebugMeshData, VertBase+2, VertBase+1, VertBase+3);
VertBase += 2;
Prev0 = Pt0;
Prev1 = Pt1;
}
VertBase += 2;
DebugMeshData.ClusterColor = SegColor;
}
if (DebugMeshData.Indices.Num())
{
CurrentData->MeshBuilders.Add(DebugMeshData);
}
}
}
CurrentData->NavMeshGeometry.PolyEdges.Empty();
CurrentData->NavMeshGeometry.NavMeshEdges.Empty();
}
node * insert(node *T,int x)
{
if(T==NULL)
{
T=(node*)malloc(sizeof(node));
T->data=x;
T->left=NULL;
T->right=NULL;
}
else
if(x > T->data) // insert in right subtree
{
T->right=insert(T->right,x);
if(BF(T)==-2)
if(x>T->right->data)
T=RR(T);
else
T=RL(T);
}
else
if(x<T->data)
{
T->left=insert(T->left,x);
if(BF(T)==2)
if(x < T->left->data)
T=LL(T);
else
T=LR(T);
}
T->ht=height(T);
return(T);
}
//---------------------------------------------------------
bool CResection::On_Execute(void)
{
CSG_PointCloud *pPoints; // Input Point Cloud
CSG_String fileName;
CSG_File *pTabStream = NULL;
int n = 6; // Number of unknowns
CSG_Vector center(3);
CSG_Vector target(3);
double c = Parameters("F") ->asDouble(); // Focal Length (mm)
double pixWmm = Parameters("W") ->asDouble() / 1000;// Pixel Width (mm)
double ppOffsetX = Parameters("ppX") ->asDouble(); // Principal Point Offset X (pixels)
double ppOffsetY = Parameters("ppY") ->asDouble(); // Principal Point Offset Y (pixels)
pPoints = Parameters("POINTS") ->asPointCloud();
fileName = Parameters("OUTPUT FILE") ->asString();
center[0] = Parameters("Xc") ->asDouble();
center[1] = Parameters("Yc") ->asDouble();
center[2] = Parameters("Zc") ->asDouble();
target[0] = Parameters("Xt") ->asDouble();
target[1] = Parameters("Yt") ->asDouble();
target[2] = Parameters("Zt") ->asDouble();
int pointCount = pPoints->Get_Point_Count();
bool estPPOffsets = false;
if ( Parameters("EST_OFFSETS")->asBool() ) {
estPPOffsets = true;
n = 8; // Increase number of unknowns by 2
}
bool applyDistortions = false;
CSG_Vector K(3);
if ( Parameters("GIVE_DISTORTIONS")->asBool() ) {
applyDistortions = true;
K[0] = Parameters("K1") ->asDouble();
K[1] = Parameters("K2") ->asDouble();
K[2] = Parameters("K3") ->asDouble();
}
double dxapp = center [0] - target [0];
double dyapp = center [1] - target [1];
double dzapp = center [2] - target [2];
double h_d = sqrt (dxapp * dxapp + dyapp * dyapp + dzapp * dzapp); // Distance between Proj. Center & Target (m)
double h_dmm = h_d * 1000; // Convert to mm
if( fileName.Length() == 0 )
{
SG_UI_Msg_Add_Error(_TL("Please provide an output file name!"));
return( false );
}
pTabStream = new CSG_File();
if( !pTabStream->Open(fileName, SG_FILE_W, false) )
{
SG_UI_Msg_Add_Error(CSG_String::Format(_TL("Unable to open output file %s!"), fileName.c_str()));
delete (pTabStream);
return (false);
}
CSG_Vector rotns = methods::calcRotations(center,target); // Approx. rotations omega, kappa, alpha
CSG_String msg = "********* Initial Approximate Values *********";
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Rotation Angles:");
pTabStream->Write(SG_T("\n") + msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Omega:\t") + SG_Get_String(rotns[0],6,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Kappa:\t") + SG_Get_String(rotns[1],6,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Alpha:\t") + SG_Get_String(rotns[2],6,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Projection Center:");
pTabStream->Write(SG_T("\n") + msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Xc:\t") + SG_Get_String(center[0],4,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Yc:\t") + SG_Get_String(center[1],4,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Zc:\t") + SG_Get_String(center[2],4,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
if (estPPOffsets) {
msg = SG_T("Principal Point Offsets:");
pTabStream->Write(SG_T("\n") + msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("ppX:\t") + SG_Get_String(ppOffsetX,5,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("ppY:\t") + SG_Get_String(ppOffsetY,5,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
}
double itrNo = 0;
CSG_Matrix invN;
while (true) { // Begin Iterations
itrNo++;
double omega = rotns[0];
double kappa = rotns[1];
double alpha = rotns[2];
CSG_Matrix R = methods::calcRotnMatrix(rotns); // Rotation Matrix from approximate values
CSG_Matrix E(3,3); // [w1;w2;w3] = E * [dw;dk;da]
E[0][0] = -1;
E[0][1] = E[1][0] = E[2][0] = 0;
E[0][2] = sin(kappa);
E[1][1] = -cos(omega);
E[1][2] = -sin(omega) * cos(kappa);
E[2][1] = sin(omega);
E[2][2] = -cos(omega) * cos(kappa);
CSG_Matrix N(n,n); // Transpose(Design Matrix) * I * Design Matrix
CSG_Vector ATL(n); // Transpose(Design Matrix) * I * Shortened obs. vector
double SS = 0;
double sigma_naught = 0;
for (int i = 0; i < pointCount; i++) {
CSG_Vector pqs(3); // Approx. pi, qi, si
for (int j = 0; j < 3; j++) {
pqs[j] = R[j][0] * (pPoints->Get_X(i) - center[0]) +
R[j][1] * (pPoints->Get_Y(i) - center[1]) +
R[j][2] * (pPoints->Get_Z(i) - center[2]);
}
double p_i = pqs[0];
double q_i = pqs[1];
double s_i = pqs[2];
double dR = 0;
// Undistorted
double x_u = c * p_i / q_i;
double y_u = c * s_i / q_i;
double c_hat = c;
if (applyDistortions) {
double r2 = x_u * x_u + y_u * y_u;
dR = K[0] * r2 + K[1] * r2 * r2 + K[2] * r2 * r2 * r2;
c_hat = c * (1 - dR);
}
// Approx. image coordinates (with distortions)
double x_i = (1 - dR) * x_u + ppOffsetX * pixWmm;
double z_i = (1 - dR) * y_u + ppOffsetY * pixWmm;
// Shortened obervation vector: dxi & dzi
double dx_i = pPoints->Get_Attribute(i,0) * pixWmm - x_i;
double dz_i = pPoints->Get_Attribute(i,1) * pixWmm - z_i;
SS += pow(dx_i,2) + pow(dz_i,2);
/*
x_i, z_i in [mm]
p_i,q_i,s_i in [m]
h_d in [m]
c, c_hat in [mm]
h_dmm in [mm]
*/
CSG_Matrix L(3,2); // CSG_Matrix takes columns first and rows second
CSG_Matrix V(3,3);
CSG_Matrix LR(3,2);
CSG_Matrix LVE(3,2);
L[0][0] = L[1][2] = c_hat / (1000 * q_i);
L[0][2] = L[1][0] = 0;
L[0][1] = -x_u * (1 - dR) / (1000 * q_i);
L[1][1] = -y_u * (1 - dR) / (1000 * q_i);
V[0][0] = V[1][1] = V[2][2] = 0;
V[0][1] = s_i / h_d;
V[0][2] = -q_i / h_d;
V[1][0] = -s_i / h_d;
V[1][2] = p_i / h_d;
V[2][0] = q_i / h_d;
V[2][1] = -p_i / h_d;
LVE = ( L * V ) * E;
LR = L * R;
// Design Matrix (J)
CSG_Matrix design(n,2);
for(int j = 0; j < 2; j++) {
for(int k = 0; k < 3; k++) {
design[j][k] = LVE[j][k];
design[j][k+3] = -LR[j][k];
}
}
if ( estPPOffsets ) {
design[0][6] = design[1][7] = 1.0;
}
// Build Normal Matrix
for(int j = 0; j < n; j++) {
for(int k = 0; k < n; k++) {
N[j][k] += (design[0][j] * design[0][k] + design[1][j] * design[1][k]);
}
}
// Build Tranpose (J) * I * (Shortened obs. vector)
for (int m=0; m < n; m++) {
ATL[m] += design[0][m] * dx_i + design[1][m] * dz_i;
}
L.Destroy();
V.Destroy();
LR.Destroy();
LVE.Destroy();
pqs.Destroy();
design.Destroy();
} // end looping over observations
// Eigen values and Eigen Vectors
CSG_Vector eigenVals(n);
CSG_Matrix eigenVecs(n,n);
SG_Matrix_Eigen_Reduction(N, eigenVecs, eigenVals, true);
// One of the Eigen Values is 0
if (std::any_of(eigenVals.cbegin(),
eigenVals.cend(),
[] (double i) { return i == 0; })) {
msg = "The Normal Matrix has a rank defect. Please measure more points.";
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
break;
}
double mx = *std::max_element(eigenVals.cbegin(), eigenVals.cend());
double mn = *std::min_element(eigenVals.cbegin(), eigenVals.cend());
// Ratio of Smallest to the Biggest Eigen value is too small
if ((mn / mx) < pow(10,-12.0)) {
msg = SG_T("Condition of the Matrix of Normal Equations:\t") + CSG_String::Format(SG_T(" %13.5e"), mn/mx);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = "The Normal Matrix is weakly conditioned. Please measure more points.";
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
break;
}
// Calculate the adjustments
double absMax = 0;
invN = N.Get_Inverse();
CSG_Vector est_param_incs = invN * ATL;
for (int i = 0; i < n; i++) {
if (abs(est_param_incs[i]) > absMax) {
absMax = abs(est_param_incs[i]);
}
}
if (absMax < thresh) {
msg = "Solution has converged.";
pTabStream->Write(SG_T("\n") + msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
break;
}
for (int a = 0; a < 3; a++) {
rotns[a] += est_param_incs[a] / h_dmm; // New Approx. rotations omega, kappa, alpha
center[a] += est_param_incs[a+3] / 1000; // New Approx. Projection Center
}
if ( estPPOffsets ) {
ppOffsetX += (est_param_incs[6] / pixWmm); // New Approx. Principal Point
ppOffsetY += (est_param_incs[7] / pixWmm);
}
sigma_naught = sqrt(SS / (2 * pointCount - n));
// Writing To Output File & SAGA Console
msg = "********* Iteration: " + SG_Get_String(itrNo,0,false) + " *********";
pTabStream->Write(SG_T("\n") + msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = "Sum of Squared Residuals:\t" + SG_Get_String(SS,5,false);
pTabStream->Write(SG_T("\n") + msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = "Sigma Naught:\t" + SG_Get_String(sigma_naught,5,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Condition of the Matrix of Normal Equations:\t") + CSG_String::Format(SG_T(" %13.5e"), mn/mx);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
R.Destroy();
E.Destroy();
N.Destroy();
ATL.Destroy();
invN.Destroy();
eigenVals.Destroy();
eigenVecs.Destroy();
est_param_incs.Destroy();
} // end of iterations
msg = "********* Final Estimated Parameters *********";
pTabStream->Write(SG_T("\n") + msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Rotation Angles:");
pTabStream->Write(SG_T("\n") + msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Omega:\t") + SG_Get_String(rotns[0],6,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Kappa:\t") + SG_Get_String(rotns[1],6,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Alpha:\t") + SG_Get_String(rotns[2],6,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Projection Center:");
pTabStream->Write(SG_T("\n") + msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Xc:\t") + SG_Get_String(center[0],4,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Yc:\t") + SG_Get_String(center[1],4,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("Zc:\t") + SG_Get_String(center[2],4,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
if (estPPOffsets) {
msg = SG_T("Principal Point Offsets:");
pTabStream->Write(SG_T("\n") + msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("ppX:\t") + SG_Get_String(ppOffsetX,5,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
msg = SG_T("ppY:\t") + SG_Get_String(ppOffsetY,5,false);
pTabStream->Write(msg + SG_T("\n"));
SG_UI_Msg_Add(msg, true);
}
K.Destroy();
rotns.Destroy();
center.Destroy();
target.Destroy();
pTabStream->Close();
return true;
}
/* Force the target to call `dlopen()'. Modify its registers and stack contents
* and continue execution until a segmentation fault is caught. Return 0 on
* success and -1 on failure.
*/
static int force_dlopen(pid_t pid, char *filename)
{
void *linker_addr, *dlopen_off, *dlopen_addr;
regs_t regs;
size_t size;
int r = -1;
if(resolve_symbol(LINKER_PATH, DLOPEN_NAME1, &dlopen_off) != 0 &&
resolve_symbol(LINKER_PATH, DLOPEN_NAME2, &dlopen_off) != 0 &&
resolve_symbol(LINKER_PATH, DLOPEN_NAME3, &dlopen_off) != 0)
{
printf("[*] Could not resolve dlopen()\n");
goto ret;
}
if((linker_addr = get_linker_addr(pid)) == NULL)
{
printf("[*] Linker not found in PID %d\n", pid);
goto ret;
}
dlopen_addr = linker_addr + (ptrdiff_t)dlopen_off;
printf("[*] Resolved dlopen() at %p\n", dlopen_addr);
if(read_registers(pid, ®s) != 0)
goto ret;
/* Prepare `do_dlopen()' input arguments. On Android >= 7, we set the 4th
* argument to a value that emulates `__builtin_return_address()', so that
* our DSO is loaded in the correct namespace.
*/
ARG0(regs) = SP(regs) + SP_OFF;
ARG1(regs) = RTLD_NOW | RTLD_GLOBAL;
ARG2(regs) = 0;
ARG3(regs) = PC(regs);
/* We set the new PC and also set LR to force the debuggee to crash. */
#if defined(__aarch64__)
LR(regs) = 0xffffffffffffffff;
PC(regs) = (reg_t)dlopen_addr;
#elif defined(__arm__)
LR(regs) = 0xffffffff;
PC(regs) = (reg_t)dlopen_addr | 1;
CPSR(regs) |= 1 << 5;
#endif
printf("[*] Modifying target's state\n");
if(write_registers(pid, ®s) != 0)
goto ret;
size = strlen(filename) + 1;
if(write_memory(pid, (void *)SP(regs) + SP_OFF, filename, size) != size)
goto ret;
printf("[*] Waiting for target to throw SIGSEGV or SIGBUS\n");
if(resume_and_wait(pid) != 0)
goto ret;
r = 0;
ret:
return r;
}
int main( int argc, char *argv[] ) {
/* input */
if( argc != 5 ) {
std::cerr<<"Usage: "<<argv[0]<<" [input map] [lon step] [lat step] [output file]"<<std::endl;
exit(-1);
}
/* open input map */
std::ifstream fin(argv[1]);
if( ! fin ) {
std::cerr<<"Cannot read from file "<<argv[1]<<std::endl;
exit(0);
}
/* read in point data */
std::vector<PointData> Map;
for( std::string line; std::getline(fin, line); ){
PointData pdtmp( line.c_str() );
Map.push_back(pdtmp);
}
fin.close();
if( Map.size() <= 0 ) {
std::cerr<<"Empty map!"<<std::endl;
exit(0);
}
/* search for boundaries */
float lonstep = atof(argv[2]), latstep = atof(argv[3]);
if( lonstep<=0 || latstep<=0 ) {
std::cerr<<"positive number expected for lon/lat steps!"<<std::endl;
exit(0);
}
PointData UL( Map.at(0) ); // upper-left corner
PointData LR( Map.at(0) ); // lower-right corner
for( int i=0; i<Map.size(); i++ ) {
PointData &pdcurrent = Map.at(i);
if( pdcurrent.IsWestOf(UL) ) UL.lon = pdcurrent.lon;
else if( LR.IsWestOf(pdcurrent) ) LR.lon = pdcurrent.lon;
if( pdcurrent.IsNorthOf(UL) ) UL.lat = pdcurrent.lat;
else if( LR.IsNorthOf(pdcurrent) ) LR.lat = pdcurrent.lat;
}
std::cout<<"Upper-left: "<<UL<<std::endl;
std::cout<<"Lower-right: "<<LR<<std::endl;
/* compute data average */
float mean = 0.;
for( int i=0; i<Map.size(); i++ ) mean += Map.at(i).dat;
mean /= Map.size();
std::cout<<"data mean = "<<mean<<std::endl;
/* data matrix */
int npts_lon = (int)floor( (LR.lon - UL.lon)/lonstep + 0.5 ) + 1;
int npts_lat = (int)floor( (UL.lat - LR.lat)/latstep + 0.5 ) + 1;
std::vector<float> DataM;
DataM.resize( npts_lon * npts_lat );
std::fill( DataM.begin(), DataM.end(), mean);
for( int i=0; i<Map.size(); i++ ) {
PointData &pdcurrent = Map.at(i);
int idxlon = (int)floor( (pdcurrent.lon - UL.lon) / lonstep + 0.5 );
int idxlat = (int)floor( (UL.lat - pdcurrent.lat) / latstep + 0.5 );
DataM.at( idxlon * npts_lat + idxlat ) = pdcurrent.dat;
//std::cerr<<idxlat<<" "<<idxlon<<" "<<pdcurrent<<std::endl;
}
/* output in the format required by fm2dss */
std::ofstream fout(argv[4]);
if( ! fout ) {
std::cerr<<"Cannot write to file "<<argv[3]<<std::endl;
exit(0);
}
fout << npts_lat-2 << " " << npts_lon-2 << std::endl; // leave the outer boundary as 'cushion nodes'
fout << UL.lat-latstep << " " << UL.lon+lonstep << std::endl; // computational grid starts from the second point on each direction
fout << latstep << " " << lonstep << std::endl;
for( int i=0; i<DataM.size(); i++ ) fout << DataM.at(i) << std::endl;
fout.close();
return 0;
}
void AVLTree::Insert(TreeNode * &node, TreeNode *parent, int x, QAnimationGroup *group)
{
if (group && parent)
group->addAnimation(parent->getTurnRedAnim());
if (node == NULL)
{
QParallelAnimationGroup *anim = new QParallelAnimationGroup;
node = new TreeNode();
view->addItem(node);
node->data = x;
node->setPos(parent ? parent->pos() : QPointF(0, 0));
anim->addAnimation(node->getFadeInAnim());
if (group)
{
anim->addAnimation(node->setParent(parent));
anim->addAnimation(getPosAnim());
group->addAnimation(anim);
if (parent)
group->addAnimation(parent->getTurnBlackAnim());
}
else
{
anim->start(QAbstractAnimation::DeleteWhenStopped);
}
return;
}
TreePath *path;
if (group && parent)
{
path = new TreePath(parent, node, view);
group->addAnimation(path->getToSonAnim());
}
if (node->data > x)
{
Insert(node->Lson, node, x, group);
if (group && parent)
group->addAnimation(path->getToParentAnim());
if (2 == height(node->Lson) - height(node->Rson))
{
if (x < node->Lson->data)
LL(node, group);
else
LR(node, group);
}
}
else if (node->data < x)
{
Insert(node->Rson, node, x, group);
if (group && parent)
group->addAnimation(path->getToParentAnim());
if (2 == height(node->Rson) - height(node->Lson))
{
if (x > node->Rson->data)
RR(node, group);
else
RL(node, group);
}
}
else if (group && parent)
group->addAnimation(path->getToParentAnim());
if (group && parent)
group->addAnimation(parent->getTurnBlackAnim());
node->h = max(height(node->Lson), height(node->Rson)) + 1;
}
void World::loadScene(string filename) {
// for as4, you can optionally hard-code the scene. For as5 and as6 it must be loaded from a file.
if (_FINAL_PROJ) {
vec4 eye(0.0, 0.0, 0, 1.0);
vec4 LL(-1.0, -1.0, -3.0, 1.0);
vec4 UL(-1.0, 1.0, -3.0, 1.0);
vec4 LR(1.0, -1.0, -3.0, 1.0);
vec4 UR(1.0, 1.0, -3.0, 1.0);
_view = Viewport(eye, LL, UL, LR, UR, IMAGE_WIDTH, IMAGE_HEIGHT, 1);
_lights[LIGHT_DIRECTIONAL].push_back(
Light(0, vec3(0.5,0.5,-0.5),
LightInfo(LIGHT_DIRECTIONAL, vec3(.4, .8, 1.2))));
_lights[LIGHT_POINT].push_back(
Light(vec3(0.0,0.0,-14.0), vec3(0.5,0.5,-0.5),
LightInfo(LIGHT_POINT, vec3(1.39, 0.2, 0.2))));
_ambientLight = vec3(.5,.2,.2);
/*
_spheres.push_back(Sphere(vec4(-2.5,-1.5,-17.0,1.0), 2.0,
MaterialInfo(vec3(.4, .5, .9),
.1, .5, .5, 150, 1.0)));
_spheres.push_back(Sphere(vec4(0.0,4.0,-25.0,1.0), 2.5,
MaterialInfo(vec3(.9, .4, .5),
.4, .2, .5, 20, 0.0)));
_spheres.push_back(Sphere(vec4(1.5,-1.5,-10.0,1.0), 1.0,
MaterialInfo(vec3(.5, .9, .4),
.5, .5, .3, 4, .5)));
*/
_cubes.push_back(Cube(vec4(-3.5,-3.5,-15.0,1.0), 1.5, MaterialInfo(vec3(.4, .5, .9),
.1, .5, .5, 150, 1.0)));
_cubes.push_back(Cube(vec4(2.0, 1.5, -10,1.0), 1.5, MaterialInfo(vec3(.9, .4, .5),
.4, .2, .5, 20, 0.0)));
_cubes.push_back(Cube(vec4(-3.5,4,-120.0,1.0), 3, MaterialInfo(vec3(.5, .9, .4),
.5, .5, .3, 4, .5)));
_cubes.push_back(Cube(vec4(3.5,-4,-250.0,1.0), 3.5, MaterialInfo(vec3(.5, .9, .4),
.5, .5, .3, 4, .5)));
ksmMod = kspMod = 1.0;
}
else if (_ASSIGNMENT >= 5) {
scene = new Scene(filename);
loadInstance(scene->getRoot());
if (_ASSIGNMENT >= 6)
_bb = new BoundingBox(_spheres, 0);
}
if (_ASSIGNMENT <= 4) {
//vec4 eye(0.0, 0.0, 0, 1.0);
//vec4 LL(-1.0, -1.0, -3.0, 1.0);
//vec4 UL(-1.0, 1.0, -3.0, 1.0);
//vec4 LR(1.0, -1.0, -3.0, 1.0);
//vec4 UR(1.0, 1.0, -3.0, 1.0);
//_view = Viewport(eye, LL, UL, LR, UR, IMAGE_WIDTH, IMAGE_HEIGHT);
//_lights[LIGHT_DIRECTIONAL].push_back(
// Light(0, vec3(0.5,0.5,-0.5),
// LightInfo(LIGHT_DIRECTIONAL, vec3(.4, .8, 1.2))));
//_lights[LIGHT_POINT].push_back(
// Light(vec3(0.0,0.0,-14.0), vec3(0.5,0.5,-0.5),
// LightInfo(LIGHT_POINT, vec3(1.39, 0.2, 0.2))));
//_ambientLight = vec3(.5,.2,.2);
//_spheres.push_back(Sphere(vec4(-2.5,-1.5,-17.0,1.0), 2.0,
// MaterialInfo(vec3(.4, .5, .9),
// .1, .5, .5, 150, 1.0)));
//_spheres.push_back(Sphere(vec4(0.0,4.0,-25.0,1.0), 2.5,
// MaterialInfo(vec3(.9, .4, .5),
// .4, .2, .5, 20, 0.0)));
//_spheres.push_back(Sphere(vec4(1.5,-1.5,-10.0,1.0), 1.0,
// MaterialInfo(vec3(.5, .9, .4),
// .5, .5, .3, 4, .5)));
//ksmMod = kspMod = 1.0;
}
}
void World::loadInstance(SceneInstance *si) {
// Compute transform information. Should add a mat4 even if there is no transform.
mat4 mTransform;
if (!si->computeTransform(mTransform)) {
mTransform = identity3D(); // if there is no transform, push an identity matrix.
}
if (!transformStack.empty() && transformStack.top() != identity3D()) {
mat4 mPreviousTransform = transformStack.top();
mTransform = mPreviousTransform * mTransform;
}
transformStack.push(mTransform);
SceneGroup *sg = si->getChild();
// Get camera info, if any.
CameraInfo camInfo;
if (sg->computeCamera(camInfo)) {
vec4 eye(0.0, 0.0, 0.0, 1.0);
double left = camInfo.sides[FRUS_LEFT];
double right = camInfo.sides[FRUS_RIGHT];
double top = camInfo.sides[FRUS_TOP];
double bottom = camInfo.sides[FRUS_BOTTOM];
double depth = -1*camInfo.sides[FRUS_NEAR];
vec4 LL(left, bottom, depth, 1.0);
vec4 UL(left, top, depth, 1.0);
vec4 LR(right, bottom, depth, 1.0);
vec4 UR(right, top, depth, 1.0);
eye = mTransform * eye;
LL = mTransform * LL;
UL = mTransform * UL;
LR = mTransform * LR;
UR = mTransform * UR;
_view = Viewport(eye, LL, UL, LR, UR, IMAGE_WIDTH, IMAGE_HEIGHT, camInfo.perspective);
}
// Compute light info, if any.
LightInfo li;
if (sg->computeLight(li)) {
vec3 direction, position;
switch (li.type) {
case LIGHT_DIRECTIONAL:
case LIGHT_POINT:
case LIGHT_SPOT:
direction = vec3(-1*mTransform[0][2],-1*mTransform[1][2],-1*mTransform[2][2]);
position = mTransform * vec3(0.0);
_lights[li.type].push_back(Light(position, direction, li));
break;
case LIGHT_AMBIENT:
_ambientLight = li.color;
break;
}
}
// Computes sphere info, if any.
double radius;
MaterialInfo matInfo;
if (sg->computeSphere(radius, matInfo, 0)) {
_spheres.push_back(Sphere(vec4(0.0), radius, matInfo, mTransform));
}
// Recursively parse scene.
for (int i = 0; i < sg->getChildCount(); i++) {
loadInstance(sg->getChild(i));
}
// Pops the transformation matrix of this SceneGroup. This function should have added this matrix at the beginning.
transformStack.pop();
}
void AVLTree::Delete(TreeNode * &node, int x, QAnimationGroup *group)
{
if (node == NULL)
return;
if (group&& node->parent)
group->addAnimation(node->parent->getTurnRedAnim());
TreePath *path;
if (group && node->parent)
{
path = new TreePath(node->parent, node, view);
group->addAnimation(path->getToSonAnim());
}
TreeNode* parent = node->parent;
if (x < node->data)
{
//如果x小于节点的值,就继续在节点的左子树中删除x
Delete(node->Lson, x, group);
if (group && node->parent)
group->addAnimation(path->getToParentAnim());
if (2 == height(node->Rson) - height(node->Lson))
{
if (node->Rson->Lson != NULL &&
(height(node->Rson->Lson)>height(node->Rson->Rson)))
RL(node, group);
else
RR(node, group);
}
}
else if (x > node->data)
{
Delete(node->Rson, x, group);//如果x大于节点的值,就继续在节点的右子树中删除x
if (group && node->parent)
group->addAnimation(path->getToParentAnim());
if (2 == height(node->Lson) - height(node->Rson))
{
if (node->Lson->Rson != NULL &&
(height(node->Lson->Rson)>height(node->Lson->Lson)))
LR(node, group);
else
LL(node, group);
}
}
else//如果相等,此节点就是要删除的节点
{
if (group)
group->addAnimation(node->getPopAnim());
else
node->getPopAnim()->start(QAbstractAnimation::DeleteWhenStopped);
if (node->Lson && node->Rson)//此节点有两个儿子
{
TreeNode* temp = node->Rson;//temp指向节点的右儿子
while (temp->Lson != NULL) temp = temp->Lson;//找到右子树中值最小的节点
//把右子树中最小节点的值赋值给本节点
if (group)
group->addAnimation(node->getValueAnim(temp->data));
else
node->data = temp->data;
//node->freq = temp->freq;
Delete(node->Rson, temp->data, group);//删除右子树中最小值的节点
if (group && node->parent)
group->addAnimation(path->getToParentAnim());
if (2 == height(node->Lson) - height(node->Rson))
{
if (node->Lson->Rson != NULL &&
(height(node->Lson->Rson)>height(node->Lson->Lson)))
LR(node, group);
else
LL(node, group);
}
}
else//此节点有1个或0个儿子
{
TreeNode* temp = node;
TreeNode* parent = node->parent;
if (group && node->parent)
group->addAnimation(path->getToParentAnim());
if (node->Lson == NULL)//有右儿子或者没有儿子
node = node->Rson;
else if (node->Rson == NULL)//有左儿子
node = node->Lson;
if (group)
{
QParallelAnimationGroup *anim = new QParallelAnimationGroup;
anim->addAnimation(temp->getFadeOutAnim());
if (node)
anim->addAnimation(node->setParent(parent));
group->addAnimation(anim);
}
else
{
temp->getFadeOutAnim()->start(QAbstractAnimation::DeleteWhenStopped);
if (node)
node->setParent(parent)->start(QAbstractAnimation::DeleteWhenStopped);
}
if (group)
group->addAnimation(getPosAnim());
}
}
if (group && parent)
group->addAnimation(parent->getTurnBlackAnim());
if (node == NULL)
return;
node->h = max(height(node->Lson), height(node->Rson)) + 1;
return;
}
