prim: fix crash in PrimAddNode

When adding different ID's in a non-sequential order, segfault
occurred. Fix in Prim's logic
fibheap: fix possible crash due to variable ID's

As it was mentioned in previous commit in Prim, providing variable ID's
caused crash in application. However without any support in FibHeap for
variable ID, possible crash might occur. With this addition, no crash is
possible when using variable ID's